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

🐛 Resolve DUE Servo pulse issue (#24305)

Browse Source 
Co-authored-by: sjasonsmith <20053467+sjasonsmith@users.noreply.github.com>
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
InsanityAutomation
2022-06-21 22:09:24 -04:00
committed by Scott Lahteine
parent 0523874e9c
commit 0dc59311ec
9 changed files with 205 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,7 +77,8 @@ HAL_SERVO_TIMER_ISR() {
;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
if (currentServoIndex[timer] < 0) {
int8_t cho = currentServoIndex[timer]; // Handle the prior servo first
if (cho < 0) { // Servo -1 indicates the refresh interval completed...
#if defined(_useTimer1) && defined(_useTimer2)
if (currentServoIndex[timer ^ 1] >= 0) {
// Wait for both channels
@ -86,41 +87,33 @@ HAL_SERVO_TIMER_ISR() {
return;
}
#endif
tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL;
tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; // ...so reset the timer
SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT);
}
else if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive)
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
// Select the next servo controlled by this timer
currentServoIndex[timer]++;
currentServoIndex[timer] = ++cho; // go to the next channel (or 0)
if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
if (SERVO(timer, cho).Pin.isActive) // activated?
digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive) // check if activated
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, currentServoIndex[timer]).ticks;
tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks;
}
else {
// finished all channels so wait for the refresh period to expire before starting over
currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
const uint16_t tcCounterValue = getTimerCount();
if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - (uint16_t)usToTicks(REFRESH_INTERVAL);
else
tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed
currentServoIndex[timer] = -1; // reset the timer COUNT.reg on the next call
const uint16_t cval = getTimerCount() - 256 / (SERVO_TIMER_PRESCALER), // allow 256 cycles to ensure the next CV not missed
ival = (TC_COUNTER_START_VAL) - (uint16_t)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
tc->COUNT16.CC[tcChannel].reg = min(cval, ival);
}
if (tcChannel == 0) {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC0);
// Clear the interrupt
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0;
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; // Clear the interrupt
}
else {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC1);
// Clear the interrupt
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1;
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; // Clear the interrupt
}
}
@ -201,9 +194,9 @@ void initISR(const timer16_Sequence_t timer) {
}
}
void finISR(const timer16_Sequence_t timer) {
void finISR(const timer16_Sequence_t timer_index) {
Tc * const tc = timer_config[SERVO_TC].pTc;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
const uint8_t tcChannel = TIMER_TCCHANNEL(timer_index);
// Disable the match channel interrupt request
tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1;