Store servo angles in EEPROM

2.0.x
Jan 6 years ago committed by Scott Lahteine
parent 9953213513
commit aa80e448e2

@ -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

@ -103,8 +103,8 @@ inline void servo_probe_test() {
SERIAL_PROTOCOLLNPGM("Servo probe test"); SERIAL_PROTOCOLLNPGM("Servo probe test");
SERIAL_PROTOCOLLNPAIR(". using index: ", probe_index); SERIAL_PROTOCOLLNPAIR(". using index: ", probe_index);
SERIAL_PROTOCOLLNPAIR(". deploy angle: ", z_servo_angle[0]); SERIAL_PROTOCOLLNPAIR(". deploy angle: ", servo_angles[probe_index][0]);
SERIAL_PROTOCOLLNPAIR(". stow angle: ", z_servo_angle[1]); SERIAL_PROTOCOLLNPAIR(". stow angle: ", servo_angles[probe_index][1]);
bool probe_inverting; bool probe_inverting;
@ -146,10 +146,10 @@ inline void servo_probe_test() {
uint8_t i = 0; uint8_t i = 0;
bool deploy_state, stow_state; bool deploy_state, stow_state;
do { 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); safe_delay(500);
deploy_state = READ(PROBE_TEST_PIN); 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); safe_delay(500);
stow_state = READ(PROBE_TEST_PIN); stow_state = READ(PROBE_TEST_PIN);
} while (++i < 4); } while (++i < 4);
@ -170,7 +170,7 @@ inline void servo_probe_test() {
#endif #endif
} }
else { // measure active signal length 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); safe_delay(500);
SERIAL_PROTOCOLLNPGM("please trigger probe"); SERIAL_PROTOCOLLNPGM("please trigger probe");
uint16_t probe_counter = 0; uint16_t probe_counter = 0;
@ -194,7 +194,7 @@ inline void servo_probe_test() {
else else
SERIAL_PROTOCOLLNPGM("noise detected - please re-run test"); // less than 2mS pulse 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 } // pulse detected

@ -487,6 +487,7 @@ void GcodeSuite::process_parsed_command(
#if HAS_SERVOS #if HAS_SERVOS
case 280: M280(); break; // M280: Set servo position absolute case 280: M280(); break; // M280: Set servo position absolute
case 281: M281(); break;
#endif #endif
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING)

@ -172,6 +172,7 @@
* M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS) * M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS)
* M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS) * M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS)
* M280 - Set servo position absolute: "M280 P<index> S<angle|µs>". (Requires servos) * 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) * M290 - Babystepping (Requires BABYSTEPPING)
* M300 - Play beep sound S<frequency Hz> P<duration ms> * M300 - Play beep sound S<frequency Hz> P<duration ms>
* M301 - Set PID parameters P I and D. (Requires PIDTEMP) * M301 - Set PID parameters P I and D. (Requires PIDTEMP)
@ -627,6 +628,7 @@ private:
#if HAS_SERVOS #if HAS_SERVOS
static void M280(); static void M280();
static void M281();
#endif #endif
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING)

@ -37,7 +37,7 @@
*/ */
// Change EEPROM version if the structure changes // Change EEPROM version if the structure changes
#define EEPROM_VERSION "V55" #define EEPROM_VERSION "V56"
#define EEPROM_OFFSET 100 #define EEPROM_OFFSET 100
// Check the integrity of data offsets. // Check the integrity of data offsets.
@ -70,6 +70,10 @@
#include "../feature/bedlevel/bedlevel.h" #include "../feature/bedlevel/bedlevel.h"
#endif #endif
#if HAS_SERVOS
#include "servo.h"
#endif
#if HAS_BED_PROBE #if HAS_BED_PROBE
#include "../module/probe.h" #include "../module/probe.h"
#endif #endif
@ -174,6 +178,13 @@ typedef struct SettingsDataStruct {
bool planner_leveling_active; // M420 S planner.leveling_active bool planner_leveling_active; // M420 S planner.leveling_active
int8_t ubl_storage_slot; // ubl.storage_slot 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 // DELTA / [XYZ]_DUAL_ENDSTOPS
// //
@ -532,6 +543,10 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(storage_slot); EEPROM_WRITE(storage_slot);
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL
#if HAS_SERVOS
EEPROM_WRITE(servo_angles);
#endif
// 11 floats for DELTA / [XYZ]_DUAL_ENDSTOPS // 11 floats for DELTA / [XYZ]_DUAL_ENDSTOPS
#if ENABLED(DELTA) #if ENABLED(DELTA)
@ -1135,6 +1150,14 @@ void MarlinSettings::postprocess() {
EEPROM_READ(dummyui8); EEPROM_READ(dummyui8);
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL
//
// SERVO_ANGLES
//
#if HAS_SERVOS
EEPROM_READ(servo_angles);
#endif
// //
// DELTA Geometry or Dual Endstops offsets // DELTA Geometry or Dual Endstops offsets
// //
@ -1774,6 +1797,37 @@ void MarlinSettings::reset(PORTARG_SOLO) {
zprobe_zoffset = Z_PROBE_OFFSET_FROM_EXTRUDER; zprobe_zoffset = Z_PROBE_OFFSET_FROM_EXTRUDER;
#endif #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) #if ENABLED(DELTA)
const float adj[ABC] = DELTA_ENDSTOP_ADJ, const float adj[ABC] = DELTA_ENDSTOP_ADJ,
dta[ABC] = DELTA_TOWER_ANGLE_TRIM; dta[ABC] = DELTA_TOWER_ANGLE_TRIM;
@ -2252,6 +2306,20 @@ void MarlinSettings::reset(PORTARG_SOLO) {
#endif // HAS_LEVELING #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 ENABLED(DELTA)
if (!forReplay) { if (!forReplay) {

@ -54,7 +54,6 @@ float zprobe_zoffset; // Initialized by settings.load()
#if HAS_Z_SERVO_PROBE #if HAS_Z_SERVO_PROBE
#include "../module/servo.h" #include "../module/servo.h"
const int z_servo_angle[2] = Z_SERVO_ANGLES;
#endif #endif
#if ENABLED(Z_PROBE_SLED) #if ENABLED(Z_PROBE_SLED)
@ -435,7 +434,7 @@ bool set_probe_deployed(const bool deploy) {
#elif HAS_Z_SERVO_PROBE && DISABLED(BLTOUCH) #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) #elif ENABLED(Z_PROBE_ALLEN_KEY)

@ -53,7 +53,6 @@
#endif #endif
#if HAS_Z_SERVO_PROBE #if HAS_Z_SERVO_PROBE
extern const int z_servo_angle[2];
void servo_probe_init(); void servo_probe_init();
#endif #endif

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

@ -30,6 +30,7 @@
#include "../HAL/shared/servo.h" #include "../HAL/shared/servo.h"
extern HAL_SERVO_LIB servo[NUM_SERVOS]; extern HAL_SERVO_LIB servo[NUM_SERVOS];
extern uint8_t servo_angles[NUM_SERVOS][2];
extern void servo_init(); extern void servo_init();
#define MOVE_SERVO(I, P) servo[I].move(P) #define MOVE_SERVO(I, P) servo[I].move(P)
@ -37,8 +38,8 @@ extern void servo_init();
#include "../inc/MarlinConfig.h" #include "../inc/MarlinConfig.h"
#if HAS_Z_SERVO_PROBE #if HAS_Z_SERVO_PROBE
#define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, z_servo_angle[0]) #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, z_servo_angle[1]) #define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][1])
#endif #endif
#endif // _SERVO_H_ #endif // _SERVO_H_

@ -60,22 +60,18 @@
#if DO_SWITCH_EXTRUDER #if DO_SWITCH_EXTRUDER
#if EXTRUDERS > 3 #if EXTRUDERS > 3
#define REQ_ANGLES 4
#define _SERVO_NR (e < 2 ? SWITCHING_EXTRUDER_SERVO_NR : SWITCHING_EXTRUDER_E23_SERVO_NR) #define _SERVO_NR (e < 2 ? SWITCHING_EXTRUDER_SERVO_NR : SWITCHING_EXTRUDER_E23_SERVO_NR)
#else #else
#define REQ_ANGLES 2
#define _SERVO_NR SWITCHING_EXTRUDER_SERVO_NR #define _SERVO_NR SWITCHING_EXTRUDER_SERVO_NR
#endif #endif
void move_extruder_servo(const uint8_t e) { 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(); planner.synchronize();
#if EXTRUDERS & 1 #if EXTRUDERS & 1
if (e < EXTRUDERS - 1) if (e < EXTRUDERS - 1)
#endif #endif
{ {
MOVE_SERVO(_SERVO_NR, angles[e]); MOVE_SERVO(_SERVO_NR, servo_angles[_SERVO_NR][e]);
safe_delay(500); safe_delay(500);
} }
} }
@ -85,9 +81,8 @@
#if ENABLED(SWITCHING_NOZZLE) #if ENABLED(SWITCHING_NOZZLE)
void move_nozzle_servo(const uint8_t e) { void move_nozzle_servo(const uint8_t e) {
const int16_t angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
planner.synchronize(); 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); safe_delay(500);
} }

Loading…
Cancel
Save