|
@@ -7992,9 +7992,9 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
7992
|
7992
|
* This calls planner.buffer_line several times, adding
|
7993
|
7993
|
* small incremental moves for DELTA or SCARA.
|
7994
|
7994
|
*/
|
7995
|
|
- inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
|
|
7995
|
+ inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) {
|
7996
|
7996
|
float difference[NUM_AXIS];
|
7997
|
|
- LOOP_XYZE(i) difference[i] = target[i] - current_position[i];
|
|
7997
|
+ LOOP_XYZE(i) difference[i] = logical[i] - current_position[i];
|
7998
|
7998
|
|
7999
|
7999
|
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
|
8000
|
8000
|
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
|
|
@@ -8013,18 +8013,18 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8013
|
8013
|
float fraction = float(s) * inv_steps;
|
8014
|
8014
|
|
8015
|
8015
|
LOOP_XYZE(i)
|
8016
|
|
- target[i] = current_position[i] + difference[i] * fraction;
|
|
8016
|
+ logical[i] = current_position[i] + difference[i] * fraction;
|
8017
|
8017
|
|
8018
|
|
- inverse_kinematics(target);
|
|
8018
|
+ inverse_kinematics(logical);
|
8019
|
8019
|
|
8020
|
8020
|
#if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
8021
|
|
- if (!bed_leveling_in_progress) adjust_delta(target);
|
|
8021
|
+ if (!bed_leveling_in_progress) adjust_delta(logical);
|
8022
|
8022
|
#endif
|
8023
|
8023
|
|
8024
|
|
- //DEBUG_POS("prepare_kinematic_move_to", target);
|
|
8024
|
+ //DEBUG_POS("prepare_kinematic_move_to", logical);
|
8025
|
8025
|
//DEBUG_POS("prepare_kinematic_move_to", delta);
|
8026
|
8026
|
|
8027
|
|
- planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
8027
|
+ planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
8028
|
8028
|
}
|
8029
|
8029
|
return true;
|
8030
|
8030
|
}
|
|
@@ -8156,20 +8156,20 @@ void prepare_move_to_destination() {
|
8156
|
8156
|
* options for G2/G3 arc generation. In future these options may be GCode tunable.
|
8157
|
8157
|
*/
|
8158
|
8158
|
void plan_arc(
|
8159
|
|
- float target[NUM_AXIS], // Destination position
|
8160
|
|
- float* offset, // Center of rotation relative to current_position
|
8161
|
|
- uint8_t clockwise // Clockwise?
|
|
8159
|
+ float logical[NUM_AXIS], // Destination position
|
|
8160
|
+ float* offset, // Center of rotation relative to current_position
|
|
8161
|
+ uint8_t clockwise // Clockwise?
|
8162
|
8162
|
) {
|
8163
|
8163
|
|
8164
|
8164
|
float radius = HYPOT(offset[X_AXIS], offset[Y_AXIS]),
|
8165
|
8165
|
center_X = current_position[X_AXIS] + offset[X_AXIS],
|
8166
|
8166
|
center_Y = current_position[Y_AXIS] + offset[Y_AXIS],
|
8167
|
|
- linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
|
8168
|
|
- extruder_travel = target[E_AXIS] - current_position[E_AXIS],
|
|
8167
|
+ linear_travel = logical[Z_AXIS] - current_position[Z_AXIS],
|
|
8168
|
+ extruder_travel = logical[E_AXIS] - current_position[E_AXIS],
|
8169
|
8169
|
r_X = -offset[X_AXIS], // Radius vector from center to current location
|
8170
|
8170
|
r_Y = -offset[Y_AXIS],
|
8171
|
|
- rt_X = target[X_AXIS] - center_X,
|
8172
|
|
- rt_Y = target[Y_AXIS] - center_Y;
|
|
8171
|
+ rt_X = logical[X_AXIS] - center_X,
|
|
8172
|
+ rt_Y = logical[Y_AXIS] - center_Y;
|
8173
|
8173
|
|
8174
|
8174
|
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
|
8175
|
8175
|
float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y);
|
|
@@ -8177,7 +8177,7 @@ void prepare_move_to_destination() {
|
8177
|
8177
|
if (clockwise) angular_travel -= RADIANS(360);
|
8178
|
8178
|
|
8179
|
8179
|
// Make a circle if the angular rotation is 0
|
8180
|
|
- if (angular_travel == 0 && current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS])
|
|
8180
|
+ if (angular_travel == 0 && current_position[X_AXIS] == logical[X_AXIS] && current_position[Y_AXIS] == logical[Y_AXIS])
|
8181
|
8181
|
angular_travel += RADIANS(360);
|
8182
|
8182
|
|
8183
|
8183
|
float mm_of_travel = HYPOT(angular_travel * radius, fabs(linear_travel));
|
|
@@ -8282,13 +8282,13 @@ void prepare_move_to_destination() {
|
8282
|
8282
|
|
8283
|
8283
|
// Ensure last segment arrives at target location.
|
8284
|
8284
|
#if IS_KINEMATIC
|
8285
|
|
- inverse_kinematics(target);
|
|
8285
|
+ inverse_kinematics(logical);
|
8286
|
8286
|
#if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
8287
|
|
- adjust_delta(target);
|
|
8287
|
+ adjust_delta(logical);
|
8288
|
8288
|
#endif
|
8289
|
|
- planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder);
|
|
8289
|
+ planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
|
8290
|
8290
|
#else
|
8291
|
|
- planner.buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder);
|
|
8291
|
+ planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
|
8292
|
8292
|
#endif
|
8293
|
8293
|
|
8294
|
8294
|
// As far as the parser is concerned, the position is now == target. In reality the
|
|
@@ -8303,7 +8303,7 @@ void prepare_move_to_destination() {
|
8303
|
8303
|
void plan_cubic_move(const float offset[4]) {
|
8304
|
8304
|
cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
|
8305
|
8305
|
|
8306
|
|
- // As far as the parser is concerned, the position is now == target. In reality the
|
|
8306
|
+ // As far as the parser is concerned, the position is now == destination. In reality the
|
8307
|
8307
|
// motion control system might still be processing the action and the real tool position
|
8308
|
8308
|
// in any intermediate location.
|
8309
|
8309
|
set_current_to_destination();
|
|
@@ -8376,7 +8376,7 @@ void prepare_move_to_destination() {
|
8376
|
8376
|
//*/
|
8377
|
8377
|
}
|
8378
|
8378
|
|
8379
|
|
- void inverse_kinematics(const float cartesian[XYZ]) {
|
|
8379
|
+ void inverse_kinematics(const float logical[XYZ]) {
|
8380
|
8380
|
// Inverse kinematics.
|
8381
|
8381
|
// Perform SCARA IK and place results in delta[].
|
8382
|
8382
|
// The maths and first version were done by QHARLEY.
|
|
@@ -8384,8 +8384,8 @@ void prepare_move_to_destination() {
|
8384
|
8384
|
|
8385
|
8385
|
static float C2, S2, SK1, SK2, THETA, PSI;
|
8386
|
8386
|
|
8387
|
|
- float sx = RAW_X_POSITION(cartesian[X_AXIS]) - SCARA_OFFSET_X, //Translate SCARA to standard X Y
|
8388
|
|
- sy = RAW_Y_POSITION(cartesian[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor.
|
|
8387
|
+ float sx = RAW_X_POSITION(logical[X_AXIS]) - SCARA_OFFSET_X, // Translate SCARA to standard X Y
|
|
8388
|
+ sy = RAW_Y_POSITION(logical[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor.
|
8389
|
8389
|
|
8390
|
8390
|
#if (L1 == L2)
|
8391
|
8391
|
C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1;
|
|
@@ -8403,10 +8403,10 @@ void prepare_move_to_destination() {
|
8403
|
8403
|
|
8404
|
8404
|
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
|
8405
|
8405
|
delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
|
8406
|
|
- delta[Z_AXIS] = cartesian[Z_AXIS];
|
|
8406
|
+ delta[Z_AXIS] = logical[Z_AXIS];
|
8407
|
8407
|
|
8408
|
|
- /**
|
8409
|
|
- DEBUG_POS("SCARA IK", cartesian);
|
|
8408
|
+ /*
|
|
8409
|
+ DEBUG_POS("SCARA IK", logical);
|
8410
|
8410
|
DEBUG_POS("SCARA IK", delta);
|
8411
|
8411
|
SERIAL_ECHOPAIR(" SCARA (x,y) ", sx);
|
8412
|
8412
|
SERIAL_ECHOPAIR(",", sy);
|