Commit | Line | Data |
---|---|---|
11a39396 | 1 | #include "RotaryDeltaSolution.h" |
807b9b57 | 2 | #include "ActuatorCoordinates.h" |
ae640576 DP |
3 | #include "checksumm.h" |
4 | #include "ConfigValue.h" | |
5 | #include "ConfigCache.h" | |
6 | #include "libs/Kernel.h" | |
7 | #include "libs/nuts_bolts.h" | |
8 | #include "libs/Config.h" | |
9 | #include "libs/utils.h" | |
10 | #include "StreamOutputPool.h" | |
11 | #include <fastmath.h> | |
12 | ||
a86cb8d4 JM |
13 | #define delta_e_checksum CHECKSUM("delta_e") |
14 | #define delta_f_checksum CHECKSUM("delta_f") | |
15 | #define delta_re_checksum CHECKSUM("delta_re") | |
16 | #define delta_rf_checksum CHECKSUM("delta_rf") | |
17 | #define delta_z_offset_checksum CHECKSUM("delta_z_offset") | |
ae640576 | 18 | |
a86cb8d4 | 19 | #define delta_ee_offs_checksum CHECKSUM("delta_ee_offs") |
93f20a8c JM |
20 | #define tool_offset_checksum CHECKSUM("delta_tool_offset") |
21 | ||
22 | #define delta_mirror_xy_checksum CHECKSUM("delta_mirror_xy") | |
ae640576 | 23 | |
6b661ab3 DP |
24 | const static float pi = 3.14159265358979323846; // PI |
25 | const static float two_pi = 2 * pi; | |
26 | const static float sin120 = 0.86602540378443864676372317075294; //sqrt3/2.0 | |
27 | const static float cos120 = -0.5; | |
28 | const static float tan60 = 1.7320508075688772935274463415059; //sqrt3; | |
29 | const static float sin30 = 0.5; | |
30 | const static float tan30 = 0.57735026918962576450914878050196; //1/sqrt3 | |
ae640576 | 31 | |
11a39396 | 32 | RotaryDeltaSolution::RotaryDeltaSolution(Config *config) |
ae640576 | 33 | { |
51e6a11d JM |
34 | // End effector length |
35 | delta_e = config->value(delta_e_checksum)->by_default(131.636F)->as_number(); | |
ae640576 | 36 | |
51e6a11d JM |
37 | // Base length |
38 | delta_f = config->value(delta_f_checksum)->by_default(190.526F)->as_number(); | |
ae640576 | 39 | |
51e6a11d JM |
40 | // Carbon rod length |
41 | delta_re = config->value(delta_re_checksum)->by_default(270.000F)->as_number(); | |
ae640576 | 42 | |
51e6a11d JM |
43 | // Servo horn length |
44 | delta_rf = config->value(delta_rf_checksum)->by_default(90.000F)->as_number(); | |
ae640576 | 45 | |
51e6a11d JM |
46 | // Distance from delta 8mm rod/pulley to table/bed, |
47 | // NOTE: For OpenPnP, set the zero to be about 25mm above the bed.. | |
48 | delta_z_offset = config->value(delta_z_offset_checksum)->by_default(290.700F)->as_number(); | |
ae640576 | 49 | |
51e6a11d JM |
50 | // Ball joint plane to bottom of end effector surface |
51 | delta_ee_offs = config->value(delta_ee_offs_checksum)->by_default(15.000F)->as_number(); | |
ae640576 | 52 | |
51e6a11d JM |
53 | // Distance between end effector ball joint plane and tip of tool (PnP) |
54 | tool_offset = config->value(tool_offset_checksum)->by_default(30.500F)->as_number(); | |
ae640576 | 55 | |
93f20a8c JM |
56 | // mirror the XY axis |
57 | mirror_xy= config->value(delta_mirror_xy_checksum)->by_default(true)->as_bool(); | |
58 | ||
59 | debug_flag= false; | |
51e6a11d | 60 | init(); |
ae640576 DP |
61 | } |
62 | ||
63 | // inverse kinematics | |
64 | // helper functions, calculates angle theta1 (for YZ-pane) | |
11a39396 | 65 | int RotaryDeltaSolution::delta_calcAngleYZ(float x0, float y0, float z0, float &theta) |
ae640576 | 66 | { |
c90a90a6 DP |
67 | float y1 = -0.5F * tan30 * delta_f; // f/2 * tan 30 |
68 | y0 -= 0.5F * tan30 * delta_e; // shift center to edge | |
ae640576 | 69 | // z = a + b*y |
51e6a11d JM |
70 | float a = (x0 * x0 + y0 * y0 + z0 * z0 + delta_rf * delta_rf - delta_re * delta_re - y1 * y1) / (2.0F * z0); |
71 | float b = (y1 - y0) / z0; | |
ae640576 | 72 | |
51e6a11d | 73 | float d = -(a + b * y1) * (a + b * y1) + delta_rf * (b * b * delta_rf + delta_rf); // discriminant |
c90a90a6 | 74 | if (d < 0.0F) return -1; // non-existing point |
ae640576 | 75 | |
51e6a11d JM |
76 | float yj = (y1 - a * b - sqrtf(d)) / (b * b + 1.0F); // choosing outer point |
77 | float zj = a + b * yj; | |
ae640576 | 78 | |
51e6a11d | 79 | theta = 180.0F * atanf(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0F : 0.0F); |
ae640576 DP |
80 | return 0; |
81 | } | |
82 | ||
83 | // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) | |
84 | // returned status: 0=OK, -1=non-existing position | |
11a39396 | 85 | int RotaryDeltaSolution::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) |
ae640576 | 86 | { |
51e6a11d JM |
87 | float t = (delta_f - delta_e) * tan30 / 2.0F; |
88 | float degrees_to_radians = pi / 180.0F; | |
ae640576 | 89 | |
51e6a11d JM |
90 | theta1 *= degrees_to_radians; |
91 | theta2 *= degrees_to_radians; | |
92 | theta3 *= degrees_to_radians; | |
ae640576 | 93 | |
51e6a11d JM |
94 | float y1 = -(t + delta_rf * cosf(theta1)); |
95 | float z1 = -delta_rf * sinf(theta1); | |
ae640576 | 96 | |
51e6a11d JM |
97 | float y2 = (t + delta_rf * cosf(theta2)) * sin30; |
98 | float x2 = y2 * tan60; | |
99 | float z2 = -delta_rf * sinf(theta2); | |
ae640576 | 100 | |
51e6a11d JM |
101 | float y3 = (t + delta_rf * cosf(theta3)) * sin30; |
102 | float x3 = -y3 * tan60; | |
103 | float z3 = -delta_rf * sinf(theta3); | |
ae640576 | 104 | |
51e6a11d | 105 | float dnm = (y2 - y1) * x3 - (y3 - y1) * x2; |
ae640576 | 106 | |
51e6a11d JM |
107 | float w1 = y1 * y1 + z1 * z1; |
108 | float w2 = x2 * x2 + y2 * y2 + z2 * z2; | |
109 | float w3 = x3 * x3 + y3 * y3 + z3 * z3; | |
ae640576 | 110 | |
51e6a11d JM |
111 | // x = (a1*z + b1)/dnm |
112 | float a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1); | |
113 | float b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0F; | |
ae640576 | 114 | |
51e6a11d JM |
115 | // y = (a2*z + b2)/dnm; |
116 | float a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; | |
117 | float b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0F; | |
ae640576 | 118 | |
51e6a11d JM |
119 | // a*z^2 + b*z + c = 0 |
120 | float a = a1 * a1 + a2 * a2 + dnm * dnm; | |
121 | float b = 2.0F * (a1 * b1 + a2 * (b2 - y1 * dnm) - z1 * dnm * dnm); | |
122 | float c = (b2 - y1 * dnm) * (b2 - y1 * dnm) + b1 * b1 + dnm * dnm * (z1 * z1 - delta_re * delta_re); | |
ae640576 | 123 | |
51e6a11d JM |
124 | // discriminant |
125 | float d = b * b - (float)4.0F * a * c; | |
126 | if (d < 0.0F) return -1; // non-existing point | |
ae640576 | 127 | |
51e6a11d JM |
128 | z0 = -(float)0.5F * (b + sqrtf(d)) / a; |
129 | x0 = (a1 * z0 + b1) / dnm; | |
130 | y0 = (a2 * z0 + b2) / dnm; | |
ae640576 | 131 | |
b0600bc6 | 132 | z0 -= z_calc_offset; |
51e6a11d | 133 | return 0; |
ae640576 DP |
134 | } |
135 | ||
136 | ||
11a39396 | 137 | void RotaryDeltaSolution::init() |
51e6a11d | 138 | { |
51e6a11d | 139 | //these are calculated here and not in the config() as these variables can be fine tuned by the user. |
a86cb8d4 | 140 | z_calc_offset = -(delta_z_offset - tool_offset - delta_ee_offs); |
ae640576 DP |
141 | } |
142 | ||
11a39396 | 143 | void RotaryDeltaSolution::cartesian_to_actuator(const float cartesian_mm[], ActuatorCoordinates &actuator_mm ) |
ae640576 | 144 | { |
51e6a11d JM |
145 | //We need to translate the Cartesian coordinates in mm to the actuator position required in mm so the stepper motor functions |
146 | float alpha_theta = 0.0F; | |
147 | float beta_theta = 0.0F; | |
148 | float gamma_theta = 0.0F; | |
149 | ||
93f20a8c JM |
150 | //Code from Trossen Robotics tutorial, has X in front Y to the right and Z to the left |
151 | // firepick is X at the back and negates X0 X0 | |
152 | // selected by a config option | |
153 | float x0 = cartesian_mm[X_AXIS]; | |
154 | float y0 = cartesian_mm[Y_AXIS]; | |
155 | if(mirror_xy) { | |
156 | x0= -x0; | |
157 | y0= -y0; | |
158 | } | |
51e6a11d | 159 | |
51e6a11d JM |
160 | float z_with_offset = cartesian_mm[Z_AXIS] + z_calc_offset; //The delta calculation below places zero at the top. Subtract the Z offset to make zero at the bottom. |
161 | ||
162 | int status = delta_calcAngleYZ(x0, y0, z_with_offset, alpha_theta); | |
163 | if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z_with_offset, beta_theta); // rotate co-ordinates to +120 deg | |
164 | if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z_with_offset, gamma_theta); // rotate co-ordinates to -120 deg | |
165 | ||
166 | if (status == -1) { //something went wrong, | |
167 | //force to actuator FPD home position as we know this is a valid position | |
168 | actuator_mm[ALPHA_STEPPER] = 0; | |
169 | actuator_mm[BETA_STEPPER ] = 0; | |
170 | actuator_mm[GAMMA_STEPPER] = 0; | |
171 | ||
2577a122 | 172 | //DEBUG CODE, uncomment the following to help determine what may be happening if you are trying to adapt this to your own different rotary delta. |
87bb0b79 JM |
173 | if(debug_flag) { |
174 | THEKERNEL->streams->printf("//ERROR: Delta calculation fail! Unable to move to:\n"); | |
175 | THEKERNEL->streams->printf("// x= %f\n", cartesian_mm[X_AXIS]); | |
176 | THEKERNEL->streams->printf("// y= %f\n", cartesian_mm[Y_AXIS]); | |
177 | THEKERNEL->streams->printf("// z= %f\n", cartesian_mm[Z_AXIS]); | |
178 | THEKERNEL->streams->printf("// CalcZ= %f\n", z_calc_offset); | |
179 | THEKERNEL->streams->printf("// Offz= %f\n", z_with_offset); | |
180 | } | |
51e6a11d JM |
181 | } else { |
182 | actuator_mm[ALPHA_STEPPER] = alpha_theta; | |
183 | actuator_mm[BETA_STEPPER ] = beta_theta; | |
184 | actuator_mm[GAMMA_STEPPER] = gamma_theta; | |
185 | ||
87bb0b79 JM |
186 | if(debug_flag) { |
187 | THEKERNEL->streams->printf("//cartesian x= %f\n\r", cartesian_mm[X_AXIS]); | |
188 | THEKERNEL->streams->printf("// y= %f\n\r", cartesian_mm[Y_AXIS]); | |
189 | THEKERNEL->streams->printf("// z= %f\n\r", cartesian_mm[Z_AXIS]); | |
190 | THEKERNEL->streams->printf("// Offz= %f\n\r", z_with_offset); | |
191 | THEKERNEL->streams->printf("// actuator x= %f\n\r", actuator_mm[X_AXIS]); | |
192 | THEKERNEL->streams->printf("// y= %f\n\r", actuator_mm[Y_AXIS]); | |
193 | THEKERNEL->streams->printf("// z= %f\n\r", actuator_mm[Z_AXIS]); | |
194 | } | |
51e6a11d | 195 | } |
ae640576 DP |
196 | |
197 | } | |
198 | ||
11a39396 | 199 | void RotaryDeltaSolution::actuator_to_cartesian(const ActuatorCoordinates &actuator_mm, float cartesian_mm[] ) |
ae640576 | 200 | { |
93f20a8c | 201 | float x, y, z; |
ae640576 | 202 | //Use forward kinematics |
93f20a8c JM |
203 | delta_calcForward(actuator_mm[ALPHA_STEPPER], actuator_mm[BETA_STEPPER ], actuator_mm[GAMMA_STEPPER], x, y, z); |
204 | if(mirror_xy) { | |
205 | cartesian_mm[X_AXIS]= -x; | |
206 | cartesian_mm[Y_AXIS]= -y; | |
207 | cartesian_mm[Z_AXIS]= z; | |
208 | }else{ | |
209 | cartesian_mm[X_AXIS]= x; | |
210 | cartesian_mm[Y_AXIS]= y; | |
211 | cartesian_mm[Z_AXIS]= z; | |
212 | } | |
ae640576 DP |
213 | } |
214 | ||
11a39396 | 215 | bool RotaryDeltaSolution::set_optional(const arm_options_t &options) |
51e6a11d | 216 | { |
ae640576 DP |
217 | |
218 | for(auto &i : options) { | |
219 | switch(i.first) { | |
51e6a11d JM |
220 | case 'A': delta_e = i.second; break; |
221 | case 'B': delta_f = i.second; break; | |
222 | case 'C': delta_re = i.second; break; | |
223 | case 'D': delta_rf = i.second; break; | |
87bb0b79 JM |
224 | case 'E': delta_z_offset = i.second; break; |
225 | case 'I': delta_ee_offs = i.second; break; | |
51e6a11d | 226 | case 'H': tool_offset = i.second; break; |
87bb0b79 | 227 | case 'W': debug_flag = i.second != 0; break; |
ae640576 DP |
228 | } |
229 | } | |
230 | init(); | |
231 | return true; | |
232 | } | |
233 | ||
11a39396 | 234 | bool RotaryDeltaSolution::get_optional(arm_options_t &options, bool force_all) |
51e6a11d | 235 | { |
87bb0b79 JM |
236 | options['A'] = this->delta_e; |
237 | options['B'] = this->delta_f; | |
238 | options['C'] = this->delta_re; | |
239 | options['D'] = this->delta_rf; | |
240 | options['E'] = this->delta_z_offset; | |
241 | options['I'] = this->delta_ee_offs; | |
242 | options['H'] = this->tool_offset; | |
ae640576 DP |
243 | |
244 | return true; | |
245 | }; | |
246 |