Add forgotten Marlin_main.cpp

Marlin/Marlin_main.cpp

@@ -555,22 +555,26 @@ void suicide() {
 void servo_init() {
   #if NUM_SERVOS >= 1 && HAS_SERVO_0
     servo[0].attach(SERVO0_PIN);
+    servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
   #endif
   #if NUM_SERVOS >= 2 && HAS_SERVO_1
     servo[1].attach(SERVO1_PIN);
+    servo[1].detach();
   #endif
   #if NUM_SERVOS >= 3 && HAS_SERVO_2
     servo[2].attach(SERVO2_PIN);
+    servo[2].detach();
   #endif
   #if NUM_SERVOS >= 4 && HAS_SERVO_3
     servo[3].attach(SERVO3_PIN);
+    servo[3].detach();
   #endif
 
   // Set position of Servo Endstops that are defined
   #ifdef SERVO_ENDSTOPS
     for (int i = 0; i < 3; i++)
       if (servo_endstops[i] >= 0)
-        servo[servo_endstops[i]].move(0, servo_endstop_angles[i * 2 + 1]);
+        servo[servo_endstops[i]].move(servo_endstop_angles[i * 2 + 1]);
   #endif
 
 }
@@ -1310,10 +1314,7 @@ static void setup_for_endstop_move() {
     #ifdef SERVO_ENDSTOPS
 
       // Engage Z Servo endstop if enabled
-      if (servo_endstops[Z_AXIS] >= 0) {
+      if (servo_endstops[Z_AXIS] >= 0) servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS * 2]);
-        Servo *srv = &servo[servo_endstops[Z_AXIS]];
-        srv->move(0, servo_endstop_angles[Z_AXIS * 2]);
-      }
 
     #elif defined(Z_PROBE_ALLEN_KEY)
       feedrate = Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE;
@@ -1413,8 +1414,7 @@ static void setup_for_endstop_move() {
         #endif
 
         // Change the Z servo angle
-        Servo *srv = &servo[servo_endstops[Z_AXIS]];
+        servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS * 2 + 1]);
-        srv->move(0, servo_endstop_angles[Z_AXIS * 2 + 1]);
       }
 
     #elif defined(Z_PROBE_ALLEN_KEY)
@@ -1665,8 +1665,8 @@ static void homeaxis(AxisEnum axis) {
     #ifdef SERVO_ENDSTOPS
       if (axis != Z_AXIS) {
         // Engage Servo endstop if enabled
-        if (servo_endstops[axis] > -1)
+        if (servo_endstops[axis] >= 0) 
-          servo[servo_endstops[axis]].move(0, servo_endstop_angles[axis * 2]);
+          servo[servo_endstops[axis]].move(servo_endstop_angles[axis * 2]);
       }
     #endif
 
@@ -1768,8 +1768,8 @@ static void homeaxis(AxisEnum axis) {
     {
       #ifdef SERVO_ENDSTOPS
         // Retract Servo endstop if enabled
-        if (servo_endstops[axis] > -1)
+        if (servo_endstops[axis] >= 0)
-          servo[servo_endstops[axis]].move(0, servo_endstop_angles[axis * 2 + 1]);
+          servo[servo_endstops[axis]].move(servo_endstop_angles[axis * 2 + 1]);
       #endif
     }
 
@@ -4233,10 +4233,8 @@ inline void gcode_M226() {
     int servo_position = 0;
     if (code_seen('S')) {
       servo_position = code_value_short();
-      if (servo_index >= 0 && servo_index < NUM_SERVOS) {
+      if (servo_index >= 0 && servo_index < NUM_SERVOS) 
-        Servo *srv = &servo[servo_index];
+        servo[servo_index].move(servo_position);
-        srv->move(0, servo_position);
-      }
       else {
         SERIAL_ECHO_START;
         SERIAL_ECHO("Servo ");