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

#include <Gpio.h>

#include <InterpolationTimer.h>

#include <Machine.h>

#include <InputPin.h>



// All of these methods are called with the interpolation interrupt disabled


bool

     Axis::isSeekingPosition() {


     return

          axisState     == SEEKING_POSITION ; };


void

     Axis::abort() { // Stop if seeking velocity

     // stop as fast as possible

     microStepTargetOffsetI     =

          roundToInt( microStepStopDisplacement() );

     stopMotor(); };



void

     Axis::interpolationIsrEnd() {     // called once per interpolation interrupt


     // Has interpolation moved the targert position for this axis?

     if          ( interpolationMicrostepBuffer )

          interpolating();


     else if (     axisState == SEEKING_POSITION

               ||     axisState == PROGRAM_FAULT ) {


          stepProgressF     = 2.0;     // Don't wait on this axis

     

          if ( motorState == HOLDING ) {

               setAxisState( IDLE );

               return; }


          else     // Stopping

               calculateStopVelocity(); }     // update targetStepsPerSecondF


     else     // idle or velocity mode

          return;


     AxisTimer::disableAxisInterrupts();

          setMotorState();

     AxisTimer::enableAxisInterrupts(); };


void

     Axis::interpolating() {


     adjustMicroStepTarget( interpolationMicrostepBuffer );


     calculateStopVelocity();     // update targetStepsPerSecondF


     if ( targetStepsPerSecondF ) {

     

          if          ( targetStepsPerSecondF > maxStepsPerSecF ) {

               targetStepsPerSecondF     = maxStepsPerSecF;

               stepProgressF     = 0.1; }     // Too fast - slow interpolation


          else if     ( targetStepsPerSecondF < -maxStepsPerSecF ) {

               targetStepsPerSecondF     = -maxStepsPerSecF;

               stepProgressF     = 0.1; }


          else {

               float microStepsPerInterruptPeriodSecF =

                         InterpolationTimer::interpolationInterruptPeriodSecF

                    *     microStepsPerStepF;


               // Calculate the axis speed intended by the interpolation

               float interpolationStepsPerSecF =

                         interpolationMicrostepBuffer

                    /     microStepsPerInterruptPeriodSecF;


               // Calculate progress by ratio of speed rather than distance

               stepProgressF     =

                         targetStepsPerSecondF

                    /     interpolationStepsPerSecF;


               // Feed forward a small amount of the intended speed to

               // prevent motor surging

               targetStepsPerSecondF     =

                         targetStepsPerSecondF          * 0.99     // mostly use stop velocity

                    +     interpolationStepsPerSecF     * 0.01; }; }


          else

               stepProgressF     = 0.0;     // used to adjust interpolation step size


     interpolationMicrostepBuffer     = 0.0; };


void

     Axis::minAndMaxProgress( Progress* progress ) {


     // stepProgressF is zero if movement was less than a microstep

     if ( isSeekingPosition() && stepProgressF ) {


          if ( stepProgressF < progress->minimum )

               progress->minimum     = stepProgressF;


          if ( stepProgressF > progress->maximum )

               progress->maximum     = stepProgressF; }; };


void

     Axis::calculateStopVelocity() {

     // I tried lots of algorithms to avoid using sqrt() but all were unstable so I gave up

     // but then I learned that the FPU implements a fast sqrt so no speed problem


     if ( microStepTargetOffsetI ) {

          // Calculate a target velocity to easily stop before the target offset

          // Conservative by 50%

          targetStepsPerSecondF     = sqrt(

                    float( labs( microStepTargetOffsetI ) )

               /     microStepsPerStepF

               *     decelStepsPerSecPerSecF );


          if     ( microStepTargetOffsetI < 0 )

               targetStepsPerSecondF     = -targetStepsPerSecondF; }


     else

          targetStepsPerSecondF     = 0.0;     // optimization

     };



float     // used for feedrate calculation

     Axis::scaledTargetSpeedSquared() {     // target velocity to IPS squared

     

     if ( targetStepsPerSecondF == 0.0 )

          return

               0.0;


     float unitDistancePerSec     =

               targetStepsPerSecondF

          /     stepsPerUnitDistanceF;


     if ( angular() && rotationalRadiusF )

          unitDistancePerSec          *= rotationalRadiusF; // convert from degrees to inchs


     return

               unitDistancePerSec

          *     unitDistancePerSec; };



// Used only for aborting motor movement

float

     Axis::microStepStopDisplacement() {     // velocity to signed µStep stopping distance


     if ( mvStepsPerSecF == 0.0 )

          return

               0.0;


     float avgVel          =

               mvStepsPerSecF

          *     microStepsPerStepF

          /     2.0; // µsps


     float stopTime     =

               fabs( mvStepsPerSecF )

          *     decelTime_SecPerStepsPerSecF; // seconds


     return

               avgVel

          *     stopTime; };



void

     Axis::adjustMicroStepTarget( float microstepTargetOffsetF ) {


     AxisTimer::disableAxisInterrupts();

          if ( axisState == SEEKING_VELOCITY ) { // Abort velocity mode at stop position

               microStepTargetOffsetI     =

                    roundToInt( microStepStopDisplacement() ); };


          addToMicroStepTarget( microstepTargetOffsetF );

          setAxisState( SEEKING_POSITION );

     AxisTimer::enableAxisInterrupts(); };



void     // called on fixture or tool change

     Axis::adjustMachinePosition( float offset ) {


     // adjust machine position without running motor

     float scaledOffset          = offset * microStepsPerUnitDistanceF;


     AxisTimer::disableAxisInterrupts();

          moveMachinePosition( scaledOffset );

     AxisTimer::enableAxisInterrupts(); };


void

     Axis::moveMachinePosition( float scaledOffset ) {


     uStepMachinePositionF     += scaledOffset;

     // extend to double precision

     int32_t microSteps          = roundToInt( uStepMachinePositionF );

     uStepMachinePositionI     += microSteps;

     uStepMachinePositionF     -= microSteps; };