jessikitty/Marlin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

keep check for good SERVO_INDEX

Browse Source
This commit is contained in:
Scott Lahteine
2022-06-07 17:08:45 -05:00
parent e7a6c77d74
commit 6566031ec6
2 changed files with 28 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Marlin/src/HAL/AVR/Servo.cpp
+14 -14
View File
@@ -67,26 +67,26 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
/************ static functions common to all instances ***********************/
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount;
if (Channel[timer] < 0)
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else {
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive)
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
Channel[timer]++; // increment to the next channel
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
Channel[timer]++; // increment to the next channel
if (good_servo && Channel[timer] < SERVOS_PER_TIMER) {
*OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks;
if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
// Finished all channels so wait for the refresh period to expire before starting over
const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL);
if (((unsigned)*TCNTn) + 4 < iticks) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = iticks;
else
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel
}
}
Marlin/src/HAL/DUE/Servo.cpp
+14 -15
View File
@@ -71,29 +71,28 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
#endif
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) {
const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount;
if (Channel[timer] < 0)
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
else {
//else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
else if (good_servo)
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
// clear interrupt
tc->TC_CHANNEL[channel].TC_SR;
tc->TC_CHANNEL[channel].TC_SR; // clear interrupt
Channel[timer]++; // increment to the next channel
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
Channel[timer]++; // increment to the next channel
if (good_servo && Channel[timer] < SERVOS_PER_TIMER) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
}
else {
// finished all channels so wait for the refresh period to expire before starting over
const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL);
tc->TC_CHANNEL[channel].TC_RA =
tc->TC_CHANNEL[channel].TC_CV < usToTicks(REFRESH_INTERVAL) - 4
? (unsigned int)usToTicks(REFRESH_INTERVAL) // allow a few ticks to ensure the next OCR1A not missed
: tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
tc->TC_CHANNEL[channel].TC_CV + 4 < iticks
? iticks // allow a few ticks to ensure the next OCR1A not missed
: tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel
}
}