// © RiceMotion ( Robert Carl Rice ) 2012-2016 - All rights reserved


// This software makes use of tools and libraries obtained from open source projects or released for

// use by relevant hardware manufactures. However, this software is NOT a part of any open source project.

// It is released only on a "need to know" basis for beta testers of the "RiceCNC Interpolation Engine".

// Recipents of this source code must respect the confidential nature of this software and prevent any

// distribution that could result in counterfeit copies of the "RiceCNC Interpolation Engine".


// © RiceMotion ( Robert Carl Rice ) 2012-2016 - All rights reserved

#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; }; };