From aa80e448e24a27ac0006de4e3d3e93b18c73e3e0 Mon Sep 17 00:00:00 2001
From: Jan <janschon@uni-bremen.de>
Date: Tue, 7 Aug 2018 17:04:46 +0200
Subject: [PATCH] Store servo angles in EEPROM
---
Marlin/src/gcode/config/M281.cpp | 56 ++++++++++++++++++
Marlin/src/gcode/config/M43.cpp | 12 ++--
Marlin/src/gcode/gcode.cpp | 1 +
Marlin/src/gcode/gcode.h | 2 +
Marlin/src/module/configuration_store.cpp | 70 ++++++++++++++++++++++-
Marlin/src/module/probe.cpp | 3 +-
Marlin/src/module/probe.h | 1 -
Marlin/src/module/servo.cpp | 1 +
Marlin/src/module/servo.h | 5 +-
Marlin/src/module/tool_change.cpp | 9 +--
10 files changed, 141 insertions(+), 19 deletions(-)
create mode 100644 Marlin/src/gcode/config/M281.cpp
diff --git a/Marlin/src/gcode/config/M281.cpp b/Marlin/src/gcode/config/M281.cpp
new file mode 100644
index 0000000000..0feac59ce7
--- /dev/null
+++ b/Marlin/src/gcode/config/M281.cpp
@@ -0,0 +1,56 @@
+/**
+ * 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
\ No newline at end of file
diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp
index e9d36efd67..41a66973c4 100644
--- a/Marlin/src/gcode/config/M43.cpp
+++ b/Marlin/src/gcode/config/M43.cpp
@@ -103,8 +103,8 @@ inline void servo_probe_test() {
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;
@@ -146,10 +146,10 @@ inline void servo_probe_test() {
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);
@@ -170,7 +170,7 @@ inline void servo_probe_test() {
#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;
@@ -194,7 +194,7 @@ inline void servo_probe_test() {
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
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index ef282add0a..cfe98c8fad 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -487,6 +487,7 @@ void GcodeSuite::process_parsed_command(
#if HAS_SERVOS
case 280: M280(); break; // M280: Set servo position absolute
+ case 281: M281(); break;
#endif
#if ENABLED(BABYSTEPPING)
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index 2130b1636e..0fe97bea8c 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -172,6 +172,7 @@
* 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)
@@ -627,6 +628,7 @@ private:
#if HAS_SERVOS
static void M280();
+ static void M281();
#endif
#if ENABLED(BABYSTEPPING)
diff --git a/Marlin/src/module/configuration_store.cpp b/Marlin/src/module/configuration_store.cpp
index 83cb3b4c46..097842e47f 100644
--- a/Marlin/src/module/configuration_store.cpp
+++ b/Marlin/src/module/configuration_store.cpp
@@ -37,7 +37,7 @@
*/
// 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.
@@ -70,6 +70,10 @@
#include "../feature/bedlevel/bedlevel.h"
#endif
+#if HAS_SERVOS
+ #include "servo.h"
+#endif
+
#if HAS_BED_PROBE
#include "../module/probe.h"
#endif
@@ -174,6 +178,13 @@ typedef struct SettingsDataStruct {
bool planner_leveling_active; // M420 S planner.leveling_active
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
//
@@ -532,6 +543,10 @@ void MarlinSettings::postprocess() {
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)
@@ -1135,6 +1150,14 @@ void MarlinSettings::postprocess() {
EEPROM_READ(dummyui8);
#endif // AUTO_BED_LEVELING_UBL
+ //
+ // SERVO_ANGLES
+ //
+ #if HAS_SERVOS
+ EEPROM_READ(servo_angles);
+ #endif
+
+
//
// DELTA Geometry or Dual Endstops offsets
//
@@ -1774,6 +1797,37 @@ void MarlinSettings::reset(PORTARG_SOLO) {
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;
@@ -2252,6 +2306,20 @@ void MarlinSettings::reset(PORTARG_SOLO) {
#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) {
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
index 1b9cd39cda..1170d52feb 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -54,7 +54,6 @@ float zprobe_zoffset; // Initialized by settings.load()
#if HAS_Z_SERVO_PROBE
#include "../module/servo.h"
- const int z_servo_angle[2] = Z_SERVO_ANGLES;
#endif
#if ENABLED(Z_PROBE_SLED)
@@ -435,7 +434,7 @@ bool set_probe_deployed(const bool deploy) {
#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)
diff --git a/Marlin/src/module/probe.h b/Marlin/src/module/probe.h
index fe4c17b79e..f81482dcec 100644
--- a/Marlin/src/module/probe.h
+++ b/Marlin/src/module/probe.h
@@ -53,7 +53,6 @@
#endif
#if HAS_Z_SERVO_PROBE
- extern const int z_servo_angle[2];
void servo_probe_init();
#endif
diff --git a/Marlin/src/module/servo.cpp b/Marlin/src/module/servo.cpp
index a1f9633f78..1945867c28 100644
--- a/Marlin/src/module/servo.cpp
+++ b/Marlin/src/module/servo.cpp
@@ -31,6 +31,7 @@
#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
diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
index 05f4d6f4ef..807ad8e72b 100644
--- a/Marlin/src/module/servo.h
+++ b/Marlin/src/module/servo.h
@@ -30,6 +30,7 @@
#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)
@@ -37,8 +38,8 @@ extern void servo_init();
#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_
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index e4c1cc0f50..937d49020e 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -60,22 +60,18 @@
#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);
}
}
@@ -85,9 +81,8 @@
#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);
}