#include <ThreePhaseAxis.h>
#include <OutputPin.h>
ThreePhaseAxis::ThreePhaseAxis(
char* data,
String* msgPtr )
: Axis(
data,
msgPtr ) {
char phaseApinString[8];
char phaseBpinString[8];
char phaseCpinString[8];
char halfStepPinString[8];
sscanf( data + parseLength + 1,
"%s %s %s %s",
phaseApinString,
phaseBpinString,
phaseCpinString,
halfStepPinString );
phaseApin = new OutputPin(
phaseApinString,
msgPtr );
phaseBpin = new OutputPin(
phaseBpinString,
msgPtr );
phaseCpin = new OutputPin(
phaseCpinString,
msgPtr );
halfStepPin = new OutputPin(
halfStepPinString,
msgPtr );
halfStep = false;
if ( ! halfStepPin->valid() ) {
delete halfStepPin;
halfStepPin = NULL; };
// validate
if ( phaseApin->valid()
&& phaseBpin->valid()
&& phaseCpin->valid() ) {
phaseAngle = PSA; // set initial phase
phaseApin->setPin( true);
phaseBpin->setPin( false);
phaseCpin->setPin( false);
*msgPtr += " PA";
phaseApin->fmtPin( msgPtr );
*msgPtr += " PB";
phaseBpin->fmtPin( msgPtr );
*msgPtr += " PC";
phaseCpin->fmtPin( msgPtr );
if ( halfStepPin ) {
*msgPtr += " Half Step";
halfStepPin->fmtPin( msgPtr ); }; }
else {
delete phaseApin;
phaseApin = NULL;
delete phaseBpin;
phaseBpin = NULL;
delete phaseCpin;
phaseCpin = NULL; }; };
ThreePhaseAxis::~ThreePhaseAxis() {
if ( phaseApin )
delete phaseApin;
if ( phaseBpin )
delete phaseBpin;
if ( phaseCpin )
delete phaseCpin;
if ( halfStepPin )
delete halfStepPin; };
bool
ThreePhaseAxis::isConfigured() {
return
phaseApin
&& phaseBpin
&& phaseCpin; };
void
ThreePhaseAxis::motorStepIsr() { // return true if position was processed
updateVelocity();
// Discrete half-step mode
// grey code is faster with no delay for direction change or pulse reset
if ( indexerOffsetF >= 1.0 ) {
isrStepMotor( true ); // Step immediately
stepped( 1.0 );
return; };
if ( indexerOffsetF <= -1.0 ) {
isrStepMotor( false ); // Step immediately
stepped( -1.0 ); }; };
void
ThreePhaseAxis::isrStepMotor( bool direction ) {
if ( halfStep ) {
halfStepPin->setPin( false );
halfStep = false;
return; };
if ( halfStepPin ) {
halfStepPin->setPin( true );
halfStep = true; };
switch( phaseAngle ) {
case PSA :
if ( direction ) {
phaseBpin->setPin( true );
phaseAngle = PSAB; }
else {
phaseCpin->setPin( true );
phaseAngle = PSCA; };
break;
case PSAB :
if ( direction ) {
phaseApin->setPin( false );
phaseAngle = PSB; }
else {
phaseBpin->setPin( false );
phaseAngle = PSA; };
break;
case PSB :
if ( direction ) {
phaseCpin->setPin( true );
phaseAngle = PSBC; }
else {
phaseApin->setPin( true );
phaseAngle = PSAB; };
break;
case PSBC :
if ( direction ) {
phaseBpin->setPin( false );
phaseAngle = PSC; }
else {
phaseCpin->setPin( false );
phaseAngle = PSB; };
break;
case PSC :
if ( direction ) {
phaseApin->setPin( true );
phaseAngle = PSCA; }
else {
phaseBpin->setPin( true );
phaseAngle = PSBC; };
break;
case PSCA :
if ( direction ) {
phaseCpin->setPin( false );
phaseAngle = PSA; }
else {
phaseApin->setPin( false );
phaseAngle = PSC; };
break; }; };