From 764f0d9c1c86182e36925ea1d85b86c9dba497fb Mon Sep 17 00:00:00 2001 From: Tanguy Pruvot Date: Fri, 7 Jun 2019 14:11:48 +0200 Subject: [PATCH] STM32F1: Servo "soft" PWM via timer interrupt (#14187) --- .../src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp | 122 +++++++++++++++++- .../src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h | 7 + Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h | 2 +- 3 files changed, 124 insertions(+), 7 deletions(-) diff --git a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp index ef81db69f3..3793919a8d 100644 --- a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp +++ b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp @@ -30,6 +30,7 @@ uint8_t ServoCount = 0; #include "HAL_Servo_STM32F1.h" +#include "HAL_timers_STM32F1.h" //#include "Servo.h" @@ -46,7 +47,7 @@ uint8_t ServoCount = 0; * * This uses the smallest prescaler that allows an overflow < 2^16. */ -#define MAX_OVERFLOW ((1 << 16) - 1) +#define MAX_OVERFLOW UINT16_MAX //((1 << 16) - 1) #define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) #define TAU_MSEC 20 #define TAU_USEC (TAU_MSEC * 1000) @@ -62,22 +63,45 @@ uint8_t ServoCount = 0; #define US_TO_ANGLE(us) ((int16_t)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, \ this->minAngle, this->maxAngle))) +void libServo::servoWrite(uint8_t pin, uint16_t duty_cycle) { + #ifdef SERVO0_TIMER_NUM + if (this->servoIndex == 0) { + this->pwmSetDuty(duty_cycle); + return; + } + #endif + + timer_dev *tdev = PIN_MAP[pin].timer_device; + uint8_t tchan = PIN_MAP[pin].timer_channel; + if (tdev) timer_set_compare(tdev, tchan, duty_cycle); +} + libServo::libServo() { this->servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO; } 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; + if (pin >= BOARD_NR_GPIO_PINS) return false; this->minAngle = minAngle; this->maxAngle = maxAngle; + this->angle = -1; + + #ifdef SERVO0_TIMER_NUM + if (this->servoIndex == 0 && this->setupSoftPWM(pin)) { + this->pin = pin; // set attached() + return true; + } + #endif + + if (!PWM_PIN(pin)) return false; timer_dev *tdev = PIN_MAP[pin].timer_device; uint8_t tchan = PIN_MAP[pin].timer_channel; - pinMode(pin, PWM); - pwmWrite(pin, 0); + SET_PWM(pin); + servoWrite(pin, 0); timer_pause(tdev); timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based @@ -92,12 +116,16 @@ bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t m bool libServo::detach() { if (!this->attached()) return false; - pwmWrite(this->pin, 0); + this->angle = -1; + servoWrite(this->pin, 0); return true; } int32_t libServo::read() const { if (this->attached()) { + #ifdef SERVO0_TIMER_NUM + if (this->servoIndex == 0) return this->angle; + #endif timer_dev *tdev = PIN_MAP[this->pin].timer_device; uint8_t tchan = PIN_MAP[this->pin].timer_channel; return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan))); @@ -110,13 +138,95 @@ void libServo::move(const int32_t value) { static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); if (this->attached()) { - pwmWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(constrain(value, this->minAngle, this->maxAngle)))); + this->angle = constrain(value, this->minAngle, this->maxAngle); + servoWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(this->angle))); safe_delay(servo_delay[this->servoIndex]); #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE) this->detach(); #endif } } + +#ifdef SERVO0_TIMER_NUM + extern "C" void Servo_IRQHandler(void) { + static timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + uint16_t SR = timer_get_status(tdev); + if (SR & TIMER_SR_CC1IF) { // channel 1 off + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(SERVO0_PIN, 1); // off + #else + OUT_WRITE(SERVO0_PIN, 0); + #endif + timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT); + } + if (SR & TIMER_SR_CC2IF) { // channel 2 resume + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(SERVO0_PIN, 0); // on + #else + OUT_WRITE(SERVO0_PIN, 1); + #endif + timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT); + } + } + + bool libServo::setupSoftPWM(const int32_t pin) { + timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + if (!tdev) return false; + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(pin, 1); + #else + OUT_WRITE(pin, 0); + #endif + + timer_pause(tdev); + timer_set_mode(tdev, 1, TIMER_OUTPUT_COMPARE); // counter with isr + timer_oc_set_mode(tdev, 1, TIMER_OC_MODE_FROZEN, 0); // no pin output change + timer_oc_set_mode(tdev, 2, TIMER_OC_MODE_FROZEN, 0); // no pin output change + timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based + timer_set_reload(tdev, SERVO_OVERFLOW); + timer_set_compare(tdev, 1, SERVO_OVERFLOW); + timer_set_compare(tdev, 2, SERVO_OVERFLOW); + timer_attach_interrupt(tdev, 1, Servo_IRQHandler); + timer_attach_interrupt(tdev, 2, Servo_IRQHandler); + timer_generate_update(tdev); + timer_resume(tdev); + + return true; + } + + void libServo::pwmSetDuty(const uint16_t duty_cycle) { + timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + timer_set_compare(tdev, 1, duty_cycle); + timer_generate_update(tdev); + if (duty_cycle) { + timer_enable_irq(tdev, 1); + timer_enable_irq(tdev, 2); + } + else { + timer_disable_irq(tdev, 1); + timer_disable_irq(tdev, 2); + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(this->pin, 1); // off + #else + OUT_WRITE(this->pin, 0); + #endif + } + } + + void libServo::pauseSoftPWM() { // detach + timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + timer_pause(tdev); + pwmSetDuty(0); + } + +#else + + bool libServo::setupSoftPWM(const int32_t pin) {} + void libServo::pwmSetDuty(const uint16_t duty_cycle) {} + void libServo::pauseSoftPWM() {} + +#endif + #endif // HAS_SERVOS #endif // __STM32F1__ diff --git a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h index ac83f086ee..d7b6bfc201 100644 --- a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h +++ b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h @@ -46,8 +46,15 @@ class libServo { void move(const int32_t value); int32_t read() const; private: + void servoWrite(uint8_t pin, const uint16_t duty_cycle); + uint8_t servoIndex; // index into the channel data for this servo int32_t pin = NOT_ATTACHED; int32_t minAngle; int32_t maxAngle; + int32_t angle; + + bool setupSoftPWM(const int32_t pin); + void pauseSoftPWM(); + void pwmSetDuty(const uint16_t duty_cycle); }; diff --git a/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h b/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h index ad225a8f9a..f1f2056260 100644 --- a/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h +++ b/Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h @@ -49,7 +49,7 @@ #define SET_PWM_OD(IO) pinMode(IO, PWM_OPEN_DRAIN) #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 IS_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP || _GET_MODE(IO) == GPIO_OUTPUT_OD) #define PWM_PIN(IO) (PIN_MAP[IO].timer_device != nullptr)