andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

STM32F1: Servo "soft" PWM via timer interrupt (#14187)

pull/1/head
Tanguy Pruvot 5 years ago
committed by Scott Lahteine
parent
66e22d9f5a
commit
764f0d9c1c
3 changed files with 124 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 122
      Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp
  2. 7
      Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h
  3. 2
      Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h

122
Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.cpp
View File

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

7
Marlin/src/HAL/HAL_STM32F1/HAL_Servo_STM32F1.h
View File

@ -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);
};

2
Marlin/src/HAL/HAL_STM32F1/fastio_STM32F1.h
View File

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

Write Preview
Loading…
Cancel
Save