|
@@ -33,10 +33,7 @@
|
33
|
33
|
#include "planner.h"
|
34
|
34
|
|
35
|
35
|
#if ENABLED(AXEL_TPARA)
|
36
|
|
- // For homing, as in delta
|
37
|
|
- #include "planner.h"
|
38
|
36
|
#include "endstops.h"
|
39
|
|
- #include "../lcd/marlinui.h"
|
40
|
37
|
#include "../MarlinCore.h"
|
41
|
38
|
#endif
|
42
|
39
|
|
|
@@ -46,30 +43,35 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
46
|
43
|
if (axis == Z_AXIS)
|
47
|
44
|
current_position.z = Z_HOME_POS;
|
48
|
45
|
else {
|
49
|
|
- xyz_pos_t homeposition;
|
50
|
|
- LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
|
51
|
46
|
#if ENABLED(MORGAN_SCARA)
|
52
|
47
|
// MORGAN_SCARA uses arm angles for AB home position
|
|
48
|
+ ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
|
53
|
49
|
//DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
|
54
|
|
- inverse_kinematics(homeposition);
|
55
|
|
- forward_kinematics_SCARA(delta.a, delta.b);
|
56
|
|
- current_position[axis] = cartes[axis];
|
57
|
50
|
#elif ENABLED(MP_SCARA)
|
58
|
51
|
// MP_SCARA uses a Cartesian XY home position
|
|
52
|
+ xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
|
59
|
53
|
//DEBUG_ECHOPGM("homeposition");
|
60
|
54
|
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
|
61
|
|
- delta.a = SCARA_OFFSET_THETA1;
|
62
|
|
- delta.b = SCARA_OFFSET_THETA2;
|
63
|
|
- forward_kinematics_SCARA(delta.a, delta.b);
|
64
|
|
- current_position[axis] = cartes[axis];
|
65
|
55
|
#elif ENABLED(AXEL_TPARA)
|
|
56
|
+ xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
|
66
|
57
|
//DEBUG_ECHOPGM("homeposition");
|
67
|
58
|
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
|
|
59
|
+ #endif
|
|
60
|
+
|
|
61
|
+ #if ENABLED(MORGAN_SCARA)
|
|
62
|
+ delta = homeposition;
|
|
63
|
+ #else
|
68
|
64
|
inverse_kinematics(homeposition);
|
69
|
|
- forward_kinematics_TPARA(delta.a, delta.b, delta.c);
|
70
|
|
- current_position[axis] = cartes[axis];
|
71
|
65
|
#endif
|
72
|
66
|
|
|
67
|
+ #if EITHER(MORGAN_SCARA, MP_SCARA)
|
|
68
|
+ forward_kinematics(delta.a, delta.b);
|
|
69
|
+ #elif ENABLED(AXEL_TPARA)
|
|
70
|
+ forward_kinematics(delta.a, delta.b, delta.c);
|
|
71
|
+ #endif
|
|
72
|
+
|
|
73
|
+ current_position[axis] = cartes[axis];
|
|
74
|
+
|
73
|
75
|
//DEBUG_ECHOPGM("Cartesian");
|
74
|
76
|
//DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
|
75
|
77
|
update_software_endstops(axis);
|
|
@@ -85,7 +87,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
85
|
87
|
* Maths and first version by QHARLEY.
|
86
|
88
|
* Integrated into Marlin and slightly restructured by Joachim Cerny.
|
87
|
89
|
*/
|
88
|
|
- void forward_kinematics_SCARA(const float &a, const float &b) {
|
|
90
|
+ void forward_kinematics(const float &a, const float &b) {
|
89
|
91
|
const float a_sin = sin(RADIANS(a)) * L1,
|
90
|
92
|
a_cos = cos(RADIANS(a)) * L1,
|
91
|
93
|
b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2,
|
|
@@ -174,7 +176,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
174
|
176
|
static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
|
175
|
177
|
|
176
|
178
|
// Convert ABC inputs in degrees to XYZ outputs in mm
|
177
|
|
- void forward_kinematics_TPARA(const float &a, const float &b, const float &c) {
|
|
179
|
+ void forward_kinematics(const float &a, const float &b, const float &c) {
|
178
|
180
|
const float w = c - b,
|
179
|
181
|
r = L1 * cos(RADIANS(b)) + L2 * sin(RADIANS(w - (90 - b))),
|
180
|
182
|
x = r * cos(RADIANS(a)),
|
|
@@ -227,7 +229,6 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
227
|
229
|
homeaxis(C_AXIS);
|
228
|
230
|
homeaxis(B_AXIS);
|
229
|
231
|
|
230
|
|
-
|
231
|
232
|
// Set all carriages to their home positions
|
232
|
233
|
// Do this here all at once for Delta, because
|
233
|
234
|
// XYZ isn't ABC. Applying this per-tower would
|