Store servo angles in EEPROM

Marlin/src/gcode/config/M281.cpp

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "../gcode.h"
+#include "../../module/servo.h"
+
+void GcodeSuite::M281() {
+    if (!parser.seen('P')) return;
+  const int servo_index = parser.value_int();
+  if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    bool angle_change = false;
+    if (parser.seen('L')) {
+      servo_angles[servo_index][0] = parser.value_int();
+      angle_change = true;
+    }
+    if (parser.seen('U')) {
+      servo_angles[servo_index][1] = parser.value_int();
+      angle_change = true;
+    }
+    if (!angle_change) {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPAIR(" Servo ", servo_index);
+      SERIAL_ECHOPAIR(" L", servo_angles[servo_index][0]);
+      SERIAL_ECHOLNPAIR(" U", servo_angles[servo_index][1]);
+    }
+  }
+  else {
+    SERIAL_ERROR_START();
+    SERIAL_ECHOPAIR("Servo ", servo_index);
+    SERIAL_ECHOLNPGM(" out of range");
+  }
+}
+
+#endif

Marlin/src/gcode/config/M43.cpp

 
     SERIAL_PROTOCOLLNPGM("Servo probe test");
     SERIAL_PROTOCOLLNPAIR(".  using index:  ", probe_index);
-    SERIAL_PROTOCOLLNPAIR(".  deploy angle: ", z_servo_angle[0]);
-    SERIAL_PROTOCOLLNPAIR(".  stow angle:   ", z_servo_angle[1]);
+    SERIAL_PROTOCOLLNPAIR(".  deploy angle: ", servo_angles[probe_index][0]);
+    SERIAL_PROTOCOLLNPAIR(".  stow angle:   ", servo_angles[probe_index][1]);
 
     bool probe_inverting;
 
     uint8_t i = 0;
     bool deploy_state, stow_state;
     do {
-      MOVE_SERVO(probe_index, z_servo_angle[0]); //deploy
+      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); //deploy
       safe_delay(500);
       deploy_state = READ(PROBE_TEST_PIN);
-      MOVE_SERVO(probe_index, z_servo_angle[1]); //stow
+      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); //stow
       safe_delay(500);
       stow_state = READ(PROBE_TEST_PIN);
     } while (++i < 4);
       #endif
     }
     else {                                           // measure active signal length
-      MOVE_SERVO(probe_index, z_servo_angle[0]);     // deploy
+      MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]);     // deploy
       safe_delay(500);
       SERIAL_PROTOCOLLNPGM("please trigger probe");
       uint16_t probe_counter = 0;
           else
             SERIAL_PROTOCOLLNPGM("noise detected - please re-run test"); // less than 2mS pulse
 
-          MOVE_SERVO(probe_index, z_servo_angle[1]); //stow
+          MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); //stow
 
         }  // pulse detected
 

Marlin/src/gcode/gcode.cpp

 
       #if HAS_SERVOS
         case 280: M280(); break;                                  // M280: Set servo position absolute
+        case 281: M281(); break;
       #endif
 
       #if ENABLED(BABYSTEPPING)

Marlin/src/gcode/gcode.h

  * M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS)
  * M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS)
  * M280 - Set servo position absolute: "M280 P<index> S<angle|µs>". (Requires servos)
+ * M281 - Set servo min|max position: "M281 P<index> L<min> U<max>". (Requires servos)
  * M290 - Babystepping (Requires BABYSTEPPING)
  * M300 - Play beep sound S<frequency Hz> P<duration ms>
  * M301 - Set PID parameters P I and D. (Requires PIDTEMP)
 
   #if HAS_SERVOS
     static void M280();
+    static void M281();
   #endif
 
   #if ENABLED(BABYSTEPPING)

Marlin/src/module/configuration_store.cpp

  */
 
 // Change EEPROM version if the structure changes
-#define EEPROM_VERSION "V55"
+#define EEPROM_VERSION "V56"
 #define EEPROM_OFFSET 100
 
 // Check the integrity of data offsets.
   #include "../feature/bedlevel/bedlevel.h"
 #endif
 
+#if HAS_SERVOS
+  #include "servo.h"
+#endif 
+
 #if HAS_BED_PROBE
   #include "../module/probe.h"
 #endif
   int8_t ubl_storage_slot;                              // ubl.storage_slot
 
   //
+  // SERVO_ANGLES
+  //
+  #if HAS_SERVOS
+    uint8_t servo_angles[NUM_SERVOS][2];
+  #endif
+
+  //
   // DELTA / [XYZ]_DUAL_ENDSTOPS
   //
   #if ENABLED(DELTA)
       EEPROM_WRITE(storage_slot);
     #endif // AUTO_BED_LEVELING_UBL
 
+    #if HAS_SERVOS
+      EEPROM_WRITE(servo_angles);
+    #endif
+
     // 11 floats for DELTA / [XYZ]_DUAL_ENDSTOPS
     #if ENABLED(DELTA)
 
       #endif // AUTO_BED_LEVELING_UBL
 
       //
+      // SERVO_ANGLES
+      //
+      #if HAS_SERVOS
+        EEPROM_READ(servo_angles);
+      #endif
+
+
+      //
       // DELTA Geometry or Dual Endstops offsets
       //
 
     zprobe_zoffset = Z_PROBE_OFFSET_FROM_EXTRUDER;
   #endif
 
+  //
+  // Servo Angles
+  //
+
+  #if HAS_SERVOS
+    #if ENABLED(SWITCHING_EXTRUDER)
+      #if EXTRUDERS > 3
+        #define REQ_ANGLES 4
+      #else
+        #define REQ_ANGLES 2
+      #endif
+      const uint8_t extruder_angles[2] = SWITCHING_EXTRUDER_SERVO_ANGLES;
+      static_assert(COUNT(extruder_angles) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
+      servo_angles[SWITCHING_EXTRUDER_SERVO_NR][0] = extruder_angles[0];
+      servo_angles[SWITCHING_EXTRUDER_SERVO_NR][1] = extruder_angles[1];
+    #endif
+
+    #if ENABLED(SWITCHING_NOZZLE)
+      const uint8_t nozzel_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
+      servo_angles[SWITCHING_NOZZLE_SERVO_NR][0] = nozzel_angles[0];
+      servo_angles[SWITCHING_NOZZLE_SERVO_NR][1] = nozzel_angles[1];
+    #endif
+
+    #if defined(Z_SERVO_ANGLES) && defined(Z_PROBE_SERVO_NR)
+      const uint8_t z_probe_angles[2] = Z_SERVO_ANGLES;
+      servo_angles[Z_PROBE_SERVO_NR][0] = z_probe_angles[0];
+      servo_angles[Z_PROBE_SERVO_NR][1] = z_probe_angles[1];
+    #endif
+
+  #endif
+
   #if ENABLED(DELTA)
     const float adj[ABC] = DELTA_ENDSTOP_ADJ,
                 dta[ABC] = DELTA_TOWER_ANGLE_TRIM;
 
     #endif // HAS_LEVELING
 
+    #if HAS_SERVOS
+      if (!forReplay) {
+        CONFIG_ECHO_START;
+        SERIAL_ECHOLNPGM_P(port, "Servo Angles:");
+      }
+      for (uint8_t i = 0; i < NUM_SERVOS; i++) {
+        CONFIG_ECHO_START;
+        SERIAL_ECHOPAIR_P(port, "  M281 P", i);
+        SERIAL_ECHOPAIR_P(port, " L",servo_angles[i][0]);
+        SERIAL_ECHOPAIR_P(port, " U",servo_angles[i][1]);
+        SERIAL_EOL_P(port);
+      }
+    #endif
+
     #if ENABLED(DELTA)
 
       if (!forReplay) {

Marlin/src/module/probe.cpp

 
 #if HAS_Z_SERVO_PROBE
   #include "../module/servo.h"
-  const int z_servo_angle[2] = Z_SERVO_ANGLES;
 #endif
 
 #if ENABLED(Z_PROBE_SLED)
 
       #elif HAS_Z_SERVO_PROBE && DISABLED(BLTOUCH)
 
-        MOVE_SERVO(Z_PROBE_SERVO_NR, z_servo_angle[deploy ? 0 : 1]);
+        MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][(deploy ? 0 : 1)]);
 
       #elif ENABLED(Z_PROBE_ALLEN_KEY)
 

Marlin/src/module/probe.h

 #endif
 
 #if HAS_Z_SERVO_PROBE
-  extern const int z_servo_angle[2];
   void servo_probe_init();
 #endif
 

Marlin/src/module/servo.cpp

 #include "servo.h"
 
 HAL_SERVO_LIB servo[NUM_SERVOS];
+uint8_t servo_angles[NUM_SERVOS][2];
 
 void servo_init() {
   #if NUM_SERVOS >= 1 && HAS_SERVO_0

Marlin/src/module/servo.h

 #include "../HAL/shared/servo.h"
 
 extern HAL_SERVO_LIB servo[NUM_SERVOS];
+extern uint8_t servo_angles[NUM_SERVOS][2];
 extern void servo_init();
 
 #define MOVE_SERVO(I, P) servo[I].move(P)
 #include "../inc/MarlinConfig.h"
 
 #if HAS_Z_SERVO_PROBE
-  #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, z_servo_angle[0])
-  #define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, z_servo_angle[1])
+  #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][0])
+  #define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][1])
 #endif
 
 #endif // _SERVO_H_

Marlin/src/module/tool_change.cpp

 #if DO_SWITCH_EXTRUDER
 
   #if EXTRUDERS > 3
-    #define REQ_ANGLES 4
     #define _SERVO_NR (e < 2 ? SWITCHING_EXTRUDER_SERVO_NR : SWITCHING_EXTRUDER_E23_SERVO_NR)
   #else
-    #define REQ_ANGLES 2
     #define _SERVO_NR SWITCHING_EXTRUDER_SERVO_NR
   #endif
 
   void move_extruder_servo(const uint8_t e) {
-    constexpr int16_t angles[] = SWITCHING_EXTRUDER_SERVO_ANGLES;
-    static_assert(COUNT(angles) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
     planner.synchronize();
     #if EXTRUDERS & 1
       if (e < EXTRUDERS - 1)
     #endif
     {
-      MOVE_SERVO(_SERVO_NR, angles[e]);
+      MOVE_SERVO(_SERVO_NR, servo_angles[_SERVO_NR][e]);
       safe_delay(500);
     }
   }
 #if ENABLED(SWITCHING_NOZZLE)
 
   void move_nozzle_servo(const uint8_t e) {
-    const int16_t angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
     planner.synchronize();
-    MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, angles[e]);
+    MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, servo_angles[SWITCHING_EXTRUDER_SERVO_NR][e]);
     safe_delay(500);
   }