From aa80e448e24a27ac0006de4e3d3e93b18c73e3e0 Mon Sep 17 00:00:00 2001 From: Jan 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 . + * + */ +#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 S". (Requires servos) + * M281 - Set servo min|max position: "M281 P L U". (Requires servos) * M290 - Babystepping (Requires BABYSTEPPING) * M300 - Play beep sound S P * 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); }