FPUTransform.cpp

Fri, 17 Nov 2017 10:13:31 +0100

author
mdd
date
Fri, 17 Nov 2017 10:13:31 +0100
changeset 3
770b218a4931
parent 0
2c8ba1964db7
permissions
-rw-r--r--

proper configuration, homing and planner optimization

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

mercurial