fix rotary delta FK to be mirrored like the IK
[clinton/Smoothieware.git] / src / modules / robot / arm_solutions / RotaryDeltaSolution.cpp
CommitLineData
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
24const static float pi = 3.14159265358979323846; // PI
25const static float two_pi = 2 * pi;
26const static float sin120 = 0.86602540378443864676372317075294; //sqrt3/2.0
27const static float cos120 = -0.5;
28const static float tan60 = 1.7320508075688772935274463415059; //sqrt3;
29const static float sin30 = 0.5;
30const static float tan30 = 0.57735026918962576450914878050196; //1/sqrt3
ae640576 31
11a39396 32RotaryDeltaSolution::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 65int 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 85int 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 137void 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 143void 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 199void 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 215bool 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 234bool 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