|
@@ -368,11 +368,16 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
|
368
|
368
|
|
369
|
369
|
#if ENABLED(SWITCHING_TOOLHEAD)
|
370
|
370
|
|
|
371
|
+ inline void swt_lock(const bool locked=true) {
|
|
372
|
+ const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES;
|
|
373
|
+ MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]);
|
|
374
|
+ }
|
|
375
|
+
|
|
376
|
+ void swt_init() { swt_lock(); }
|
|
377
|
+
|
371
|
378
|
inline void switching_toolhead_tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
|
372
|
379
|
if (no_move) return;
|
373
|
380
|
|
374
|
|
- constexpr uint16_t angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES;
|
375
|
|
-
|
376
|
381
|
constexpr float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS;
|
377
|
382
|
const float placexpos = toolheadposx[active_extruder],
|
378
|
383
|
grabxpos = toolheadposx[new_tool];
|
|
@@ -406,7 +411,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
|
406
|
411
|
|
407
|
412
|
planner.synchronize();
|
408
|
413
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(2) Unlock and Place Toolhead");
|
409
|
|
- MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[1]);
|
|
414
|
+ swt_lock(false);
|
410
|
415
|
safe_delay(500);
|
411
|
416
|
|
412
|
417
|
current_position.y = SWITCHING_TOOLHEAD_Y_POS;
|
|
@@ -451,7 +456,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
|
451
|
456
|
// Wait for move to finish, pause 0.2s, move servo, pause 0.5s
|
452
|
457
|
planner.synchronize();
|
453
|
458
|
safe_delay(200);
|
454
|
|
- MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[0]);
|
|
459
|
+ swt_lock();
|
455
|
460
|
safe_delay(500);
|
456
|
461
|
|
457
|
462
|
current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
|