comparison: FPUTransform.cpp
FPUTransform.cpp
- changeset 0
- 2c8ba1964db7
equal
deleted
inserted
replaced
| -1:000000000000 | 0:2c8ba1964db7 |
|---|---|
| 1 #include "FPUTransform.h" | |
| 2 | |
| 3 #if defined(UMFPUSUPPORT) && (UMFPUSUPPORT > -1) | |
| 4 | |
| 5 #include "MatrixMath.h" | |
| 6 | |
| 7 float MasterTransform[4][4]; // this is the transform that describes how to move from | |
| 8 // ideal coordinates to real world coords | |
| 9 | |
| 10 // private functions | |
| 11 void loadMatrix(float X4, float Y3, float Z1, float X2, float Y2, float Z2, float X3, float Z3, float Z4); | |
| 12 void transformDestination(float &X, float &Y, float &Z); | |
| 13 | |
| 14 bool FPUEnabled; // this is a bypass switch so that with one command the FPU can be | |
| 15 // turned off | |
| 16 | |
| 17 void loadMatrix(float X4, float Y1, float Z1, float X2, float Y2, float Z2, float X3, float Z3, float Z4) | |
| 18 { | |
| 19 float Xdiff = X4 - X3; | |
| 20 serialPrintFloat(Xdiff); | |
| 21 SERIAL_ECHOLN(""); | |
| 22 float Ydiff = Y2 - Y1; | |
| 23 serialPrintFloat(Ydiff); | |
| 24 SERIAL_ECHOLN(""); | |
| 25 //clockwise | |
| 26 float ZdiffX = Z4 - Z3; | |
| 27 serialPrintFloat(ZdiffX); | |
| 28 SERIAL_ECHOLN(""); | |
| 29 //anti clockwise | |
| 30 float ZdiffY = Z1 - Z2; | |
| 31 serialPrintFloat(ZdiffY); | |
| 32 SERIAL_ECHOLN(""); | |
| 33 | |
| 34 | |
| 35 //modified to take advantage of small angle trig. | |
| 36 float Xtheta = ZdiffX / Xdiff; | |
| 37 // serialPrintFloat(Xtheta); | |
| 38 // SERIAL_ECHOLN(""); | |
| 39 float Ytheta = ZdiffY / Ydiff; | |
| 40 // serialPrintFloat(Ytheta); | |
| 41 // SERIAL_ECHOLN(""); | |
| 42 float cosxtheta = 1-(Xtheta*Xtheta)/2; | |
| 43 // serialPrintFloat(cosxtheta); | |
| 44 // SERIAL_ECHOLN(""); | |
| 45 float sinxtheta = Xtheta; | |
| 46 // serialPrintFloat(sinxtheta); | |
| 47 // SERIAL_ECHOLN(""); | |
| 48 float cosytheta = 1-(Ytheta*Ytheta)/2; | |
| 49 // serialPrintFloat(cosytheta); | |
| 50 // SERIAL_ECHOLN(""); | |
| 51 float sinytheta = Ytheta; | |
| 52 // serialPrintFloat(sinytheta); | |
| 53 // SERIAL_ECHOLN(""); | |
| 54 | |
| 55 //these transforms are to set the origin for each rotation | |
| 56 float TranslateX0[4][4] = {{1.0, 0.0, 0.0, -X3}, | |
| 57 {0.0, 1.0, 0.0, -Y1}, | |
| 58 {0.0, 0.0, 1.0, -Z3}, | |
| 59 {0.0, 0.0, 0.0, 1.0}}; | |
| 60 | |
| 61 float TranslateY0[4][4] = {{1.0, 0.0, 0.0, -X2}, | |
| 62 {0.0, 1.0, 0.0, -Y1}, | |
| 63 {0.0, 0.0, 1.0, -Z1}, | |
| 64 {0.0, 0.0, 0.0, 1.0}}; | |
| 65 | |
| 66 //rotate in Y using XZ | |
| 67 float TransformY[4][4] = {{cosxtheta, 0.0, sinxtheta, 0.0}, | |
| 68 { 0.0, 1.0, 0.0, 0.0}, | |
| 69 {-sinxtheta, 0.0, cosxtheta, 0.0}, | |
| 70 { 0.0, 0.0, 0.0, 1.0}}; | |
| 71 //rotate in X using YZ | |
| 72 float TransformX[4][4] = {{ 1.0, 0.0, 0.0, 0.0}, | |
| 73 { 0.0, cosytheta, sinytheta, 0.0}, | |
| 74 { 0.0,sinytheta, cosytheta, 0.0}, | |
| 75 { 0.0, 0.0, 0.0, 1.0}}; | |
| 76 | |
| 77 | |
| 78 // first translate point1 to 0 then rotate in Y then translate back | |
| 79 float MatrixStage1[4][4]; | |
| 80 float MatrixStage2[4][4]; | |
| 81 //matrixMaths.MatrixMult((float*)TranslateY0, (float*)TransformX, 4, 4, 4, (float*)MatrixStage1); | |
| 82 //matrixMaths.MatrixPrint((float*)MatrixStage1, 4, 4, "MatrixStage1"); | |
| 83 //TranslateY0[0][3] = -TranslateY0[0][3]; | |
| 84 //TranslateY0[1][3] = -TranslateY0[1][3]; | |
| 85 //TranslateY0[2][3] = -TranslateY0[2][3]; | |
| 86 //matrixMaths.MatrixPrint((float*)TranslateY0, 4, 4, "TranslateY0"); | |
| 87 //matrixMaths.MatrixMult((float*)MatrixStage1, (float*)TranslateY0, 4, 4, 4, (float*)MatrixStage2); | |
| 88 //matrixMaths.MatrixPrint((float*)MatrixStage2, 4, 4, "MatrixStage2"); | |
| 89 //Now translate point3 to 0 and rotate in x before translating back | |
| 90 float MatrixStage3[4][4]; | |
| 91 float MatrixStage4[4][4]; | |
| 92 //matrixMaths.MatrixMult((float*)MatrixStage2, (float*)TranslateX0, 4, 4, 4, (float*)MatrixStage3); | |
| 93 //matrixMaths.MatrixPrint((float*)MatrixStage3, 4, 4, "MatrixStage3"); | |
| 94 //matrixMaths.MatrixMult((float*)MatrixStage3, (float*)TransformY, 4, 4, 4, (float*)MatrixStage4); | |
| 95 matrixMaths.MatrixMult((float*)TransformX, (float*)TransformY, 4, 4, 4, (float*)MasterTransform); | |
| 96 matrixMaths.MatrixPrint((float*)MatrixStage4, 4, 4, "MatrixStage4"); | |
| 97 //TranslateX0[0][3] = -TranslateX0[0][3]; | |
| 98 //TranslateX0[1][3] = -TranslateX0[1][3]; | |
| 99 //TranslateX0[2][3] = -TranslateX0[2][3]; | |
| 100 //matrixMaths.MatrixPrint((float*)TranslateX0, 4, 4, "TranslateX0"); | |
| 101 //matrixMaths.MatrixMult((float*)MatrixStage4, (float*)TranslateX0, 4, 4, 4, (float*)MasterTransform); | |
| 102 //matrixMaths.MatrixPrint((float*)MasterTransform, 4, 4, "MasterTransform (pre-invert)"); | |
| 103 | |
| 104 // We now have a way to translate from real-world coordinates to idealised coortdinates, | |
| 105 // but what we actually want is a way to transform from the idealised g-code coordinates | |
| 106 // to real world coordinates. | |
| 107 // This is simply the inverse. | |
| 108 matrixMaths.MatrixInvert((float*)MasterTransform, 4); | |
| 109 matrixMaths.MatrixPrint((float*)MasterTransform, 4, 4, "MasterTransform"); | |
| 110 } | |
| 111 | |
| 112 void transformDestination(float &X, float &Y, float &Z) | |
| 113 { | |
| 114 float oldPoint[4][1]={{X}, {Y}, {Z}, {1.0}}; | |
| 115 float newPoint[1][4]={{0.0,0.0,0.0,0.0}}; | |
| 116 matrixMaths.MatrixMult((float*)MasterTransform, (float*)oldPoint, 4, 4, 1, (float*)newPoint); | |
| 117 X=newPoint[0][0]; | |
| 118 Y=newPoint[0][1]; | |
| 119 Z=newPoint[0][2]; | |
| 120 } | |
| 121 | |
| 122 void FPUTransform_init() | |
| 123 { | |
| 124 if (FPUEnabled == true) | |
| 125 { | |
| 126 // It is important to ensure that if the bed levelling routine has not been called the | |
| 127 // printer behaves as if the real world and idealised world are one and the same | |
| 128 matrixMaths.MatrixIdentity((float*)MasterTransform,4,4); | |
| 129 SERIAL_ECHO("transform configured to identity"); | |
| 130 } | |
| 131 else | |
| 132 { | |
| 133 SERIAL_ECHO("transform correction not enabled"); | |
| 134 } | |
| 135 } | |
| 136 | |
| 137 void FPUEnable() | |
| 138 { | |
| 139 FPUEnabled = true; | |
| 140 FPUTransform_init(); | |
| 141 } | |
| 142 | |
| 143 void FPUReset() | |
| 144 { | |
| 145 FPUTransform_init(); | |
| 146 } | |
| 147 | |
| 148 void FPUDisable() | |
| 149 { | |
| 150 FPUEnabled = false; | |
| 151 } | |
| 152 | |
| 153 void FPUTransform_determineBedOrientation() | |
| 154 { | |
| 155 int X3 = 15; | |
| 156 float X4 = max_length[X_AXIS] - 20; | |
| 157 float X2 = (X4 + X3) / 2; | |
| 158 int Y1 = 15; | |
| 159 float Y2 = max_length[Y_AXIS] - 5; | |
| 160 float Z1; | |
| 161 float Z2; | |
| 162 float Z3; | |
| 163 float Z4; | |
| 164 | |
| 165 //get Z for X15 Y15, X15 Y(Y_MAX_LENGTH - 15) and X(max_length[X_AXIS] - 15) Y15 | |
| 166 Z3 = Probe_Bed(X3,Y1,PROBE_N); | |
| 167 Z4 = Probe_Bed(X4,Y1,PROBE_N); | |
| 168 Z1 = (Z3 + Z4) / 2; | |
| 169 Z2 = Probe_Bed(X2,Y2,PROBE_N); | |
| 170 if(FPUEnabled) | |
| 171 { | |
| 172 loadMatrix(X4, Y1, Z1, X2, Y2, Z2, X3, Z3, Z4); | |
| 173 } | |
| 174 } | |
| 175 | |
| 176 void FPUTransform_transformDestination() | |
| 177 { | |
| 178 float XPoint = destination[X_AXIS]; // float variable | |
| 179 float YPoint = destination[Y_AXIS]; // float variable | |
| 180 float ZPoint = destination[Z_AXIS]; // float variable | |
| 181 if(FPUEnabled) | |
| 182 { | |
| 183 transformDestination(XPoint, YPoint, ZPoint); | |
| 184 } | |
| 185 modified_destination[X_AXIS] = XPoint; // float variable | |
| 186 modified_destination[Y_AXIS] = YPoint; // float variable | |
| 187 modified_destination[Z_AXIS] = ZPoint; // float variable | |
| 188 } | |
| 189 | |
| 190 #endif //UMFPUSUPPORT |
