#include <QuadratureAxis.h>
#include <OutputPin.h>
QuadratureAxis::QuadratureAxis(
char* data,
String* msgPtr )
: Axis(
data,
msgPtr ) {
char phase1pinString[8];
char phase2pinString[8];
sscanf( data + parseLength + 1,
"%s %s",
phase1pinString,
phase2pinString );
phase1pin = new OutputPin( phase1pinString, msgPtr );
phase2pin = new OutputPin( phase2pinString, msgPtr );
// validate
if ( phase1pin->valid()
&& phase2pin->valid() ) {
phaseAngle = FS0; // set initial phase
phase1pin->setPin( false);
phase2pin->setPin( false);
*msgPtr += " P1";
phase1pin->fmtPin( msgPtr );
*msgPtr += " P2";
phase2pin->fmtPin( msgPtr ); }
else {
delete phase1pin;
phase1pin = NULL;
delete phase2pin;
phase2pin = NULL; }; };
QuadratureAxis::~QuadratureAxis() {
if ( phase1pin )
delete phase1pin;
if ( phase2pin )
delete phase2pin; };
bool
QuadratureAxis::isConfigured() {
return phase1pin && phase2pin; };
void
QuadratureAxis::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
QuadratureAxis::isrStepMotor( bool direction ) {
switch( phaseAngle ) {
case FS0 : // neither pin active
if ( direction ) {
phase1pin->setPin( true );
phaseAngle = FS1; }
else {
phase2pin->setPin( true );
phaseAngle = FS3; };
break;
case FS1 : // phase1pin active
if ( direction ) {
phase2pin->setPin( true );
phaseAngle = FS2; }
else {
phase1pin->setPin( false );
phaseAngle = FS0; };
break;
case FS2 : // phase1pin and phase2pin active
if ( direction ) {
phase1pin->setPin( false );
phaseAngle = FS3; }
else {
phase2pin->setPin( false );
phaseAngle = FS1; };
break;
case FS3 : // phase2pin active
if ( direction ) {
phase2pin->setPin( false );
phaseAngle = FS0; }
else {
phase1pin->setPin( true );
phaseAngle = FS2; };
break; }; };