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



void

     Axis::deleteLinks() {


     deleteSuperiorLinks();

     while ( firstSubordinateLink )

          delete firstSubordinateLink; };

          

void

     Axis::deleteSuperiorLinks() {


     while ( firstSuperiorLink )

          delete firstSuperiorLink; };



// virtual methods

void

     Axis::adjustProgramPosition( float positionF ) {


     // adjust program position without causing motor movement

     AxisTimer::disableAxisInterrupts();

          moveProgramPosition(

               positionF

          *     microStepsPerUnitDistanceF );

     AxisTimer::enableAxisInterrupts();     };


void

     Axis::moveProgramPosition( float scaledOffset ) {


     uStepProgramPositionF     += scaledOffset;

     // extend to double precision

     int32_t microSteps          = roundToInt( uStepProgramPositionF );

     uStepProgramPositionI     += microSteps;

     uStepProgramPositionF     -= microSteps; };


void

     Axis::calculateTransformedTargets( float microstepDeltaF ) {


     if ( microstepDeltaF == 0.0 )

          return;


     moveProgramPosition( microstepDeltaF );

     

     /*

     Program space is coupled to machine space by the transformation matrix


     A default identity transform axis will not be dependent on other axes,

     i.e., no superiors. For the default we adjust the machine position

     according to the delta. However, if the axis has superiors, then this

     axis machine position changes only if the axis machine position has a

     scaled dependency upon its program position.

     */

     if ( ! firstSuperiorLink )

          interpolationMicrostepBuffer     += microstepDeltaF;


     // adjustTargetPositions for subordinate axes but not programPositions.

     // For scaling, this axis will be a subordinate of itself

     Link* link          = firstSubordinateLink;

     while ( link ) {

          link->calculateTransformedTargets( microstepDeltaF );

          link        = link->nextSubordinateLink; }; };