|
@@ -8092,68 +8092,78 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8092
|
8092
|
* This calls planner.buffer_line several times, adding
|
8093
|
8093
|
* small incremental moves for DELTA or SCARA.
|
8094
|
8094
|
*/
|
8095
|
|
- inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) {
|
|
8095
|
+ inline bool prepare_kinematic_move_to(float ltarget[NUM_AXIS]) {
|
8096
|
8096
|
|
8097
|
8097
|
// Get the top feedrate of the move in the XY plane
|
8098
|
8098
|
float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
|
8099
|
8099
|
|
8100
|
|
- // If the move is only in Z don't split up the move.
|
8101
|
|
- // This shortcut cannot be used if planar bed leveling
|
8102
|
|
- // is in use, but is fine with mesh-based bed leveling
|
8103
|
|
- if (logical[X_AXIS] == current_position[X_AXIS] && logical[Y_AXIS] == current_position[Y_AXIS]) {
|
8104
|
|
- inverse_kinematics(logical);
|
8105
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
8100
|
+ // If the move is only in Z/E don't split up the move
|
|
8101
|
+ if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
|
|
8102
|
+ inverse_kinematics(ltarget);
|
|
8103
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
|
8106
|
8104
|
return true;
|
8107
|
8105
|
}
|
8108
|
8106
|
|
8109
|
|
- // Get the distance moved in XYZ
|
|
8107
|
+ // Get the cartesian distances moved in XYZE
|
8110
|
8108
|
float difference[NUM_AXIS];
|
8111
|
|
- LOOP_XYZE(i) difference[i] = logical[i] - current_position[i];
|
|
8109
|
+ LOOP_XYZE(i) difference[i] = ltarget[i] - current_position[i];
|
8112
|
8110
|
|
|
8111
|
+ // Get the linear distance in XYZ
|
8113
|
8112
|
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
|
|
8113
|
+
|
|
8114
|
+ // If the move is very short, check the E move distance
|
8114
|
8115
|
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
|
|
8116
|
+
|
|
8117
|
+ // No E move either? Game over.
|
8115
|
8118
|
if (UNEAR_ZERO(cartesian_mm)) return false;
|
8116
|
8119
|
|
8117
|
8120
|
// Minimum number of seconds to move the given distance
|
8118
|
8121
|
float seconds = cartesian_mm / _feedrate_mm_s;
|
8119
|
8122
|
|
8120
|
8123
|
// The number of segments-per-second times the duration
|
8121
|
|
- // gives the number of segments we should produce
|
|
8124
|
+ // gives the number of segments
|
8122
|
8125
|
uint16_t segments = delta_segments_per_second * seconds;
|
8123
|
8126
|
|
|
8127
|
+ // For SCARA minimum segment size is 0.5mm
|
8124
|
8128
|
#if IS_SCARA
|
8125
|
8129
|
NOMORE(segments, cartesian_mm * 2);
|
8126
|
8130
|
#endif
|
8127
|
8131
|
|
|
8132
|
+ // At least one segment is required
|
8128
|
8133
|
NOLESS(segments, 1);
|
8129
|
8134
|
|
8130
|
|
- // Each segment produces this much of the move
|
8131
|
|
- float inv_segments = 1.0 / segments,
|
8132
|
|
- segment_distance[XYZE] = {
|
8133
|
|
- difference[X_AXIS] * inv_segments,
|
8134
|
|
- difference[Y_AXIS] * inv_segments,
|
8135
|
|
- difference[Z_AXIS] * inv_segments,
|
8136
|
|
- difference[E_AXIS] * inv_segments
|
|
8135
|
+ // The approximate length of each segment
|
|
8136
|
+ float segment_distance[XYZE] = {
|
|
8137
|
+ difference[X_AXIS] / segments,
|
|
8138
|
+ difference[Y_AXIS] / segments,
|
|
8139
|
+ difference[Z_AXIS] / segments,
|
|
8140
|
+ difference[E_AXIS] / segments
|
8137
|
8141
|
};
|
8138
|
8142
|
|
8139
|
8143
|
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
|
8140
|
8144
|
// SERIAL_ECHOPAIR(" seconds=", seconds);
|
8141
|
8145
|
// SERIAL_ECHOLNPAIR(" segments=", segments);
|
8142
|
8146
|
|
8143
|
|
- // Send all the segments to the planner
|
|
8147
|
+ // Drop one segment so the last move is to the exact target.
|
|
8148
|
+ // If there's only 1 segment, loops will be skipped entirely.
|
|
8149
|
+ --segments;
|
|
8150
|
+
|
|
8151
|
+ // Using "raw" coordinates saves 6 float subtractions
|
|
8152
|
+ // per segment, saving valuable CPU cycles
|
8144
|
8153
|
|
8145
|
8154
|
#if ENABLED(USE_RAW_KINEMATICS)
|
8146
|
8155
|
|
8147
|
8156
|
// Get the raw current position as starting point
|
8148
|
|
- float raw[ABC] = {
|
|
8157
|
+ float raw[XYZE] = {
|
8149
|
8158
|
RAW_CURRENT_POSITION(X_AXIS),
|
8150
|
8159
|
RAW_CURRENT_POSITION(Y_AXIS),
|
8151
|
|
- RAW_CURRENT_POSITION(Z_AXIS)
|
|
8160
|
+ RAW_CURRENT_POSITION(Z_AXIS),
|
|
8161
|
+ current_position[E_AXIS]
|
8152
|
8162
|
};
|
8153
|
8163
|
|
8154
|
|
- #define DELTA_E raw[E_AXIS]
|
8155
|
|
- #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) raw[i] += ADDEND;
|
|
8164
|
+ #define DELTA_VAR raw
|
8156
|
8165
|
|
|
8166
|
+ // Delta can inline its kinematics
|
8157
|
8167
|
#if ENABLED(DELTA)
|
8158
|
8168
|
#define DELTA_IK() DELTA_RAW_IK()
|
8159
|
8169
|
#else
|
|
@@ -8163,11 +8173,12 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8163
|
8173
|
#else
|
8164
|
8174
|
|
8165
|
8175
|
// Get the logical current position as starting point
|
8166
|
|
- LOOP_XYZE(i) logical[i] = current_position[i];
|
|
8176
|
+ float logical[XYZE];
|
|
8177
|
+ memcpy(logical, current_position, sizeof(logical));
|
8167
|
8178
|
|
8168
|
|
- #define DELTA_E logical[E_AXIS]
|
8169
|
|
- #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) logical[i] += ADDEND;
|
|
8179
|
+ #define DELTA_VAR logical
|
8170
|
8180
|
|
|
8181
|
+ // Delta can inline its kinematics
|
8171
|
8182
|
#if ENABLED(DELTA)
|
8172
|
8183
|
#define DELTA_IK() DELTA_LOGICAL_IK()
|
8173
|
8184
|
#else
|
|
@@ -8178,16 +8189,26 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8178
|
8189
|
|
8179
|
8190
|
#if ENABLED(USE_DELTA_IK_INTERPOLATION)
|
8180
|
8191
|
|
8181
|
|
- // Get the starting delta for interpolation
|
8182
|
|
- if (segments >= 2) inverse_kinematics(logical);
|
|
8192
|
+ // Only interpolate XYZ. Advance E normally.
|
|
8193
|
+ #define DELTA_NEXT(ADDEND) LOOP_XYZ(i) DELTA_VAR[i] += ADDEND;
|
|
8194
|
+
|
|
8195
|
+ // Get the starting delta if interpolation is possible
|
|
8196
|
+ if (segments >= 2) DELTA_IK();
|
8183
|
8197
|
|
|
8198
|
+ // Loop using decrement
|
8184
|
8199
|
for (uint16_t s = segments + 1; --s;) {
|
8185
|
|
- if (s > 1) {
|
|
8200
|
+ // Are there at least 2 moves left?
|
|
8201
|
+ if (s >= 2) {
|
8186
|
8202
|
// Save the previous delta for interpolation
|
8187
|
8203
|
float prev_delta[ABC] = { delta[A_AXIS], delta[B_AXIS], delta[C_AXIS] };
|
8188
|
8204
|
|
8189
|
8205
|
// Get the delta 2 segments ahead (rather than the next)
|
8190
|
8206
|
DELTA_NEXT(segment_distance[i] + segment_distance[i]);
|
|
8207
|
+
|
|
8208
|
+ // Advance E normally
|
|
8209
|
+ DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
|
|
8210
|
+
|
|
8211
|
+ // Get the exact delta for the move after this
|
8191
|
8212
|
DELTA_IK();
|
8192
|
8213
|
|
8193
|
8214
|
// Move to the interpolated delta position first
|
|
@@ -8195,33 +8216,43 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8195
|
8216
|
(prev_delta[A_AXIS] + delta[A_AXIS]) * 0.5,
|
8196
|
8217
|
(prev_delta[B_AXIS] + delta[B_AXIS]) * 0.5,
|
8197
|
8218
|
(prev_delta[C_AXIS] + delta[C_AXIS]) * 0.5,
|
8198
|
|
- logical[E_AXIS], _feedrate_mm_s, active_extruder
|
|
8219
|
+ DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder
|
8199
|
8220
|
);
|
8200
|
8221
|
|
|
8222
|
+ // Advance E once more for the next move
|
|
8223
|
+ DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
|
|
8224
|
+
|
8201
|
8225
|
// Do an extra decrement of the loop
|
8202
|
8226
|
--s;
|
8203
|
8227
|
}
|
8204
|
8228
|
else {
|
8205
|
|
- // Get the last segment delta (only when segments is odd)
|
8206
|
|
- DELTA_NEXT(segment_distance[i])
|
|
8229
|
+ // Get the last segment delta. (Used when segments is odd)
|
|
8230
|
+ DELTA_NEXT(segment_distance[i]);
|
|
8231
|
+ DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
|
8207
|
8232
|
DELTA_IK();
|
8208
|
8233
|
}
|
8209
|
8234
|
|
8210
|
8235
|
// Move to the non-interpolated position
|
8211
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_E, _feedrate_mm_s, active_extruder);
|
|
8236
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
|
8212
|
8237
|
}
|
8213
|
8238
|
|
8214
|
8239
|
#else
|
8215
|
8240
|
|
|
8241
|
+ #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) DELTA_VAR[i] += ADDEND;
|
|
8242
|
+
|
8216
|
8243
|
// For non-interpolated delta calculate every segment
|
8217
|
8244
|
for (uint16_t s = segments + 1; --s;) {
|
8218
|
|
- DELTA_NEXT(segment_distance[i])
|
|
8245
|
+ DELTA_NEXT(segment_distance[i]);
|
8219
|
8246
|
DELTA_IK();
|
8220
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
8247
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
|
8221
|
8248
|
}
|
8222
|
8249
|
|
8223
|
8250
|
#endif
|
8224
|
8251
|
|
|
8252
|
+ // Since segment_distance is only approximate,
|
|
8253
|
+ // the final move must be to the exact destination.
|
|
8254
|
+ inverse_kinematics(ltarget);
|
|
8255
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
|
8225
|
8256
|
return true;
|
8226
|
8257
|
}
|
8227
|
8258
|
|