|
@@ -37,46 +37,7 @@
|
37
|
37
|
#include "../MarlinCore.h"
|
38
|
38
|
#endif
|
39
|
39
|
|
40
|
|
-float delta_segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
|
41
|
|
-
|
42
|
|
-void scara_set_axis_is_at_home(const AxisEnum axis) {
|
43
|
|
- if (axis == Z_AXIS)
|
44
|
|
- current_position.z = Z_HOME_POS;
|
45
|
|
- else {
|
46
|
|
- #if ENABLED(MORGAN_SCARA)
|
47
|
|
- // MORGAN_SCARA uses arm angles for AB home position
|
48
|
|
- ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
|
49
|
|
- //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
|
50
|
|
- #elif ENABLED(MP_SCARA)
|
51
|
|
- // MP_SCARA uses a Cartesian XY home position
|
52
|
|
- xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
|
53
|
|
- //DEBUG_ECHOPGM("homeposition");
|
54
|
|
- //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
|
55
|
|
- #elif ENABLED(AXEL_TPARA)
|
56
|
|
- xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
|
57
|
|
- //DEBUG_ECHOPGM("homeposition");
|
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
|
64
|
|
- inverse_kinematics(homeposition);
|
65
|
|
- #endif
|
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
|
|
-
|
75
|
|
- //DEBUG_ECHOPGM("Cartesian");
|
76
|
|
- //DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
|
77
|
|
- update_software_endstops(axis);
|
78
|
|
- }
|
79
|
|
-}
|
|
40
|
+float segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
|
80
|
41
|
|
81
|
42
|
#if EITHER(MORGAN_SCARA, MP_SCARA)
|
82
|
43
|
|
|
@@ -109,6 +70,27 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
109
|
70
|
//*/
|
110
|
71
|
}
|
111
|
72
|
|
|
73
|
+#endif
|
|
74
|
+
|
|
75
|
+#if ENABLED(MORGAN_SCARA)
|
|
76
|
+
|
|
77
|
+ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
|
78
|
+ if (axis == Z_AXIS)
|
|
79
|
+ current_position.z = Z_HOME_POS;
|
|
80
|
+ else {
|
|
81
|
+ // MORGAN_SCARA uses a Cartesian XY home position
|
|
82
|
+ xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
|
|
83
|
+ //DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y);
|
|
84
|
+
|
|
85
|
+ delta = homeposition;
|
|
86
|
+ forward_kinematics(delta.a, delta.b);
|
|
87
|
+ current_position[axis] = cartes[axis];
|
|
88
|
+
|
|
89
|
+ //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
|
|
90
|
+ update_software_endstops(axis);
|
|
91
|
+ }
|
|
92
|
+ }
|
|
93
|
+
|
112
|
94
|
/**
|
113
|
95
|
* Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
|
114
|
96
|
*
|
|
@@ -156,6 +138,29 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
156
|
138
|
|
157
|
139
|
#elif ENABLED(MP_SCARA)
|
158
|
140
|
|
|
141
|
+ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
|
142
|
+ if (axis == Z_AXIS)
|
|
143
|
+ current_position.z = Z_HOME_POS;
|
|
144
|
+ else {
|
|
145
|
+ // MP_SCARA uses arm angles for AB home position
|
|
146
|
+ #ifndef SCARA_OFFSET_THETA1
|
|
147
|
+ #define SCARA_OFFSET_THETA1 12 // degrees
|
|
148
|
+ #endif
|
|
149
|
+ #ifndef SCARA_OFFSET_THETA2
|
|
150
|
+ #define SCARA_OFFSET_THETA2 131 // degrees
|
|
151
|
+ #endif
|
|
152
|
+ ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
|
|
153
|
+ //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
|
|
154
|
+
|
|
155
|
+ inverse_kinematics(homeposition);
|
|
156
|
+ forward_kinematics(delta.a, delta.b);
|
|
157
|
+ current_position[axis] = cartes[axis];
|
|
158
|
+
|
|
159
|
+ //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
|
|
160
|
+ update_software_endstops(axis);
|
|
161
|
+ }
|
|
162
|
+ }
|
|
163
|
+
|
159
|
164
|
void inverse_kinematics(const xyz_pos_t &raw) {
|
160
|
165
|
const float x = raw.x, y = raw.y, c = HYPOT(x, y),
|
161
|
166
|
THETA3 = ATAN2(y, x),
|
|
@@ -175,6 +180,22 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
175
|
180
|
|
176
|
181
|
static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
|
177
|
182
|
|
|
183
|
+ void scara_set_axis_is_at_home(const AxisEnum axis) {
|
|
184
|
+ if (axis == Z_AXIS)
|
|
185
|
+ current_position.z = Z_HOME_POS;
|
|
186
|
+ else {
|
|
187
|
+ xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
|
|
188
|
+ //DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
|
|
189
|
+
|
|
190
|
+ inverse_kinematics(homeposition);
|
|
191
|
+ forward_kinematics(delta.a, delta.b, delta.c);
|
|
192
|
+ current_position[axis] = cartes[axis];
|
|
193
|
+
|
|
194
|
+ //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
|
|
195
|
+ update_software_endstops(axis);
|
|
196
|
+ }
|
|
197
|
+ }
|
|
198
|
+
|
178
|
199
|
// Convert ABC inputs in degrees to XYZ outputs in mm
|
179
|
200
|
void forward_kinematics(const float &a, const float &b, const float &c) {
|
180
|
201
|
const float w = c - b,
|