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

#include <Axis.h>


// Initializer

SplineAxis::SplineAxis(

     char*          data,

     Axis*          firstAxisPtr,

     String*          msgPtr ) {


     nextSplineAxis  = NULL;

     type            = Invalid; // Assume invalid

       currentPoint     = 0.0;


     char            axisNameC;

     p2 = p3 = p4    = 0.0;

     int parsed      = sscanf( data,

          "%c%f,%f,%f",

          &axisNameC,

          &p2,

          &p3,

          &p4 );

  

     // Find the axis

     if ( axisNameC >= 97 )

          axisNameC     -= 32; // x->X, y->Y, z->Z, a->A, b->B, c->C


     axis          = firstAxisPtr;

     while (          axis

               &&     axis->axisNameC != axisNameC )

          axis = axis->nextAxis;


     if          ( axis == NULL )

          *msgPtr     += "\nE Spline axis not Configured!";


     else if ( parsed == 4 )

          type          = Bezier;


     else if ( parsed == 2 ) {

          type          = Linear;

          p4               = p2;

          p2               = 0.0; }


     else {

          snprintf( data, 40,

               "\nE %s Invalid",

               data );

          *msgPtr          += String( data ); }; };


bool

     SplineAxis::valid() {

     

     return

          type != Invalid; };


void

     SplineAxis::calculateProgramTarget(

          float t,

          float p2f,

          float p3f,

          float p4f ) {


     float  microstepTargetF;

     if ( type == Bezier ) // Cubic Bezier Interpolation

        microstepTargetF     =

                    p2 * p2f

               +     p3 * p3f

               +     p4 * p4f;


     else // Linear Interpolation

          microstepTargetF     =

               p4 * t;

     axis->calculateTransformedTargets( microstepTargetF - currentPoint );

     currentPoint     = microstepTargetF; };