|
@@ -555,22 +555,26 @@ void suicide() {
|
555
|
555
|
void servo_init() {
|
556
|
556
|
#if NUM_SERVOS >= 1 && HAS_SERVO_0
|
557
|
557
|
servo[0].attach(SERVO0_PIN);
|
|
558
|
+ servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
|
558
|
559
|
#endif
|
559
|
560
|
#if NUM_SERVOS >= 2 && HAS_SERVO_1
|
560
|
561
|
servo[1].attach(SERVO1_PIN);
|
|
562
|
+ servo[1].detach();
|
561
|
563
|
#endif
|
562
|
564
|
#if NUM_SERVOS >= 3 && HAS_SERVO_2
|
563
|
565
|
servo[2].attach(SERVO2_PIN);
|
|
566
|
+ servo[2].detach();
|
564
|
567
|
#endif
|
565
|
568
|
#if NUM_SERVOS >= 4 && HAS_SERVO_3
|
566
|
569
|
servo[3].attach(SERVO3_PIN);
|
|
570
|
+ servo[3].detach();
|
567
|
571
|
#endif
|
568
|
572
|
|
569
|
573
|
// Set position of Servo Endstops that are defined
|
570
|
574
|
#ifdef SERVO_ENDSTOPS
|
571
|
575
|
for (int i = 0; i < 3; i++)
|
572
|
576
|
if (servo_endstops[i] >= 0)
|
573
|
|
- servo[servo_endstops[i]].move(0, servo_endstop_angles[i * 2 + 1]);
|
|
577
|
+ servo[servo_endstops[i]].move(servo_endstop_angles[i * 2 + 1]);
|
574
|
578
|
#endif
|
575
|
579
|
|
576
|
580
|
}
|
|
@@ -1310,10 +1314,7 @@ static void setup_for_endstop_move() {
|
1310
|
1314
|
#ifdef SERVO_ENDSTOPS
|
1311
|
1315
|
|
1312
|
1316
|
// Engage Z Servo endstop if enabled
|
1313
|
|
- if (servo_endstops[Z_AXIS] >= 0) {
|
1314
|
|
- Servo *srv = &servo[servo_endstops[Z_AXIS]];
|
1315
|
|
- srv->move(0, servo_endstop_angles[Z_AXIS * 2]);
|
1316
|
|
- }
|
|
1317
|
+ if (servo_endstops[Z_AXIS] >= 0) servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS * 2]);
|
1317
|
1318
|
|
1318
|
1319
|
#elif defined(Z_PROBE_ALLEN_KEY)
|
1319
|
1320
|
feedrate = Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE;
|
|
@@ -1413,8 +1414,7 @@ static void setup_for_endstop_move() {
|
1413
|
1414
|
#endif
|
1414
|
1415
|
|
1415
|
1416
|
// Change the Z servo angle
|
1416
|
|
- Servo *srv = &servo[servo_endstops[Z_AXIS]];
|
1417
|
|
- srv->move(0, servo_endstop_angles[Z_AXIS * 2 + 1]);
|
|
1417
|
+ servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS * 2 + 1]);
|
1418
|
1418
|
}
|
1419
|
1419
|
|
1420
|
1420
|
#elif defined(Z_PROBE_ALLEN_KEY)
|
|
@@ -1665,8 +1665,8 @@ static void homeaxis(AxisEnum axis) {
|
1665
|
1665
|
#ifdef SERVO_ENDSTOPS
|
1666
|
1666
|
if (axis != Z_AXIS) {
|
1667
|
1667
|
// Engage Servo endstop if enabled
|
1668
|
|
- if (servo_endstops[axis] > -1)
|
1669
|
|
- servo[servo_endstops[axis]].move(0, servo_endstop_angles[axis * 2]);
|
|
1668
|
+ if (servo_endstops[axis] >= 0)
|
|
1669
|
+ servo[servo_endstops[axis]].move(servo_endstop_angles[axis * 2]);
|
1670
|
1670
|
}
|
1671
|
1671
|
#endif
|
1672
|
1672
|
|
|
@@ -1768,8 +1768,8 @@ static void homeaxis(AxisEnum axis) {
|
1768
|
1768
|
{
|
1769
|
1769
|
#ifdef SERVO_ENDSTOPS
|
1770
|
1770
|
// Retract Servo endstop if enabled
|
1771
|
|
- if (servo_endstops[axis] > -1)
|
1772
|
|
- servo[servo_endstops[axis]].move(0, servo_endstop_angles[axis * 2 + 1]);
|
|
1771
|
+ if (servo_endstops[axis] >= 0)
|
|
1772
|
+ servo[servo_endstops[axis]].move(servo_endstop_angles[axis * 2 + 1]);
|
1773
|
1773
|
#endif
|
1774
|
1774
|
}
|
1775
|
1775
|
|
|
@@ -4233,10 +4233,8 @@ inline void gcode_M226() {
|
4233
|
4233
|
int servo_position = 0;
|
4234
|
4234
|
if (code_seen('S')) {
|
4235
|
4235
|
servo_position = code_value_short();
|
4236
|
|
- if (servo_index >= 0 && servo_index < NUM_SERVOS) {
|
4237
|
|
- Servo *srv = &servo[servo_index];
|
4238
|
|
- srv->move(0, servo_position);
|
4239
|
|
- }
|
|
4236
|
+ if (servo_index >= 0 && servo_index < NUM_SERVOS)
|
|
4237
|
+ servo[servo_index].move(servo_position);
|
4240
|
4238
|
else {
|
4241
|
4239
|
SERIAL_ECHO_START;
|
4242
|
4240
|
SERIAL_ECHO("Servo ");
|