|
@@ -1266,13 +1266,14 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
|
1266
|
1266
|
|
1267
|
1267
|
// Engage Z Servo endstop if enabled
|
1268
|
1268
|
if (servo_endstops[Z_AXIS] >= 0) {
|
|
1269
|
+ Servo *srv = &servo[servo_endstops[Z_AXIS]];
|
1269
|
1270
|
#if SERVO_LEVELING
|
1270
|
|
- servo[servo_endstops[Z_AXIS]].attach(0);
|
|
1271
|
+ srv->attach(0);
|
1271
|
1272
|
#endif
|
1272
|
|
- servo[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
|
|
1273
|
+ srv->write(servo_endstop_angles[Z_AXIS * 2]);
|
1273
|
1274
|
#if SERVO_LEVELING
|
1274
|
1275
|
delay(PROBE_SERVO_DEACTIVATION_DELAY);
|
1275
|
|
- servo[servo_endstops[Z_AXIS]].detach();
|
|
1276
|
+ srv->detach();
|
1276
|
1277
|
#endif
|
1277
|
1278
|
}
|
1278
|
1279
|
|
|
@@ -1318,7 +1319,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
|
1318
|
1319
|
|
1319
|
1320
|
}
|
1320
|
1321
|
|
1321
|
|
- static void stow_z_probe() {
|
|
1322
|
+ static void stow_z_probe(bool doRaise=true) {
|
1322
|
1323
|
|
1323
|
1324
|
#ifdef SERVO_ENDSTOPS
|
1324
|
1325
|
|
|
@@ -1326,19 +1327,21 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
|
1326
|
1327
|
if (servo_endstops[Z_AXIS] >= 0) {
|
1327
|
1328
|
|
1328
|
1329
|
#if Z_RAISE_AFTER_PROBING > 0
|
1329
|
|
- do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // this also updates current_position
|
1330
|
|
- st_synchronize();
|
|
1330
|
+ if (doRaise) {
|
|
1331
|
+ do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // this also updates current_position
|
|
1332
|
+ st_synchronize();
|
|
1333
|
+ }
|
1331
|
1334
|
#endif
|
1332
|
1335
|
|
|
1336
|
+ // Change the Z servo angle
|
|
1337
|
+ Servo *srv = &servo[servo_endstops[Z_AXIS]];
|
1333
|
1338
|
#if SERVO_LEVELING
|
1334
|
|
- servo[servo_endstops[Z_AXIS]].attach(0);
|
|
1339
|
+ srv->attach(0);
|
1335
|
1340
|
#endif
|
1336
|
|
-
|
1337
|
|
- servo[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
|
1338
|
|
-
|
|
1341
|
+ srv->write(servo_endstop_angles[Z_AXIS * 2 + 1]);
|
1339
|
1342
|
#if SERVO_LEVELING
|
1340
|
1343
|
delay(PROBE_SERVO_DEACTIVATION_DELAY);
|
1341
|
|
- servo[servo_endstops[Z_AXIS]].detach();
|
|
1344
|
+ srv->detach();
|
1342
|
1345
|
#endif
|
1343
|
1346
|
}
|
1344
|
1347
|
|
|
@@ -1522,19 +1525,20 @@ static void homeaxis(AxisEnum axis) {
|
1522
|
1525
|
current_position[axis] = 0;
|
1523
|
1526
|
sync_plan_position();
|
1524
|
1527
|
|
1525
|
|
- // Engage Servo endstop if enabled
|
1526
|
|
- #if defined(SERVO_ENDSTOPS) && !defined(Z_PROBE_SLED)
|
|
1528
|
+ #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
|
1527
|
1529
|
|
1528
|
|
- #if SERVO_LEVELING
|
1529
|
|
- if (axis == Z_AXIS) deploy_z_probe(); else
|
1530
|
|
- #endif
|
1531
|
|
- {
|
1532
|
|
- if (servo_endstops[axis] > -1)
|
1533
|
|
- servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
|
1534
|
|
- }
|
|
1530
|
+ // Deploy a probe if there is one, and homing towards the bed
|
|
1531
|
+ if (axis == Z_AXIS && axis_home_dir < 0) deploy_z_probe();
|
|
1532
|
+
|
|
1533
|
+ #elif defined(SERVO_ENDSTOPS)
|
1535
|
1534
|
|
1536
|
|
- #endif // SERVO_ENDSTOPS && !Z_PROBE_SLED
|
|
1535
|
+ // Engage Servo endstop if enabled
|
|
1536
|
+ if (servo_endstops[axis] > -1)
|
|
1537
|
+ servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
|
|
1538
|
+
|
|
1539
|
+ #endif
|
1537
|
1540
|
|
|
1541
|
+ // Set a flag for Z motor locking
|
1538
|
1542
|
#ifdef Z_DUAL_ENDSTOPS
|
1539
|
1543
|
if (axis == Z_AXIS) In_Homing_Process(true);
|
1540
|
1544
|
#endif
|
|
@@ -1612,14 +1616,17 @@ static void homeaxis(AxisEnum axis) {
|
1612
|
1616
|
endstops_hit_on_purpose(); // clear endstop hit flags
|
1613
|
1617
|
axis_known_position[axis] = true;
|
1614
|
1618
|
|
1615
|
|
- // Retract Servo endstop if enabled
|
1616
|
|
- #ifdef SERVO_ENDSTOPS
|
|
1619
|
+ #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
|
|
1620
|
+
|
|
1621
|
+ // Stow the Z probe if it had been deployed
|
|
1622
|
+ if (axis == Z_AXIS && axis_home_dir < 0) stow_z_probe();
|
|
1623
|
+
|
|
1624
|
+ #elif defined(SERVO_ENDSTOPS)
|
|
1625
|
+
|
|
1626
|
+ // Retract Servo endstop if enabled
|
1617
|
1627
|
if (servo_endstops[axis] > -1)
|
1618
|
1628
|
servo[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
|
1619
|
|
- #endif
|
1620
|
1629
|
|
1621
|
|
- #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
|
1622
|
|
- if (axis == Z_AXIS) stow_z_probe();
|
1623
|
1630
|
#endif
|
1624
|
1631
|
|
1625
|
1632
|
}
|
|
@@ -2996,7 +3003,7 @@ inline void gcode_M42() {
|
2996
|
3003
|
current_position[E_AXIS] = E_current = st_get_position_mm(E_AXIS);
|
2997
|
3004
|
|
2998
|
3005
|
//
|
2999
|
|
- // OK, do the inital probe to get us close to the bed.
|
|
3006
|
+ // OK, do the initial probe to get us close to the bed.
|
3000
|
3007
|
// Then retrace the right amount and use that in subsequent probes
|
3001
|
3008
|
//
|
3002
|
3009
|
|
|
@@ -3105,12 +3112,14 @@ inline void gcode_M42() {
|
3105
|
3112
|
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
|
3106
|
3113
|
st_synchronize();
|
3107
|
3114
|
|
|
3115
|
+ // Stow between
|
3108
|
3116
|
if (deploy_probe_for_each_reading) {
|
3109
|
3117
|
stow_z_probe();
|
3110
|
3118
|
delay(1000);
|
3111
|
3119
|
}
|
3112
|
3120
|
}
|
3113
|
3121
|
|
|
3122
|
+ // Stow after
|
3114
|
3123
|
if (!deploy_probe_for_each_reading) {
|
3115
|
3124
|
stow_z_probe();
|
3116
|
3125
|
delay(1000);
|
|
@@ -4062,13 +4071,14 @@ inline void gcode_M226() {
|
4062
|
4071
|
if (code_seen('S')) {
|
4063
|
4072
|
servo_position = code_value();
|
4064
|
4073
|
if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
|
|
4074
|
+ Servo *srv = &servo[servo_index];
|
4065
|
4075
|
#if SERVO_LEVELING
|
4066
|
|
- servo[servo_index].attach(0);
|
|
4076
|
+ srv->attach(0);
|
4067
|
4077
|
#endif
|
4068
|
|
- servo[servo_index].write(servo_position);
|
|
4078
|
+ srv->write(servo_position);
|
4069
|
4079
|
#if SERVO_LEVELING
|
4070
|
4080
|
delay(PROBE_SERVO_DEACTIVATION_DELAY);
|
4071
|
|
- servo[servo_index].detach();
|
|
4081
|
+ srv->detach();
|
4072
|
4082
|
#endif
|
4073
|
4083
|
}
|
4074
|
4084
|
else {
|
|
@@ -4372,12 +4382,12 @@ inline void gcode_M303() {
|
4372
|
4382
|
*/
|
4373
|
4383
|
inline void gcode_M400() { st_synchronize(); }
|
4374
|
4384
|
|
4375
|
|
-#if defined(ENABLE_AUTO_BED_LEVELING) && (defined(SERVO_ENDSTOPS) || defined(Z_PROBE_ALLEN_KEY)) && not defined(Z_PROBE_SLED)
|
|
4385
|
+#if defined(ENABLE_AUTO_BED_LEVELING) && !defined(Z_PROBE_SLED) && (defined(SERVO_ENDSTOPS) || defined(Z_PROBE_ALLEN_KEY))
|
4376
|
4386
|
|
4377
|
4387
|
#ifdef SERVO_ENDSTOPS
|
4378
|
4388
|
void raise_z_for_servo() {
|
4379
|
4389
|
float zpos = current_position[Z_AXIS], z_dest = Z_RAISE_BEFORE_HOMING;
|
4380
|
|
- if (!axis_known_position[Z_AXIS]) z_dest += zpos;
|
|
4390
|
+ z_dest += axis_known_position[Z_AXIS] ? -zprobe_zoffset : zpos;
|
4381
|
4391
|
if (zpos < z_dest)
|
4382
|
4392
|
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_dest); // also updates current_position
|
4383
|
4393
|
}
|
|
@@ -4400,7 +4410,7 @@ inline void gcode_M400() { st_synchronize(); }
|
4400
|
4410
|
#ifdef SERVO_ENDSTOPS
|
4401
|
4411
|
raise_z_for_servo();
|
4402
|
4412
|
#endif
|
4403
|
|
- stow_z_probe();
|
|
4413
|
+ stow_z_probe(false);
|
4404
|
4414
|
}
|
4405
|
4415
|
|
4406
|
4416
|
#endif
|