// © 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 <StepDirectionAxis.h>

#include <OutputPin.h>



StepDirectionAxis::StepDirectionAxis(

     char*          data,

     String*          msgPtr )


     :     Axis(

          data,

          msgPtr ) {


     char        directionPinString[8];

     char        stepPinString[8];

     

     // Could be inherited by DRV8711Axis but using SPI control instead

     uint32_t     myParseLength;


     sscanf( data + parseLength + 1,

          "%s %s%ln",

          directionPinString,

          stepPinString,

          &myParseLength );


     parseLength          += myParseLength + 1;

     

     directionPin     = new OutputPin( directionPinString, msgPtr );

     stepPin               = new OutputPin( stepPinString, msgPtr );


     if (     directionPin->valid()

          &&     stepPin->valid() ) {


          directionPin->setPin( false);

          stepPin->setPin( false);


          *msgPtr          += "\n dir";

          directionPin->fmtPin( msgPtr );


          *msgPtr          += "\n step";

          stepPin->fmtPin( msgPtr );

    

          phaseAngle          = Quiescent; }


     else {

          delete directionPin;

          delete stepPin;

          directionPin     = NULL;

          stepPin               = NULL; };

  

     if ( isConfigured() )

          *msgPtr          += " StepDir Configured"; };



StepDirectionAxis::~StepDirectionAxis() {


     if ( directionPin )

          delete directionPin;


     if ( stepPin )

          delete stepPin; };


bool

     StepDirectionAxis::isConfigured() {


     return directionPin && stepPin; };


void

     StepDirectionAxis::motorStepIsr() { // called on axis interrupt


     if ( ! directionPin )

          return;

  

     updateVelocity();


     switch( phaseAngle ) {


          case  StateStepPositive :

          stepPin->setPin( true ); // toggle the step pin

          stepped( 1.0 );

          phaseAngle          = NeedsPulseReset;

          break;          // Can't do anything else until step is reset


          case  StateStepNegative :

          stepPin->setPin( true ); // toggle the step pin

          stepped( -1.0 );

          phaseAngle          = NeedsPulseReset;

          break;     // Can't do anything else until step is reset


          case  NeedsPulseReset :

          stepPin->setPin( false ); // reset the step pin

          // idle for now but continue to calculate direction for next step

          phaseAngle          = Quiescent;

          // fall through to Quiescent


          case Quiescent :

          if          ( indexerOffsetF >= 1.0 ) {

               directionPin->setPin( true );

               phaseAngle       = StateStepPositive; } // Step on next interrupt


          else if ( indexerOffsetF <= -1.0 ) {

               directionPin->setPin( false );

               phaseAngle       = StateStepNegative; }; };     }; // Step on next interrupt