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`.
This commit is contained in:
jmz52 2018-08-21 05:13:51 +03:00 committed by Scott Lahteine
parent 0456caf0ad
commit a77f8c6dd5
4 changed files with 103 additions and 20 deletions

View file

@ -27,22 +27,88 @@
#if HAS_SERVOS #if HAS_SERVOS
uint8_t ServoCount; //=0
#include "HAL_Servo_Stm32f1.h" #include "HAL_Servo_Stm32f1.h"
int8_t libServo::attach(const int pin) { //#include "Servo.h"
if (this->servoIndex >= MAX_SERVOS) return -1;
return Servo::attach(pin); #include <boards.h>
#include <io.h>
#include <pwm.h>
#include <wirish_math.h>
/**
* 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;
} }
int8_t libServo::attach(const int pin, const int min, const int max) { bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) {
return Servo::attach(pin, min, max); 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;
} }
void libServo::move(const int value) { bool libServo::detach() {
if (!this->attached()) return false;
pwmWrite(this->pin, 0);
return true;
}
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; constexpr uint16_t servo_delay[] = SERVO_DELAY;
static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); 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]); safe_delay(servo_delay[this->servoIndex]);
#if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE) #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
this->detach(); this->detach();

View file

@ -24,19 +24,34 @@
#ifndef HAL_SERVO_STM32F1_H #ifndef HAL_SERVO_STM32F1_H
#define HAL_SERVO_STM32F1_H #define HAL_SERVO_STM32F1_H
// Path needed, otherwise HAL version is used // Pin number of unattached pins
#include <../../libraries/Servo/src/Servo.h> #define NOT_ATTACHED (-1)
#define INVALID_SERVO 255
// Inherit and expand on the official library #ifndef MAX_SERVOS
class libServo : public Servo { #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: public:
int8_t attach(const int pin); libServo();
int8_t attach(const int pin, const int min, const int max); bool attach(const int32_t pin, const int32_t minAngle=SERVO_DEFAULT_MIN_ANGLE, const int32_t maxAngle=SERVO_DEFAULT_MAX_ANGLE);
void move(const int value); bool attached() const { return this->pin != NOT_ATTACHED; }
bool detach();
void move(const int32_t value);
int32_t read() const;
private: private:
uint16_t min_ticks;
uint16_t max_ticks;
uint8_t servoIndex; // index into the channel data for this servo 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 #endif // HAL_SERVO_STM32F1_H

View file

@ -53,7 +53,7 @@
#include "../../inc/MarlinConfig.h" #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 <Arduino.h> //#include <Arduino.h>
#include "servo.h" #include "servo.h"

View file

@ -74,6 +74,8 @@
#elif defined(TARGET_LPC1768) #elif defined(TARGET_LPC1768)
#include "../HAL_LPC1768/LPC1768_Servo.h" #include "../HAL_LPC1768/LPC1768_Servo.h"
#elif defined(STM32F1) || defined(STM32F1xx)
#include "../HAL_STM32F1/HAL_Servo_STM32F1.h"
#elif defined(STM32F4) || defined(STM32F4xx) #elif defined(STM32F4) || defined(STM32F4xx)
#include "../HAL_STM32F4/HAL_Servo_STM32F4.h" #include "../HAL_STM32F4/HAL_Servo_STM32F4.h"
#else #else