From a77f8c6dd51bc035a90815b9d9be15bfbede8e71 Mon Sep 17 00:00:00 2001 From: jmz52 Date: Tue, 21 Aug 2018 05:13:51 +0300 Subject: [PATCH] Implement servo support for STM32F1 (#11580) Implement servo support for STM32F1. Original code was incomplete and depended on Servo library from stm32duino which conflicts with `module/servo.h`. --- .../src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.cpp | 82 +++++++++++++++++-- .../src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.h | 37 ++++++--- Marlin/src/HAL/shared/servo.cpp | 2 +- Marlin/src/HAL/shared/servo.h | 2 + 4 files changed, 103 insertions(+), 20 deletions(-) diff --git a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.cpp b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.cpp index fe293d50d4..60cdfbf2f3 100644 --- a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.cpp +++ b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.cpp @@ -27,22 +27,88 @@ #if HAS_SERVOS +uint8_t ServoCount; //=0 + #include "HAL_Servo_Stm32f1.h" -int8_t libServo::attach(const int pin) { - if (this->servoIndex >= MAX_SERVOS) return -1; - return Servo::attach(pin); +//#include "Servo.h" + +#include +#include +#include +#include + +/** + * 20 millisecond period config. For a 1-based prescaler, + * + * (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec + * => prescaler * overflow = 20 * CYC_MSEC + * + * This uses the smallest prescaler that allows an overflow < 2^16. + */ +#define MAX_OVERFLOW ((1 << 16) - 1) +#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) +#define TAU_MSEC 20 +#define TAU_USEC (TAU_MSEC * 1000) +#define TAU_CYC (TAU_MSEC * CYC_MSEC) +#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) +#define SERVO_OVERFLOW ((uint16)round((double)TAU_CYC / SERVO_PRESCALER)) + +// Unit conversions +#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) +#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) +#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \ + SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW))) +#define US_TO_ANGLE(us) ((int16)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, \ + this->minAngle, this->maxAngle))) + +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; + + this->pin = pin; + this->minAngle = minAngle; + this->maxAngle = maxAngle; + + timer_dev *tdev = PIN_MAP[this->pin].timer_device; + uint8 tchan = PIN_MAP[this->pin].timer_channel; + + pinMode(this->pin, PWM); + pwmWrite(this->pin, 0); + + timer_pause(tdev); + timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based + timer_set_reload(tdev, SERVO_OVERFLOW); + timer_generate_update(tdev); + timer_resume(tdev); + + return true; } -int8_t libServo::attach(const int pin, const int min, const int max) { - return Servo::attach(pin, min, max); +bool libServo::detach() { + if (!this->attached()) return false; + pwmWrite(this->pin, 0); + return true; } -void libServo::move(const int value) { +int32_t libServo::read() const { + if (this->attached()) { + timer_dev *tdev = PIN_MAP[this->pin].timer_device; + uint8 tchan = PIN_MAP[this->pin].timer_channel; + return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan))); + } + return 0; +} + +void libServo::move(const int32_t value) { constexpr uint16_t servo_delay[] = SERVO_DELAY; static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); - if (this->attach(0) >= 0) { - this->write(value); + + if (this->attached()) { + pwmWrite(this->pin, US_TO_COMPARE(ANGLE_TO_US(constrain(value, this->minAngle, this->maxAngle)))); safe_delay(servo_delay[this->servoIndex]); #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE) this->detach(); diff --git a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.h b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.h index c27f7bd076..778269361f 100644 --- a/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.h +++ b/Marlin/src/HAL/HAL_STM32F1/HAL_Servo_Stm32f1.h @@ -24,19 +24,34 @@ #ifndef HAL_SERVO_STM32F1_H #define HAL_SERVO_STM32F1_H -// Path needed, otherwise HAL version is used -#include <../../libraries/Servo/src/Servo.h> +// Pin number of unattached pins +#define NOT_ATTACHED (-1) +#define INVALID_SERVO 255 -// Inherit and expand on the official library -class libServo : public Servo { -public: - int8_t attach(const int pin); - int8_t attach(const int pin, const int min, const int max); - void move(const int value); -private: - uint16_t min_ticks; - uint16_t max_ticks; +#ifndef MAX_SERVOS + #define MAX_SERVOS 3 +#endif + +#define SERVO_DEFAULT_MIN_PW 544 +#define SERVO_DEFAULT_MAX_PW 2400 +#define SERVO_DEFAULT_MIN_ANGLE 0 +#define SERVO_DEFAULT_MAX_ANGLE 180 + +#define HAL_SERVO_LIB libServo + +class libServo { + public: + libServo(); + bool attach(const int32_t pin, const int32_t minAngle=SERVO_DEFAULT_MIN_ANGLE, const int32_t maxAngle=SERVO_DEFAULT_MAX_ANGLE); + bool attached() const { return this->pin != NOT_ATTACHED; } + bool detach(); + void move(const int32_t value); + int32_t read() const; + private: uint8_t servoIndex; // index into the channel data for this servo + int32_t pin = NOT_ATTACHED; + int32_t minAngle; + int32_t maxAngle; }; #endif // HAL_SERVO_STM32F1_H diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index 2c6be5e1f2..82b7b9ade6 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -53,7 +53,7 @@ #include "../../inc/MarlinConfig.h" -#if HAS_SERVOS && !(IS_32BIT_TEENSY || defined(TARGET_LPC1768) || defined(STM32F4) || defined(STM32F4xx)) +#if HAS_SERVOS && !(IS_32BIT_TEENSY || defined(TARGET_LPC1768) || defined(STM32F1) || defined(STM32F1xx) || defined(STM32F4) || defined(STM32F4xx)) //#include #include "servo.h" diff --git a/Marlin/src/HAL/shared/servo.h b/Marlin/src/HAL/shared/servo.h index e1a57485b0..16eefa5190 100644 --- a/Marlin/src/HAL/shared/servo.h +++ b/Marlin/src/HAL/shared/servo.h @@ -74,6 +74,8 @@ #elif defined(TARGET_LPC1768) #include "../HAL_LPC1768/LPC1768_Servo.h" +#elif defined(STM32F1) || defined(STM32F1xx) + #include "../HAL_STM32F1/HAL_Servo_STM32F1.h" #elif defined(STM32F4) || defined(STM32F4xx) #include "../HAL_STM32F4/HAL_Servo_STM32F4.h" #else