diff -r 000000000000 -r 2c8ba1964db7 FPUTransform.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FPUTransform.cpp Sat Nov 07 13:23:07 2015 +0100 @@ -0,0 +1,190 @@ +#include "FPUTransform.h" + +#if defined(UMFPUSUPPORT) && (UMFPUSUPPORT > -1) + +#include "MatrixMath.h" + +float MasterTransform[4][4]; // this is the transform that describes how to move from + // ideal coordinates to real world coords + +// private functions +void loadMatrix(float X4, float Y3, float Z1, float X2, float Y2, float Z2, float X3, float Z3, float Z4); +void transformDestination(float &X, float &Y, float &Z); + +bool FPUEnabled; // this is a bypass switch so that with one command the FPU can be + // turned off + +void loadMatrix(float X4, float Y1, float Z1, float X2, float Y2, float Z2, float X3, float Z3, float Z4) +{ +float Xdiff = X4 - X3; + serialPrintFloat(Xdiff); + SERIAL_ECHOLN(""); +float Ydiff = Y2 - Y1; + serialPrintFloat(Ydiff); + SERIAL_ECHOLN(""); +//clockwise +float ZdiffX = Z4 - Z3; + serialPrintFloat(ZdiffX); + SERIAL_ECHOLN(""); +//anti clockwise +float ZdiffY = Z1 - Z2; + serialPrintFloat(ZdiffY); + SERIAL_ECHOLN(""); + + +//modified to take advantage of small angle trig. +float Xtheta = ZdiffX / Xdiff; +// serialPrintFloat(Xtheta); +// SERIAL_ECHOLN(""); +float Ytheta = ZdiffY / Ydiff; +// serialPrintFloat(Ytheta); +// SERIAL_ECHOLN(""); +float cosxtheta = 1-(Xtheta*Xtheta)/2; +// serialPrintFloat(cosxtheta); +// SERIAL_ECHOLN(""); +float sinxtheta = Xtheta; +// serialPrintFloat(sinxtheta); +// SERIAL_ECHOLN(""); +float cosytheta = 1-(Ytheta*Ytheta)/2; +// serialPrintFloat(cosytheta); +// SERIAL_ECHOLN(""); +float sinytheta = Ytheta; +// serialPrintFloat(sinytheta); +// SERIAL_ECHOLN(""); + +//these transforms are to set the origin for each rotation +float TranslateX0[4][4] = {{1.0, 0.0, 0.0, -X3}, + {0.0, 1.0, 0.0, -Y1}, + {0.0, 0.0, 1.0, -Z3}, + {0.0, 0.0, 0.0, 1.0}}; + +float TranslateY0[4][4] = {{1.0, 0.0, 0.0, -X2}, + {0.0, 1.0, 0.0, -Y1}, + {0.0, 0.0, 1.0, -Z1}, + {0.0, 0.0, 0.0, 1.0}}; + +//rotate in Y using XZ +float TransformY[4][4] = {{cosxtheta, 0.0, sinxtheta, 0.0}, + { 0.0, 1.0, 0.0, 0.0}, + {-sinxtheta, 0.0, cosxtheta, 0.0}, + { 0.0, 0.0, 0.0, 1.0}}; +//rotate in X using YZ +float TransformX[4][4] = {{ 1.0, 0.0, 0.0, 0.0}, + { 0.0, cosytheta, sinytheta, 0.0}, + { 0.0,sinytheta, cosytheta, 0.0}, + { 0.0, 0.0, 0.0, 1.0}}; + + +// first translate point1 to 0 then rotate in Y then translate back +float MatrixStage1[4][4]; +float MatrixStage2[4][4]; +//matrixMaths.MatrixMult((float*)TranslateY0, (float*)TransformX, 4, 4, 4, (float*)MatrixStage1); +//matrixMaths.MatrixPrint((float*)MatrixStage1, 4, 4, "MatrixStage1"); +//TranslateY0[0][3] = -TranslateY0[0][3]; +//TranslateY0[1][3] = -TranslateY0[1][3]; +//TranslateY0[2][3] = -TranslateY0[2][3]; +//matrixMaths.MatrixPrint((float*)TranslateY0, 4, 4, "TranslateY0"); +//matrixMaths.MatrixMult((float*)MatrixStage1, (float*)TranslateY0, 4, 4, 4, (float*)MatrixStage2); +//matrixMaths.MatrixPrint((float*)MatrixStage2, 4, 4, "MatrixStage2"); +//Now translate point3 to 0 and rotate in x before translating back +float MatrixStage3[4][4]; +float MatrixStage4[4][4]; +//matrixMaths.MatrixMult((float*)MatrixStage2, (float*)TranslateX0, 4, 4, 4, (float*)MatrixStage3); +//matrixMaths.MatrixPrint((float*)MatrixStage3, 4, 4, "MatrixStage3"); +//matrixMaths.MatrixMult((float*)MatrixStage3, (float*)TransformY, 4, 4, 4, (float*)MatrixStage4); +matrixMaths.MatrixMult((float*)TransformX, (float*)TransformY, 4, 4, 4, (float*)MasterTransform); +matrixMaths.MatrixPrint((float*)MatrixStage4, 4, 4, "MatrixStage4"); +//TranslateX0[0][3] = -TranslateX0[0][3]; +//TranslateX0[1][3] = -TranslateX0[1][3]; +//TranslateX0[2][3] = -TranslateX0[2][3]; +//matrixMaths.MatrixPrint((float*)TranslateX0, 4, 4, "TranslateX0"); +//matrixMaths.MatrixMult((float*)MatrixStage4, (float*)TranslateX0, 4, 4, 4, (float*)MasterTransform); +//matrixMaths.MatrixPrint((float*)MasterTransform, 4, 4, "MasterTransform (pre-invert)"); + +// We now have a way to translate from real-world coordinates to idealised coortdinates, +// but what we actually want is a way to transform from the idealised g-code coordinates +// to real world coordinates. +// This is simply the inverse. +matrixMaths.MatrixInvert((float*)MasterTransform, 4); +matrixMaths.MatrixPrint((float*)MasterTransform, 4, 4, "MasterTransform"); +} + +void transformDestination(float &X, float &Y, float &Z) +{ +float oldPoint[4][1]={{X}, {Y}, {Z}, {1.0}}; +float newPoint[1][4]={{0.0,0.0,0.0,0.0}}; +matrixMaths.MatrixMult((float*)MasterTransform, (float*)oldPoint, 4, 4, 1, (float*)newPoint); +X=newPoint[0][0]; +Y=newPoint[0][1]; +Z=newPoint[0][2]; +} + +void FPUTransform_init() +{ +if (FPUEnabled == true) + { + // It is important to ensure that if the bed levelling routine has not been called the + // printer behaves as if the real world and idealised world are one and the same + matrixMaths.MatrixIdentity((float*)MasterTransform,4,4); + SERIAL_ECHO("transform configured to identity"); + } +else + { + SERIAL_ECHO("transform correction not enabled"); + } +} + +void FPUEnable() +{ + FPUEnabled = true; + FPUTransform_init(); +} + +void FPUReset() +{ + FPUTransform_init(); +} + +void FPUDisable() +{ + FPUEnabled = false; +} + +void FPUTransform_determineBedOrientation() +{ +int X3 = 15; +float X4 = max_length[X_AXIS] - 20; +float X2 = (X4 + X3) / 2; +int Y1 = 15; +float Y2 = max_length[Y_AXIS] - 5; +float Z1; +float Z2; +float Z3; +float Z4; + +//get Z for X15 Y15, X15 Y(Y_MAX_LENGTH - 15) and X(max_length[X_AXIS] - 15) Y15 +Z3 = Probe_Bed(X3,Y1,PROBE_N); +Z4 = Probe_Bed(X4,Y1,PROBE_N); +Z1 = (Z3 + Z4) / 2; +Z2 = Probe_Bed(X2,Y2,PROBE_N); +if(FPUEnabled) + { + loadMatrix(X4, Y1, Z1, X2, Y2, Z2, X3, Z3, Z4); + } +} + +void FPUTransform_transformDestination() +{ +float XPoint = destination[X_AXIS]; // float variable +float YPoint = destination[Y_AXIS]; // float variable +float ZPoint = destination[Z_AXIS]; // float variable +if(FPUEnabled) + { + transformDestination(XPoint, YPoint, ZPoint); + } +modified_destination[X_AXIS] = XPoint; // float variable +modified_destination[Y_AXIS] = YPoint; // float variable +modified_destination[Z_AXIS] = ZPoint; // float variable +} + +#endif //UMFPUSUPPORT