bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

🧑‍💻 Misc. servo code cleanup

Browse Source
This commit is contained in:
Scott Lahteine 2022-06-09 00:51:47 -05:00 committed by Scott Lahteine
parent ea22640d78
commit 1156557a47
6 changed files with 32 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/src/HAL/AVR/Servo.cpp
View File

@ -66,7 +66,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
/************ static functions common to all instances ***********************/ /************ static functions common to all instances ***********************/
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
if (Channel[timer] < 0) if (Channel[timer] < 0)
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else { else {

21
Marlin/src/HAL/DUE/Servo.cpp
View File

@ -52,7 +52,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
// ------------------------ // ------------------------
/// Interrupt handler for the TC0 channel 1. /// Interrupt handler for the TC0 channel 1.
// ------------------------ // ------------------------
void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t);
#ifdef _useTimer1 #ifdef _useTimer1
void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); } void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); }
@ -70,7 +70,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); } void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); }
#endif #endif
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) {
// clear interrupt // clear interrupt
tc->TC_CHANNEL[channel].TC_SR; tc->TC_CHANNEL[channel].TC_SR;
if (Channel[timer] < 0) if (Channel[timer] < 0)
@ -113,26 +113,21 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) {
TC_Start(tc, channel); TC_Start(tc, channel);
} }
void initISR(timer16_Sequence_t timer) { void initISR(const timer16_Sequence_t timer) {
#ifdef _useTimer1 #ifdef _useTimer1
if (timer == _timer1) if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
_initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
#endif #endif
#ifdef _useTimer2 #ifdef _useTimer2
if (timer == _timer2) if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
_initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
#endif #endif
#ifdef _useTimer3 #ifdef _useTimer3
if (timer == _timer3) if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
_initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
#endif #endif
#ifdef _useTimer4 #ifdef _useTimer4
if (timer == _timer4) if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
_initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
#endif #endif
#ifdef _useTimer5 #ifdef _useTimer5
if (timer == _timer5) if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
_initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
#endif #endif
} }

4
Marlin/src/HAL/SAMD51/Servo.cpp
View File

@ -124,7 +124,7 @@ HAL_SERVO_TIMER_ISR() {
} }
} }
void initISR(timer16_Sequence_t timer) { void initISR(const timer16_Sequence_t timer) {
Tc * const tc = timer_config[SERVO_TC].pTc; Tc * const tc = timer_config[SERVO_TC].pTc;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer); const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
@ -201,7 +201,7 @@ void initISR(timer16_Sequence_t timer) {
} }
} }
void finISR(timer16_Sequence_t timer) { void finISR(const timer16_Sequence_t timer) {
Tc * const tc = timer_config[SERVO_TC].pTc; Tc * const tc = timer_config[SERVO_TC].pTc;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer); const uint8_t tcChannel = TIMER_TCCHANNEL(timer);

16
Marlin/src/HAL/STM32F1/Servo.cpp
View File

@ -147,17 +147,17 @@ void libServo::move(const int32_t value) {
uint16_t SR = timer_get_status(tdev); uint16_t SR = timer_get_status(tdev);
if (SR & TIMER_SR_CC1IF) { // channel 1 off if (SR & TIMER_SR_CC1IF) { // channel 1 off
#ifdef SERVO0_PWM_OD #ifdef SERVO0_PWM_OD
OUT_WRITE_OD(SERVO0_PIN, 1); // off OUT_WRITE_OD(SERVO0_PIN, HIGH); // off
#else #else
OUT_WRITE(SERVO0_PIN, 0); OUT_WRITE(SERVO0_PIN, LOW);
#endif #endif
timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT); timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT);
} }
if (SR & TIMER_SR_CC2IF) { // channel 2 resume if (SR & TIMER_SR_CC2IF) { // channel 2 resume
#ifdef SERVO0_PWM_OD #ifdef SERVO0_PWM_OD
OUT_WRITE_OD(SERVO0_PIN, 0); // on OUT_WRITE_OD(SERVO0_PIN, LOW); // on
#else #else
OUT_WRITE(SERVO0_PIN, 1); OUT_WRITE(SERVO0_PIN, HIGH);
#endif #endif
timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT); timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT);
} }
@ -167,9 +167,9 @@ void libServo::move(const int32_t value) {
timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0);
if (!tdev) return false; if (!tdev) return false;
#ifdef SERVO0_PWM_OD #ifdef SERVO0_PWM_OD
OUT_WRITE_OD(inPin, 1); OUT_WRITE_OD(inPin, HIGH);
#else #else
OUT_WRITE(inPin, 0); OUT_WRITE(inPin, LOW);
#endif #endif
timer_pause(tdev); timer_pause(tdev);
@ -200,9 +200,9 @@ void libServo::move(const int32_t value) {
timer_disable_irq(tdev, 1); timer_disable_irq(tdev, 1);
timer_disable_irq(tdev, 2); timer_disable_irq(tdev, 2);
#ifdef SERVO0_PWM_OD #ifdef SERVO0_PWM_OD
OUT_WRITE_OD(pin, 1); // off OUT_WRITE_OD(pin, HIGH); // off
#else #else
OUT_WRITE(pin, 0); OUT_WRITE(pin, LOW);
#endif #endif
} }
} }

13
Marlin/src/HAL/shared/servo.cpp
View File

@ -65,7 +65,7 @@ uint8_t ServoCount = 0; // the total number of attached
/************ static functions common to all instances ***********************/ /************ static functions common to all instances ***********************/
static boolean isTimerActive(timer16_Sequence_t timer) { static bool anyTimerChannelActive(const timer16_Sequence_t timer) {
// returns true if any servo is active on this timer // returns true if any servo is active on this timer
LOOP_L_N(channel, SERVOS_PER_TIMER) { LOOP_L_N(channel, SERVOS_PER_TIMER) {
if (SERVO(timer, channel).Pin.isActive) if (SERVO(timer, channel).Pin.isActive)
@ -101,17 +101,18 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) {
max = (MAX_PULSE_WIDTH - inMax) / 4; max = (MAX_PULSE_WIDTH - inMax) / 4;
// initialize the timer if it has not already been initialized // initialize the timer if it has not already been initialized
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) initISR(timer); if (!anyTimerChannelActive(timer)) initISR(timer);
servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive
return servoIndex; return servoIndex;
} }
void Servo::detach() { void Servo::detach() {
servo_info[servoIndex].Pin.isActive = false; servo_info[servoIndex].Pin.isActive = false;
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) finISR(timer); if (!anyTimerChannelActive(timer)) finISR(timer);
//pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input
} }
void Servo::write(int value) { void Servo::write(int value) {

12
Marlin/src/HAL/shared/servo_private.h
View File

@ -70,10 +70,10 @@
#define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond()) #define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond())
// convenience macros // convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo #define SERVO_INDEX_TO_TIMER(_servo_nbr) timer16_Sequence_t(_servo_nbr / (SERVOS_PER_TIMER)) // the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // returns the index of the servo on this timer #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // macro to access servo index by timer and channel #define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // servo index by timer and channel
#define SERVO(_timer,_channel) (servo_info[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel #define SERVO(_timer,_channel) servo_info[SERVO_INDEX(_timer,_channel)] // servo class by timer and channel
// Types // Types
@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS];
// Public functions // Public functions
extern void initISR(timer16_Sequence_t timer); extern void initISR(const timer16_Sequence_t timer);
extern void finISR(timer16_Sequence_t timer); extern void finISR(const timer16_Sequence_t timer);