diff --git a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp index f8fc315ba..ef81db69f 100644 --- a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp +++ b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp @@ -27,7 +27,7 @@ #if HAS_SERVOS -uint8_t ServoCount; //=0 +uint8_t ServoCount = 0; #include "HAL_Servo_STM32F1.h" @@ -68,16 +68,16 @@ libServo::libServo() { bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) { if (this->servoIndex >= MAX_SERVOS) return false; + if (!PWM_PIN(pin)) return false; - this->pin = pin; this->minAngle = minAngle; this->maxAngle = maxAngle; - timer_dev *tdev = PIN_MAP[this->pin].timer_device; - uint8_t tchan = PIN_MAP[this->pin].timer_channel; + timer_dev *tdev = PIN_MAP[pin].timer_device; + uint8_t tchan = PIN_MAP[pin].timer_channel; - pinMode(this->pin, PWM); - pwmWrite(this->pin, 0); + pinMode(pin, PWM); + pwmWrite(pin, 0); timer_pause(tdev); timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based @@ -85,6 +85,8 @@ bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t m timer_generate_update(tdev); timer_resume(tdev); + this->pin = pin; // set attached() + return true; } diff --git a/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h b/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h index 3ca2dfdd9..ad225a8f9 100644 --- a/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h +++ b/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h @@ -51,7 +51,7 @@ #define IS_INPUT(IO) (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD) #define IS_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP) -#define PWM_PIN(P) (PIN_MAP[IO].timer_device != nullptr) +#define PWM_PIN(IO) (PIN_MAP[IO].timer_device != nullptr) // digitalRead/Write wrappers #define extDigitalRead(IO) digitalRead(IO) diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp index e7ee35413..f973e81b1 100644 --- a/Marlin/src/gcode/control/M280.cpp +++ b/Marlin/src/gcode/control/M280.cpp @@ -34,8 +34,13 @@ void GcodeSuite::M280() { if (!parser.seen('P')) return; const int servo_index = parser.value_int(); if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) { - if (parser.seen('S')) - MOVE_SERVO(servo_index, parser.value_int()); + if (parser.seen('S')) { + const int a = parser.value_int(); + if (a == -1) + servo[servo_index].detach(); + else + MOVE_SERVO(servo_index, a); + } else { SERIAL_ECHO_START(); SERIAL_ECHOPAIR(" Servo ", servo_index);