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

#include <Gpio.h>

#include <MainCommands.h>

#include <InputPin.h>

#include <StepCounter.h>

#include <ClosedLoopCounter.h>



//     All methods defined here are call with axis interrupts disabled


void     // axis interrupts disabled

     Axis::updateVelocity() { // Called from derived axis at axis interrupt 15 kHz


     /* Check for stop condition.

     This code stops motor faster to prevent overruns and oscillations.

     The PROGRAM_FAULT state persists only if not set to IDLE by interpolation

     */

     if ( axisState == SEEKING_POSITION ) {

          if (     (     targetStepsPerSecondF > 0.0

                    &&     microStepTargetOffsetI <= 0 )

               ||     (     targetStepsPerSecondF < 0.0

                    &&     microStepTargetOffsetI >= 0 ) ) {


               // Overrun target position

               setAxisState( PROGRAM_FAULT );

               targetStepsPerSecondF     = 0.0; }; }


     // TODO: Make sure motor remains stopped

     else if ( axisState == PROGRAM_FAULT )

          targetStepsPerSecondF     = 0.0;


     // Update motor velocity

     switch ( motorState ) {


          case DISABLED:

          return;


          case ACCELERATING:


          if          ( mvStepsPerSecF < targetStepsPerSecondF ) {

               flags  |= VELOCITY_CHANGED;

               mvStepsPerSecF     += accelSpeedStepSizeF;

               if ( mvStepsPerSecF >= targetStepsPerSecondF ) {

                    mvStepsPerSecF     = targetStepsPerSecondF;

                    setMotorState(); }; }


          else if ( mvStepsPerSecF > targetStepsPerSecondF ) {

               flags  |= VELOCITY_CHANGED;

               mvStepsPerSecF     -= accelSpeedStepSizeF;

               if ( mvStepsPerSecF <= targetStepsPerSecondF ) {

                    mvStepsPerSecF     = targetStepsPerSecondF;

                    setMotorState(); }; }


          else

               setMotorState();


          break;


          case DECELERATING: // Target velocity is zero


          if          ( mvStepsPerSecF < targetStepsPerSecondF ) {

               flags  |= VELOCITY_CHANGED;

               mvStepsPerSecF     += decelSpeedStepSizeF;

               if ( mvStepsPerSecF > targetStepsPerSecondF ) {

                    mvStepsPerSecF = targetStepsPerSecondF;

                    setMotorState(); }; }


          else if ( mvStepsPerSecF > targetStepsPerSecondF ) {

               flags  |= VELOCITY_CHANGED;

               mvStepsPerSecF     -= decelSpeedStepSizeF;

               if ( mvStepsPerSecF < targetStepsPerSecondF ) {

                    mvStepsPerSecF = targetStepsPerSecondF;

                    setMotorState(); }; }


          else

               setMotorState(); };


     // update predicted armature phase error (angle between coils and armature)

     indexerOffsetF     += mvStepsPerSecF * indexerStepSize; };


void     // axis interrupts disabled

     Axis::stepped( float microStepsF ) {


     indexerOffsetF          -= microStepsF;


     moveMachinePositionAndTarget( microStepsF ); };


void     // axis interrupts disabled

     Axis::moveMachinePositionAndTarget( float microStepsF ) {

     

     // Record closed loop steps for adjustment and stall detection

     if ( closedLoopStepCounter )

          closedLoopStepCounter->stepped += microStepsF;


     moveMachinePosition( microStepsF );


     if ( axisState == SEEKING_POSITION )

          addToMicroStepTarget( -microStepsF ); };     


void     // axis interrupts disabled

     Axis::addToMicroStepTarget( float microStepsOffsetF ) {


     microStepTargetOffsetF     += microStepsOffsetF;

     

     // carry the integer part to not overflow 23 bit float precision

     int32_t microsteps          = roundToInt( microStepTargetOffsetF );

     if ( microsteps ) {          // maintain within a half microstep

          microStepTargetOffsetI     += microsteps;

          microStepTargetOffsetF     -= microsteps; }; };


void     // axis interrupts disabled

     Axis::updateTimerInterval() {


     indexerStepSize          =     // microsteps per second

          AxisTimer::axisInterruptPerioduSF     // µSec

     *     microStepsPerStepF

     *     0.000001;


     accelSpeedStepSizeF     =

          AxisTimer::axisInterruptPerioduSF

     *     accelStepsPerSecPerMicroSecF;


     decelSpeedStepSizeF     =

          AxisTimer::axisInterruptPerioduSF

     *     decelStepsPerSecPerMicroSecF; };


void     // axis interrupts disabled

     Axis::runMotor( float stepsPerSecondF ) {


     targetStepsPerSecondF     = stepsPerSecondF;

     setMotorState(); };


void     // axis interrupts disabled

     Axis::stopMotor() {


     targetStepsPerSecondF     = 0.0;

     setMotorState(); };



void     // Axis interrupt disabled

     Axis::setAxisState( AxisState newState ) {


     if ( axisState != newState ) {

          axisState     = newState;

          flags          |= STATUS_CHANGED; }; };



void     // Axis interrupt disabled

     Axis::setMotorState() {


     MotorState newState;


     if          ( targetStepsPerSecondF == mvStepsPerSecF )

          newState     = mvStepsPerSecF ?

               CONSTANT_SPEED :

               HOLDING;


     else if ( mvStepsPerSecF > 0.0 )

          newState     = targetStepsPerSecondF > mvStepsPerSecF ?

               ACCELERATING :

               DECELERATING;


     else if ( mvStepsPerSecF < 0.0 )

          newState     = targetStepsPerSecondF < mvStepsPerSecF ?

               ACCELERATING :

               DECELERATING;


     else     // mvStepsPerSecF == 0.0

          newState     = ACCELERATING;


     if ( motorState != newState ) {

          motorState     = newState;

          flags          |= STATUS_CHANGED;

          motorStateChanged(); }; };