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

#include <InputPin.h>


#include <StepDirectionCounter.h>

#include <QuadratureCounter4X.h>

#include <HallCounter.h>

#include <MachineCommands.h>



void

     Axis::addSlaveInput(

          char*          data,

          String*          msgPtr ) {

     

     if ( slaveStepCounter )

          delete slaveStepCounter;

     slaveStepCounter     = NULL;


     *msgPtr          += " slave ";

     switch ( *data ) {


          case COUNTER_TYPE_STEP_DIRECTION :

          addStepDirectionCounter(

               data + 1,

               msgPtr );

          break;


          case COUNTER_TYPE_QUADRATURE :

          addQuadratureCounter(

               data + 1,

               msgPtr );

          break;


          case COUNTER_TYPE_HALL :

          addHallCounter(

               data + 1,

               msgPtr );

          break;


          default :

          *msgPtr          += "invalid"; }; };



void

     Axis::addStepDirectionCounter(

          char*          data,

          String*          msgPtr ) {


     char     directionInputPinString[8]; // Q1

     char     stepInputPinString[8];          // Q2

     float     scale          = 1.0;


     sscanf( data,

          " %s %s %f",

          directionInputPinString,

          stepInputPinString,

          &scale );


     InputPin* directionInputPin     = new InputPin(

          directionInputPinString,

          msgPtr );


     InputPin* stepInputPin          = new InputPin(

          stepInputPinString,

          msgPtr );


     if (     directionInputPin->valid()

          &&     stepInputPin->valid() ) {


          *msgPtr          += "direction";

          directionInputPin->fmtPin( msgPtr );


          *msgPtr          += "  step";

          stepInputPin->fmtPin( msgPtr );


          *msgPtr          += "  scale ";

          *msgPtr          += String( scale );


          slaveStepCounter     = new StepDirectionCounter(

               directionInputPin,

               stepInputPin,

               scale ); }


     else {

          delete directionInputPin;

          delete stepInputPin; }; };



void

     Axis::addQuadratureCounter(

          char*          data,

          String*          msgPtr ) {


     char     q1PinString[8];

     char     q2PinString[8];

     float     scale          = 1.0;


     sscanf( data,

          " %s %s %f",

          q1PinString,

          q2PinString,

          &scale );


     InputPin* q1InputPin     = new InputPin(

          q1PinString,

          msgPtr );


     InputPin* q2InputPin     = new InputPin(

          q2PinString,

          msgPtr );


     if (     q1InputPin->valid()

          &&     q2InputPin->valid() ) {


          *msgPtr          += "Q1";

          q1InputPin->fmtPin( msgPtr );


          *msgPtr          += "  Q2";

          q2InputPin->fmtPin( msgPtr );


          *msgPtr          += "  scale ";

          *msgPtr          += String( scale );


          slaveStepCounter     = new QuadratureCounter4X(

               q1InputPin,

               q2InputPin,

               scale ); }

     

     else {

          delete q1InputPin;

          delete q2InputPin; }; };



void

     Axis::addHallCounter(

          char*          data,

          String*          msgPtr ) {


     char     hallAPinString[8];

     char     hallBPinString[8];

     char     hallCPinString[8];

     float     scale          = 1.0;


     sscanf( data,

          " %s %s %s %f",

          hallAPinString,

          hallBPinString,

          hallCPinString,

          &scale );


     InputPin* hallAInputPin     = new InputPin(

          hallAPinString,

          msgPtr );


     InputPin* hallBInputPin     = new InputPin(

          hallBPinString,

          msgPtr );


     InputPin* hallCInputPin     = new InputPin(

          hallCPinString,

          msgPtr );


     if (     hallAInputPin->valid()

          &&     hallBInputPin->valid()

          &&     hallCInputPin->valid() ) {


          *msgPtr          += "HallA";

          hallAInputPin->fmtPin( msgPtr );


          *msgPtr          += "  HallB";

          hallBInputPin->fmtPin( msgPtr );


          *msgPtr          += "  HallC";

          hallCInputPin->fmtPin( msgPtr );


          *msgPtr          += "  scale ";

          *msgPtr          += String( scale );


          slaveStepCounter     = new HallCounter(

               hallAInputPin,

               hallBInputPin,

               hallCInputPin,

               scale ); }


     else {

          delete hallAInputPin;

          delete hallBInputPin;

          delete hallCInputPin; }; };



void

     Axis::addSlaveInput() {


     if ( slaveStepCounter )

          interpolationMicrostepBuffer

               += slaveStepCounter->readScaledCount();     };