--- /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