|
@@ -331,15 +331,13 @@ float position_shift[3] = { 0 };
|
331
|
331
|
// Set by M206, M428, or menu item. Saved to EEPROM.
|
332
|
332
|
float home_offset[3] = { 0 };
|
333
|
333
|
|
|
334
|
+#define LOGICAL_POSITION(POS, AXIS) (POS + home_offset[AXIS] + position_shift[AXIS])
|
334
|
335
|
#define RAW_POSITION(POS, AXIS) (POS - home_offset[AXIS] - position_shift[AXIS])
|
335
|
336
|
#define RAW_CURRENT_POSITION(AXIS) (RAW_POSITION(current_position[AXIS], AXIS))
|
336
|
337
|
|
337
|
338
|
// Software Endstops. Default to configured limits.
|
338
|
339
|
float sw_endstop_min[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
|
339
|
340
|
float sw_endstop_max[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
|
340
|
|
-#if ENABLED(DELTA)
|
341
|
|
- float delta_clip_start_height = Z_MAX_POS;
|
342
|
|
-#endif
|
343
|
341
|
|
344
|
342
|
#if FAN_COUNT > 0
|
345
|
343
|
int fanSpeeds[FAN_COUNT] = { 0 };
|
|
@@ -463,6 +461,7 @@ static uint8_t target_extruder;
|
463
|
461
|
#define TOWER_3 Z_AXIS
|
464
|
462
|
|
465
|
463
|
float delta[3] = { 0 };
|
|
464
|
+ float cartesian_position[3] = { 0 };
|
466
|
465
|
#define SIN_60 0.8660254037844386
|
467
|
466
|
#define COS_60 0.5
|
468
|
467
|
float endstop_adj[3] = { 0 };
|
|
@@ -481,12 +480,13 @@ static uint8_t target_extruder;
|
481
|
480
|
float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1);
|
482
|
481
|
float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2);
|
483
|
482
|
float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3);
|
484
|
|
- //float delta_diagonal_rod_2 = sq(delta_diagonal_rod);
|
485
|
483
|
float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
|
|
484
|
+ float delta_clip_start_height = Z_MAX_POS;
|
486
|
485
|
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
487
|
486
|
int delta_grid_spacing[2] = { 0, 0 };
|
488
|
487
|
float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
|
489
|
488
|
#endif
|
|
489
|
+ float delta_safe_distance_from_top();
|
490
|
490
|
#else
|
491
|
491
|
static bool home_all_axis = true;
|
492
|
492
|
#endif
|
|
@@ -564,6 +564,7 @@ void stop();
|
564
|
564
|
void get_available_commands();
|
565
|
565
|
void process_next_command();
|
566
|
566
|
void prepare_move_to_destination();
|
|
567
|
+void set_current_from_steppers();
|
567
|
568
|
|
568
|
569
|
#if ENABLED(ARC_SUPPORT)
|
569
|
570
|
void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
|
|
@@ -614,7 +615,7 @@ static void report_current_position();
|
614
|
615
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
615
|
616
|
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);
|
616
|
617
|
#endif
|
617
|
|
- calculate_delta(current_position);
|
|
618
|
+ inverse_kinematics(current_position);
|
618
|
619
|
planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
|
619
|
620
|
}
|
620
|
621
|
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta()
|
|
@@ -1403,7 +1404,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
|
1403
|
1404
|
|
1404
|
1405
|
static float x_home_pos(int extruder) {
|
1405
|
1406
|
if (extruder == 0)
|
1406
|
|
- return base_home_pos(X_AXIS) + home_offset[X_AXIS];
|
|
1407
|
+ return LOGICAL_POSITION(base_home_pos(X_AXIS), X_AXIS);
|
1407
|
1408
|
else
|
1408
|
1409
|
/**
|
1409
|
1410
|
* In dual carriage mode the extruder offset provides an override of the
|
|
@@ -1438,7 +1439,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
|
1438
|
1439
|
* at the same positions relative to the machine.
|
1439
|
1440
|
*/
|
1440
|
1441
|
static void update_software_endstops(AxisEnum axis) {
|
1441
|
|
- float offs = home_offset[axis] + position_shift[axis];
|
|
1442
|
+ float offs = LOGICAL_POSITION(0, axis);
|
1442
|
1443
|
|
1443
|
1444
|
#if ENABLED(DUAL_X_CARRIAGE)
|
1444
|
1445
|
if (axis == X_AXIS) {
|
|
@@ -1509,7 +1510,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
|
1509
|
1510
|
if (active_extruder != 0)
|
1510
|
1511
|
current_position[X_AXIS] = x_home_pos(active_extruder);
|
1511
|
1512
|
else
|
1512
|
|
- current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
|
|
1513
|
+ current_position[X_AXIS] = LOGICAL_POSITION(base_home_pos(X_AXIS), X_AXIS);
|
1513
|
1514
|
update_software_endstops(X_AXIS);
|
1514
|
1515
|
return;
|
1515
|
1516
|
}
|
|
@@ -1520,7 +1521,8 @@ static void set_axis_is_at_home(AxisEnum axis) {
|
1520
|
1521
|
if (axis == X_AXIS || axis == Y_AXIS) {
|
1521
|
1522
|
|
1522
|
1523
|
float homeposition[3];
|
1523
|
|
- for (int i = 0; i < 3; i++) homeposition[i] = base_home_pos(i);
|
|
1524
|
+ for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
|
|
1525
|
+ homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
|
1524
|
1526
|
|
1525
|
1527
|
// SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
|
1526
|
1528
|
// SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
|
|
@@ -1529,24 +1531,13 @@ static void set_axis_is_at_home(AxisEnum axis) {
|
1529
|
1531
|
* Works out real Homeposition angles using inverse kinematics,
|
1530
|
1532
|
* and calculates homing offset using forward kinematics
|
1531
|
1533
|
*/
|
1532
|
|
- calculate_delta(homeposition);
|
1533
|
|
-
|
1534
|
|
- // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
|
1535
|
|
- // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
|
1536
|
|
-
|
1537
|
|
- for (int i = 0; i < 2; i++) delta[i] -= home_offset[i];
|
1538
|
|
-
|
1539
|
|
- // SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(home_offset[X_AXIS]);
|
1540
|
|
- // SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(home_offset[Y_AXIS]);
|
1541
|
|
- // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
|
1542
|
|
- // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
|
1543
|
|
-
|
1544
|
|
- calculate_SCARA_forward_Transform(delta);
|
|
1534
|
+ inverse_kinematics(homeposition);
|
|
1535
|
+ forward_kinematics_SCARA(delta);
|
1545
|
1536
|
|
1546
|
|
- // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]);
|
|
1537
|
+ // SERIAL_ECHOPAIR("Delta X=", delta[X_AXIS]);
|
1547
|
1538
|
// SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
|
1548
|
1539
|
|
1549
|
|
- current_position[axis] = delta[axis];
|
|
1540
|
+ current_position[axis] = LOGICAL_POSITION(delta[axis], axis);
|
1550
|
1541
|
|
1551
|
1542
|
/**
|
1552
|
1543
|
* SCARA home positions are based on configuration since the actual
|
|
@@ -1558,7 +1549,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
|
1558
|
1549
|
else
|
1559
|
1550
|
#endif
|
1560
|
1551
|
{
|
1561
|
|
- current_position[axis] = base_home_pos(axis) + home_offset[axis];
|
|
1552
|
+ current_position[axis] = LOGICAL_POSITION(base_home_pos(axis), axis);
|
1562
|
1553
|
update_software_endstops(axis);
|
1563
|
1554
|
|
1564
|
1555
|
#if HAS_BED_PROBE && Z_HOME_DIR < 0 && DISABLED(Z_MIN_PROBE_ENDSTOP)
|
|
@@ -1659,7 +1650,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
|
1659
|
1650
|
if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_move_to_destination_raw", destination);
|
1660
|
1651
|
#endif
|
1661
|
1652
|
refresh_cmd_timeout();
|
1662
|
|
- calculate_delta(destination);
|
|
1653
|
+ inverse_kinematics(destination);
|
1663
|
1654
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], MMM_TO_MMS_SCALED(feedrate_mm_m), active_extruder);
|
1664
|
1655
|
set_current_to_destination();
|
1665
|
1656
|
}
|
|
@@ -1787,7 +1778,7 @@ static void clean_up_after_endstop_or_probe_move() {
|
1787
|
1778
|
SERIAL_ECHOLNPGM(")");
|
1788
|
1779
|
}
|
1789
|
1780
|
#endif
|
1790
|
|
- float z_dest = home_offset[Z_AXIS] + z_raise;
|
|
1781
|
+ float z_dest = LOGICAL_POSITION(z_raise, Z_AXIS);
|
1791
|
1782
|
|
1792
|
1783
|
if (zprobe_zoffset < 0)
|
1793
|
1784
|
z_dest -= zprobe_zoffset;
|
|
@@ -2088,9 +2079,9 @@ static void clean_up_after_endstop_or_probe_move() {
|
2088
|
2079
|
}
|
2089
|
2080
|
|
2090
|
2081
|
#if ENABLED(DELTA)
|
2091
|
|
- #define Z_FROM_STEPPERS() z_before + stepper.get_axis_position_mm(Z_AXIS) - z_mm
|
|
2082
|
+ #define SET_Z_FROM_STEPPERS() set_current_from_steppers()
|
2092
|
2083
|
#else
|
2093
|
|
- #define Z_FROM_STEPPERS() stepper.get_axis_position_mm(Z_AXIS)
|
|
2084
|
+ #define SET_Z_FROM_STEPPERS() current_position[Z_AXIS] = LOGICAL_POSITION(stepper.get_axis_position_mm(Z_AXIS), Z_AXIS)
|
2094
|
2085
|
#endif
|
2095
|
2086
|
|
2096
|
2087
|
// Do a single Z probe and return with current_position[Z_AXIS]
|
|
@@ -2111,7 +2102,7 @@ static void clean_up_after_endstop_or_probe_move() {
|
2111
|
2102
|
|
2112
|
2103
|
do_blocking_move_to_z(-(Z_MAX_LENGTH + 10), Z_PROBE_SPEED_FAST);
|
2113
|
2104
|
endstops.hit_on_purpose();
|
2114
|
|
- current_position[Z_AXIS] = Z_FROM_STEPPERS();
|
|
2105
|
+ SET_Z_FROM_STEPPERS();
|
2115
|
2106
|
SYNC_PLAN_POSITION_KINEMATIC();
|
2116
|
2107
|
|
2117
|
2108
|
// move up the retract distance
|
|
@@ -2125,7 +2116,7 @@ static void clean_up_after_endstop_or_probe_move() {
|
2125
|
2116
|
// move back down slowly to find bed
|
2126
|
2117
|
do_blocking_move_to_z(current_position[Z_AXIS] - home_bump_mm(Z_AXIS) * 2, Z_PROBE_SPEED_SLOW);
|
2127
|
2118
|
endstops.hit_on_purpose();
|
2128
|
|
- current_position[Z_AXIS] = Z_FROM_STEPPERS();
|
|
2119
|
+ SET_Z_FROM_STEPPERS();
|
2129
|
2120
|
SYNC_PLAN_POSITION_KINEMATIC();
|
2130
|
2121
|
|
2131
|
2122
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
@@ -2959,7 +2950,7 @@ inline void gcode_G28() {
|
2959
|
2950
|
|
2960
|
2951
|
if (home_all_axis || homeX || homeY) {
|
2961
|
2952
|
// Raise Z before homing any other axes and z is not already high enough (never lower z)
|
2962
|
|
- destination[Z_AXIS] = home_offset[Z_AXIS] + MIN_Z_HEIGHT_FOR_HOMING;
|
|
2953
|
+ destination[Z_AXIS] = LOGICAL_POSITION(MIN_Z_HEIGHT_FOR_HOMING, Z_AXIS);
|
2963
|
2954
|
if (destination[Z_AXIS] > current_position[Z_AXIS]) {
|
2964
|
2955
|
|
2965
|
2956
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
@@ -3214,12 +3205,12 @@ inline void gcode_G28() {
|
3214
|
3205
|
;
|
3215
|
3206
|
line_to_current_position();
|
3216
|
3207
|
|
3217
|
|
- current_position[X_AXIS] = x + home_offset[X_AXIS];
|
3218
|
|
- current_position[Y_AXIS] = y + home_offset[Y_AXIS];
|
|
3208
|
+ current_position[X_AXIS] = LOGICAL_POSITION(x, X_AXIS);
|
|
3209
|
+ current_position[Y_AXIS] = LOGICAL_POSITION(y, Y_AXIS);
|
3219
|
3210
|
line_to_current_position();
|
3220
|
3211
|
|
3221
|
3212
|
#if Z_RAISE_BETWEEN_PROBINGS > 0 || MIN_Z_HEIGHT_FOR_HOMING > 0
|
3222
|
|
- current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
|
|
3213
|
+ current_position[Z_AXIS] = LOGICAL_POSITION(MESH_HOME_SEARCH_Z, Z_AXIS);
|
3223
|
3214
|
line_to_current_position();
|
3224
|
3215
|
#endif
|
3225
|
3216
|
|
|
@@ -3637,14 +3628,14 @@ inline void gcode_G28() {
|
3637
|
3628
|
#endif
|
3638
|
3629
|
|
3639
|
3630
|
// Probe at 3 arbitrary points
|
3640
|
|
- float z_at_pt_1 = probe_pt( ABL_PROBE_PT_1_X + home_offset[X_AXIS],
|
3641
|
|
- ABL_PROBE_PT_1_Y + home_offset[Y_AXIS],
|
|
3631
|
+ float z_at_pt_1 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_1_X, X_AXIS),
|
|
3632
|
+ LOGICAL_POSITION(ABL_PROBE_PT_1_Y, Y_AXIS),
|
3642
|
3633
|
stow_probe_after_each, verbose_level),
|
3643
|
|
- z_at_pt_2 = probe_pt( ABL_PROBE_PT_2_X + home_offset[X_AXIS],
|
3644
|
|
- ABL_PROBE_PT_2_Y + home_offset[Y_AXIS],
|
|
3634
|
+ z_at_pt_2 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_2_X, X_AXIS),
|
|
3635
|
+ LOGICAL_POSITION(ABL_PROBE_PT_2_Y, Y_AXIS),
|
3645
|
3636
|
stow_probe_after_each, verbose_level),
|
3646
|
|
- z_at_pt_3 = probe_pt( ABL_PROBE_PT_3_X + home_offset[X_AXIS],
|
3647
|
|
- ABL_PROBE_PT_3_Y + home_offset[Y_AXIS],
|
|
3637
|
+ z_at_pt_3 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_3_X, X_AXIS),
|
|
3638
|
+ LOGICAL_POSITION(ABL_PROBE_PT_3_Y, Y_AXIS),
|
3648
|
3639
|
stow_probe_after_each, verbose_level);
|
3649
|
3640
|
|
3650
|
3641
|
if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
|
@@ -5168,9 +5159,9 @@ static void report_current_position() {
|
5168
|
5159
|
SERIAL_EOL;
|
5169
|
5160
|
|
5170
|
5161
|
SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
|
5171
|
|
- SERIAL_PROTOCOL(delta[X_AXIS] + home_offset[X_AXIS]);
|
|
5162
|
+ SERIAL_PROTOCOL(delta[X_AXIS]);
|
5172
|
5163
|
SERIAL_PROTOCOLPGM(" Psi+Theta (90):");
|
5173
|
|
- SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90 + home_offset[Y_AXIS]);
|
|
5164
|
+ SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90);
|
5174
|
5165
|
SERIAL_EOL;
|
5175
|
5166
|
|
5176
|
5167
|
SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
|
|
@@ -5880,7 +5871,7 @@ inline void gcode_M303() {
|
5880
|
5871
|
//gcode_get_destination(); // For X Y Z E F
|
5881
|
5872
|
delta[X_AXIS] = delta_x;
|
5882
|
5873
|
delta[Y_AXIS] = delta_y;
|
5883
|
|
- calculate_SCARA_forward_Transform(delta);
|
|
5874
|
+ forward_kinematics_SCARA(delta);
|
5884
|
5875
|
destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
|
5885
|
5876
|
destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
|
5886
|
5877
|
prepare_move_to_destination();
|
|
@@ -6068,18 +6059,9 @@ inline void gcode_M400() { stepper.synchronize(); }
|
6068
|
6059
|
|
6069
|
6060
|
void quickstop_stepper() {
|
6070
|
6061
|
stepper.quick_stop();
|
6071
|
|
- #if DISABLED(DELTA) && DISABLED(SCARA)
|
|
6062
|
+ #if DISABLED(SCARA)
|
6072
|
6063
|
stepper.synchronize();
|
6073
|
|
- #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
6074
|
|
- vector_3 pos = planner.adjusted_position(); // values directly from steppers...
|
6075
|
|
- current_position[X_AXIS] = pos.x;
|
6076
|
|
- current_position[Y_AXIS] = pos.y;
|
6077
|
|
- current_position[Z_AXIS] = pos.z;
|
6078
|
|
- #else
|
6079
|
|
- current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
|
6080
|
|
- current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
|
6081
|
|
- current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
6082
|
|
- #endif
|
|
6064
|
+ set_current_from_steppers();
|
6083
|
6065
|
sync_plan_position(); // ...re-apply to planner position
|
6084
|
6066
|
#endif
|
6085
|
6067
|
}
|
|
@@ -6146,7 +6128,7 @@ inline void gcode_M428() {
|
6146
|
6128
|
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
|
6147
|
6129
|
if (axis_homed[i]) {
|
6148
|
6130
|
float base = (current_position[i] > (sw_endstop_min[i] + sw_endstop_max[i]) / 2) ? base_home_pos(i) : 0,
|
6149
|
|
- diff = current_position[i] - base;
|
|
6131
|
+ diff = current_position[i] - LOGICAL_POSITION(base, i);
|
6150
|
6132
|
if (diff > -20 && diff < 20) {
|
6151
|
6133
|
set_home_offset((AxisEnum)i, home_offset[i] - diff);
|
6152
|
6134
|
}
|
|
@@ -6278,7 +6260,7 @@ inline void gcode_M503() {
|
6278
|
6260
|
|
6279
|
6261
|
// Define runplan for move axes
|
6280
|
6262
|
#if ENABLED(DELTA)
|
6281
|
|
- #define RUNPLAN(RATE_MM_S) calculate_delta(destination); \
|
|
6263
|
+ #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \
|
6282
|
6264
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
|
6283
|
6265
|
#else
|
6284
|
6266
|
#define RUNPLAN(RATE_MM_S) line_to_destination(MMS_TO_MMM(RATE_MM_S));
|
|
@@ -6400,7 +6382,7 @@ inline void gcode_M503() {
|
6400
|
6382
|
|
6401
|
6383
|
#if ENABLED(DELTA)
|
6402
|
6384
|
// Move XYZ to starting position, then E
|
6403
|
|
- calculate_delta(lastpos);
|
|
6385
|
+ inverse_kinematics(lastpos);
|
6404
|
6386
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
|
6405
|
6387
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
|
6406
|
6388
|
#else
|
|
@@ -7740,7 +7722,13 @@ void clamp_to_software_endstops(float target[3]) {
|
7740
|
7722
|
delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
|
7741
|
7723
|
}
|
7742
|
7724
|
|
7743
|
|
- void calculate_delta(float cartesian[3]) {
|
|
7725
|
+ void inverse_kinematics(const float in_cartesian[3]) {
|
|
7726
|
+
|
|
7727
|
+ const float cartesian[3] = {
|
|
7728
|
+ RAW_POSITION(in_cartesian[X_AXIS], X_AXIS),
|
|
7729
|
+ RAW_POSITION(in_cartesian[Y_AXIS], Y_AXIS),
|
|
7730
|
+ RAW_POSITION(in_cartesian[Z_AXIS], Z_AXIS)
|
|
7731
|
+ };
|
7744
|
7732
|
|
7745
|
7733
|
delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
|
7746
|
7734
|
- sq(delta_tower1_x - cartesian[X_AXIS])
|
|
@@ -7766,14 +7754,97 @@ void clamp_to_software_endstops(float target[3]) {
|
7766
|
7754
|
}
|
7767
|
7755
|
|
7768
|
7756
|
float delta_safe_distance_from_top() {
|
7769
|
|
- float cartesian[3] = { 0 };
|
7770
|
|
- calculate_delta(cartesian);
|
|
7757
|
+ float cartesian[3] = {
|
|
7758
|
+ LOGICAL_POSITION(0, X_AXIS),
|
|
7759
|
+ LOGICAL_POSITION(0, Y_AXIS),
|
|
7760
|
+ LOGICAL_POSITION(0, Z_AXIS)
|
|
7761
|
+ };
|
|
7762
|
+ inverse_kinematics(cartesian);
|
7771
|
7763
|
float distance = delta[TOWER_3];
|
7772
|
|
- cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
|
7773
|
|
- calculate_delta(cartesian);
|
|
7764
|
+ cartesian[Y_AXIS] = LOGICAL_POSITION(DELTA_PRINTABLE_RADIUS, Y_AXIS);
|
|
7765
|
+ inverse_kinematics(cartesian);
|
7774
|
7766
|
return abs(distance - delta[TOWER_3]);
|
7775
|
7767
|
}
|
7776
|
7768
|
|
|
7769
|
+ void forward_kinematics_DELTA(float z1, float z2, float z3) {
|
|
7770
|
+ //As discussed in Wikipedia "Trilateration"
|
|
7771
|
+ //we are establishing a new coordinate
|
|
7772
|
+ //system in the plane of the three carriage points.
|
|
7773
|
+ //This system will have the origin at tower1 and
|
|
7774
|
+ //tower2 is on the x axis. tower3 is in the X-Y
|
|
7775
|
+ //plane with a Z component of zero. We will define unit
|
|
7776
|
+ //vectors in this coordinate system in our original
|
|
7777
|
+ //coordinate system. Then when we calculate the
|
|
7778
|
+ //Xnew, Ynew and Znew values, we can translate back into
|
|
7779
|
+ //the original system by moving along those unit vectors
|
|
7780
|
+ //by the corresponding values.
|
|
7781
|
+ // https://en.wikipedia.org/wiki/Trilateration
|
|
7782
|
+
|
|
7783
|
+ // Variable names matched to Marlin, c-version
|
|
7784
|
+ // and avoiding a vector library
|
|
7785
|
+ // by Andreas Hardtung 2016-06-7
|
|
7786
|
+ // based on a Java function from
|
|
7787
|
+ // "Delta Robot Kinematics by Steve Graves" V3
|
|
7788
|
+
|
|
7789
|
+ // Result is in cartesian_position[].
|
|
7790
|
+
|
|
7791
|
+ //Create a vector in old coordinates along x axis of new coordinate
|
|
7792
|
+ float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
|
|
7793
|
+
|
|
7794
|
+ //Get the Magnitude of vector.
|
|
7795
|
+ float d = sqrt( p12[0]*p12[0] + p12[1]*p12[1] + p12[2]*p12[2] );
|
|
7796
|
+
|
|
7797
|
+ //Create unit vector by dividing by magnitude.
|
|
7798
|
+ float ex[3] = { p12[0]/d, p12[1]/d, p12[2]/d };
|
|
7799
|
+
|
|
7800
|
+ //Now find vector from the origin of the new system to the third point.
|
|
7801
|
+ float p13[3] = { delta_tower3_x - delta_tower1_x, delta_tower3_y - delta_tower1_y, z3 - z1 };
|
|
7802
|
+
|
|
7803
|
+ //Now use dot product to find the component of this vector on the X axis.
|
|
7804
|
+ float i = ex[0]*p13[0] + ex[1]*p13[1] + ex[2]*p13[2];
|
|
7805
|
+
|
|
7806
|
+ //Now create a vector along the x axis that represents the x component of p13.
|
|
7807
|
+ float iex[3] = { ex[0]*i, ex[1]*i, ex[2]*i };
|
|
7808
|
+
|
|
7809
|
+ //Now subtract the X component away from the original vector leaving only the Y component. We use the
|
|
7810
|
+ //variable that will be the unit vector after we scale it.
|
|
7811
|
+ float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2]};
|
|
7812
|
+
|
|
7813
|
+ //The magnitude of Y component
|
|
7814
|
+ float j = sqrt(sq(ey[0]) + sq(ey[1]) + sq(ey[2]));
|
|
7815
|
+
|
|
7816
|
+ //Now make vector a unit vector
|
|
7817
|
+ ey[0] /= j; ey[1] /= j; ey[2] /= j;
|
|
7818
|
+
|
|
7819
|
+ //The cross product of the unit x and y is the unit z
|
|
7820
|
+ //float[] ez = vectorCrossProd(ex, ey);
|
|
7821
|
+ float ez[3] = { ex[1]*ey[2] - ex[2]*ey[1], ex[2]*ey[0] - ex[0]*ey[2], ex[0]*ey[1] - ex[1]*ey[0] };
|
|
7822
|
+
|
|
7823
|
+ //Now we have the d, i and j values defined in Wikipedia.
|
|
7824
|
+ //We can plug them into the equations defined in
|
|
7825
|
+ //Wikipedia for Xnew, Ynew and Znew
|
|
7826
|
+ float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + d*d)/(d*2);
|
|
7827
|
+ float Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + i*i + j*j)/2 - i*Xnew) /j;
|
|
7828
|
+ float Znew = sqrt(delta_diagonal_rod_2_tower_1 - Xnew*Xnew - Ynew*Ynew);
|
|
7829
|
+
|
|
7830
|
+ //Now we can start from the origin in the old coords and
|
|
7831
|
+ //add vectors in the old coords that represent the
|
|
7832
|
+ //Xnew, Ynew and Znew to find the point in the old system
|
|
7833
|
+ cartesian_position[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
|
|
7834
|
+ cartesian_position[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
|
|
7835
|
+ cartesian_position[Z_AXIS] = z1 + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
|
|
7836
|
+ };
|
|
7837
|
+
|
|
7838
|
+ void forward_kinematics_DELTA(float point[3]) {
|
|
7839
|
+ forward_kinematics_DELTA(point[X_AXIS], point[Y_AXIS], point[Z_AXIS]);
|
|
7840
|
+ }
|
|
7841
|
+
|
|
7842
|
+ void set_cartesian_from_steppers() {
|
|
7843
|
+ forward_kinematics_DELTA(stepper.get_axis_position_mm(X_AXIS),
|
|
7844
|
+ stepper.get_axis_position_mm(Y_AXIS),
|
|
7845
|
+ stepper.get_axis_position_mm(Z_AXIS));
|
|
7846
|
+ }
|
|
7847
|
+
|
7777
|
7848
|
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
7778
|
7849
|
|
7779
|
7850
|
// Adjust print surface height by linear interpolation over the bed_level array.
|
|
@@ -7782,8 +7853,8 @@ void clamp_to_software_endstops(float target[3]) {
|
7782
|
7853
|
|
7783
|
7854
|
int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
|
7784
|
7855
|
float h1 = 0.001 - half, h2 = half - 0.001,
|
7785
|
|
- grid_x = max(h1, min(h2, cartesian[X_AXIS] / delta_grid_spacing[0])),
|
7786
|
|
- grid_y = max(h1, min(h2, cartesian[Y_AXIS] / delta_grid_spacing[1]));
|
|
7856
|
+ grid_x = max(h1, min(h2, RAW_POSITION(cartesian[X_AXIS], X_AXIS) / delta_grid_spacing[0])),
|
|
7857
|
+ grid_y = max(h1, min(h2, RAW_POSITION(cartesian[Y_AXIS], Y_AXIS) / delta_grid_spacing[1]));
|
7787
|
7858
|
int floor_x = floor(grid_x), floor_y = floor(grid_y);
|
7788
|
7859
|
float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
|
7789
|
7860
|
z1 = bed_level[floor_x + half][floor_y + half],
|
|
@@ -7818,6 +7889,27 @@ void clamp_to_software_endstops(float target[3]) {
|
7818
|
7889
|
|
7819
|
7890
|
#endif // DELTA
|
7820
|
7891
|
|
|
7892
|
+void set_current_from_steppers() {
|
|
7893
|
+ #if ENABLED(DELTA)
|
|
7894
|
+ set_cartesian_from_steppers();
|
|
7895
|
+ current_position[X_AXIS] = cartesian_position[X_AXIS];
|
|
7896
|
+ current_position[Y_AXIS] = cartesian_position[Y_AXIS];
|
|
7897
|
+ current_position[Z_AXIS] = cartesian_position[Z_AXIS];
|
|
7898
|
+ #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
7899
|
+ vector_3 pos = planner.adjusted_position(); // values directly from steppers...
|
|
7900
|
+ current_position[X_AXIS] = pos.x;
|
|
7901
|
+ current_position[Y_AXIS] = pos.y;
|
|
7902
|
+ current_position[Z_AXIS] = pos.z;
|
|
7903
|
+ #else
|
|
7904
|
+ current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
|
|
7905
|
+ current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
|
|
7906
|
+ current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
|
7907
|
+ #endif
|
|
7908
|
+
|
|
7909
|
+ for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
|
|
7910
|
+ current_position[i] += LOGICAL_POSITION(0, i);
|
|
7911
|
+}
|
|
7912
|
+
|
7821
|
7913
|
#if ENABLED(MESH_BED_LEVELING)
|
7822
|
7914
|
|
7823
|
7915
|
// This function is used to split lines on mesh borders so each segment is only part of one mesh area
|
|
@@ -7846,14 +7938,14 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
|
7846
|
7938
|
int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
|
7847
|
7939
|
if (cx2 != cx1 && TEST(x_splits, gcx)) {
|
7848
|
7940
|
memcpy(end, destination, sizeof(end));
|
7849
|
|
- destination[X_AXIS] = mbl.get_probe_x(gcx) + home_offset[X_AXIS] + position_shift[X_AXIS];
|
|
7941
|
+ destination[X_AXIS] = LOGICAL_POSITION(mbl.get_probe_x(gcx), X_AXIS);
|
7850
|
7942
|
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
|
7851
|
7943
|
destination[Y_AXIS] = MBL_SEGMENT_END(Y);
|
7852
|
7944
|
CBI(x_splits, gcx);
|
7853
|
7945
|
}
|
7854
|
7946
|
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
|
7855
|
7947
|
memcpy(end, destination, sizeof(end));
|
7856
|
|
- destination[Y_AXIS] = mbl.get_probe_y(gcy) + home_offset[Y_AXIS] + position_shift[Y_AXIS];
|
|
7948
|
+ destination[Y_AXIS] = LOGICAL_POSITION(mbl.get_probe_y(gcy), Y_AXIS);
|
7857
|
7949
|
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
|
7858
|
7950
|
destination[X_AXIS] = MBL_SEGMENT_END(X);
|
7859
|
7951
|
CBI(y_splits, gcy);
|
|
@@ -7879,7 +7971,7 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
|
7879
|
7971
|
|
7880
|
7972
|
#if ENABLED(DELTA) || ENABLED(SCARA)
|
7881
|
7973
|
|
7882
|
|
- inline bool prepare_delta_move_to(float target[NUM_AXIS]) {
|
|
7974
|
+ inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
|
7883
|
7975
|
float difference[NUM_AXIS];
|
7884
|
7976
|
for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
|
7885
|
7977
|
|
|
@@ -7902,14 +7994,14 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
|
7902
|
7994
|
for (int8_t i = 0; i < NUM_AXIS; i++)
|
7903
|
7995
|
target[i] = current_position[i] + difference[i] * fraction;
|
7904
|
7996
|
|
7905
|
|
- calculate_delta(target);
|
|
7997
|
+ inverse_kinematics(target);
|
7906
|
7998
|
|
7907
|
|
- #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
7999
|
+ #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
|
7908
|
8000
|
if (!bed_leveling_in_progress) adjust_delta(target);
|
7909
|
8001
|
#endif
|
7910
|
8002
|
|
7911
|
|
- //DEBUG_POS("prepare_delta_move_to", target);
|
7912
|
|
- //DEBUG_POS("prepare_delta_move_to", delta);
|
|
8003
|
+ //DEBUG_POS("prepare_kinematic_move_to", target);
|
|
8004
|
+ //DEBUG_POS("prepare_kinematic_move_to", delta);
|
7913
|
8005
|
|
7914
|
8006
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder);
|
7915
|
8007
|
}
|
|
@@ -7918,10 +8010,6 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
|
7918
|
8010
|
|
7919
|
8011
|
#endif // DELTA || SCARA
|
7920
|
8012
|
|
7921
|
|
-#if ENABLED(SCARA)
|
7922
|
|
- inline bool prepare_scara_move_to(float target[NUM_AXIS]) { return prepare_delta_move_to(target); }
|
7923
|
|
-#endif
|
7924
|
|
-
|
7925
|
8013
|
#if ENABLED(DUAL_X_CARRIAGE)
|
7926
|
8014
|
|
7927
|
8015
|
inline bool prepare_move_to_destination_dualx() {
|
|
@@ -8020,10 +8108,8 @@ void prepare_move_to_destination() {
|
8020
|
8108
|
prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
|
8021
|
8109
|
#endif
|
8022
|
8110
|
|
8023
|
|
- #if ENABLED(SCARA)
|
8024
|
|
- if (!prepare_scara_move_to(destination)) return;
|
8025
|
|
- #elif ENABLED(DELTA)
|
8026
|
|
- if (!prepare_delta_move_to(destination)) return;
|
|
8111
|
+ #if ENABLED(DELTA) || ENABLED(SCARA)
|
|
8112
|
+ if (!prepare_kinematic_move_to(destination)) return;
|
8027
|
8113
|
#else
|
8028
|
8114
|
#if ENABLED(DUAL_X_CARRIAGE)
|
8029
|
8115
|
if (!prepare_move_to_destination_dualx()) return;
|
|
@@ -8159,8 +8245,8 @@ void prepare_move_to_destination() {
|
8159
|
8245
|
clamp_to_software_endstops(arc_target);
|
8160
|
8246
|
|
8161
|
8247
|
#if ENABLED(DELTA) || ENABLED(SCARA)
|
8162
|
|
- calculate_delta(arc_target);
|
8163
|
|
- #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
8248
|
+ inverse_kinematics(arc_target);
|
|
8249
|
+ #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
|
8164
|
8250
|
adjust_delta(arc_target);
|
8165
|
8251
|
#endif
|
8166
|
8252
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
|
|
@@ -8171,8 +8257,8 @@ void prepare_move_to_destination() {
|
8171
|
8257
|
|
8172
|
8258
|
// Ensure last segment arrives at target location.
|
8173
|
8259
|
#if ENABLED(DELTA) || ENABLED(SCARA)
|
8174
|
|
- calculate_delta(target);
|
8175
|
|
- #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
8260
|
+ inverse_kinematics(target);
|
|
8261
|
+ #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
|
8176
|
8262
|
adjust_delta(target);
|
8177
|
8263
|
#endif
|
8178
|
8264
|
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder);
|
|
@@ -8239,7 +8325,7 @@ void prepare_move_to_destination() {
|
8239
|
8325
|
|
8240
|
8326
|
#if ENABLED(SCARA)
|
8241
|
8327
|
|
8242
|
|
- void calculate_SCARA_forward_Transform(float f_scara[3]) {
|
|
8328
|
+ void forward_kinematics_SCARA(float f_scara[3]) {
|
8243
|
8329
|
// Perform forward kinematics, and place results in delta[3]
|
8244
|
8330
|
// The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
|
8245
|
8331
|
|
|
@@ -8265,16 +8351,17 @@ void prepare_move_to_destination() {
|
8265
|
8351
|
//SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
|
8266
|
8352
|
}
|
8267
|
8353
|
|
8268
|
|
- void calculate_delta(float cartesian[3]) {
|
8269
|
|
- //reverse kinematics.
|
8270
|
|
- // Perform reversed kinematics, and place results in delta[3]
|
8271
|
|
- // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
|
|
8354
|
+ void inverse_kinematics(const float cartesian[3]) {
|
|
8355
|
+ // Inverse kinematics.
|
|
8356
|
+ // Perform SCARA IK and place results in delta[3].
|
|
8357
|
+ // The maths and first version were done by QHARLEY.
|
|
8358
|
+ // Integrated, tweaked by Joachim Cerny in June 2014.
|
8272
|
8359
|
|
8273
|
8360
|
float SCARA_pos[2];
|
8274
|
8361
|
static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi;
|
8275
|
8362
|
|
8276
|
|
- SCARA_pos[X_AXIS] = cartesian[X_AXIS] * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y
|
8277
|
|
- SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor.
|
|
8363
|
+ SCARA_pos[X_AXIS] = RAW_POSITION(cartesian[X_AXIS], X_AXIS) * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y
|
|
8364
|
+ SCARA_pos[Y_AXIS] = RAW_POSITION(cartesian[Y_AXIS], Y_AXIS) * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor.
|
8278
|
8365
|
|
8279
|
8366
|
#if (Linkage_1 == Linkage_2)
|
8280
|
8367
|
SCARA_C2 = ((sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS])) / (2 * (float)L1_2)) - 1;
|
|
@@ -8292,7 +8379,7 @@ void prepare_move_to_destination() {
|
8292
|
8379
|
|
8293
|
8380
|
delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - theta is support arm angle
|
8294
|
8381
|
delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG; // - equal to sub arm angle (inverted motor)
|
8295
|
|
- delta[Z_AXIS] = cartesian[Z_AXIS];
|
|
8382
|
+ delta[Z_AXIS] = RAW_POSITION(cartesian[Z_AXIS], Z_AXIS);
|
8296
|
8383
|
|
8297
|
8384
|
/**
|
8298
|
8385
|
SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
|