#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; };