#include <CurrentFeedback.h>
#include <AnalogPin.h>
#include <BLDC3.h>
#include <inc/hw_gpio.h>
#include <driverlib/interrupt.h>
#include <driverlib/timer.h>
//#define DebugDelete 1
CurrentFeedback::CurrentFeedback(
char* data,
String* msgPtr ) {
csa = NULL;
csb = NULL;
csc = NULL;
powerLevelsU.powerLevels = 0;
maxCurrentScale = 0;
char csaPinString [8];
char csbPinString [8];
char cscPinString [8];
sscanf( data,
" %s %s %s %x %lu",
csaPinString,
csbPinString,
cscPinString,
&powerLevelsU.powerLevels,
&maxCurrentScale );
*msgPtr += "\n csA";
csa = new AnalogPin(
csaPinString,
msgPtr );
if ( ! csa->valid() )
return;
*msgPtr += "\n csB";
csb = new AnalogPin(
csbPinString,
msgPtr );
if ( ! csb->valid() )
return;
*msgPtr += "\n csC";
csc = new AnalogPin(
cscPinString,
msgPtr );
if ( ! csc->valid() ) {
delete csc;
csc = NULL;
return; };
*msgPtr += "\n Current Limits\n Scaling ";
*msgPtr += String( maxCurrentScale );
*msgPtr += "\n Holding ";
*msgPtr += String( powerLevelsU.st.holdCurrent );
*msgPtr += "\n Running ";
*msgPtr += String( powerLevelsU.st.constantCurrent );
*msgPtr += "\n Accelerating ";
*msgPtr += String( powerLevelsU.st.acceleratingCurrent );
*msgPtr += "\n Decelerating ";
*msgPtr += String( powerLevelsU.st.deceleratingCurrent ); };
bool
CurrentFeedback::valid() {
return
csa
&& csb
&& csc; };
CurrentFeedback::~CurrentFeedback() {
#ifdef DebugDelete
Serial.println( " delete CurrentFeedback" );
#endif
if ( csa )
delete csa;
if ( csb )
delete csb;
if ( csc )
delete csc; };
bool
CurrentFeedback::currentLimitExceeded( BLDC3* motor ) {
return
motorCurrent( motor ) > maxCurrentCompare; };
uint32_t
CurrentFeedback::motorCurrent( BLDC3* motor ) {
// Only need to check current on low phase
if ( csa->enabled )
return
csa->analogValue();
if ( csb->enabled )
return
csb->analogValue();
return
csa->analogValue(); };
void
CurrentFeedback::motorStateChanged(
Axis::MotorState motorState ) {
switch ( motorState ) {
case Axis::HOLDING :
maxCurrentCompare =
maxCurrentScale
* powerLevelsU.st.holdCurrent;
break;
case Axis::CONSTANT_SPEED :
maxCurrentCompare =
maxCurrentScale
* powerLevelsU.st.constantCurrent;
break;
case Axis::ACCELERATING :
maxCurrentCompare =
maxCurrentScale
* powerLevelsU.st.acceleratingCurrent;
break;
case Axis::DECELERATING :
maxCurrentCompare =
maxCurrentScale
* powerLevelsU.st.deceleratingCurrent;
break;
default:
maxCurrentCompare = 0; }; };