bool Servo_Handler_1(void*) { timer16_Sequence_t timer = _timer2; if (Channel[timer] < 0) { //Timers.periodicHz(10000000, Servo_Handler_0, 0, 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 == true) { digitalWrite(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) { Timers.timer(timer)->setFrequency(usToHz(ticksToUs(SERVO(timer,Channel[timer]).ticks))); if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high } } else { Timers.timer(timer)->setFrequency(usToHz(REFRESH_INTERVAL)); Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel } return true; }
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { // clear interrupt tc->TC_CHANNEL[channel].TC_SR; 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 { if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) { digitalWrite(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) { tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks; if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high } } else { // finished all channels so wait for the refresh period to expire before starting over if( (tc->TC_CHANNEL[channel].TC_CV) + 4 < usToTicks(REFRESH_INTERVAL) ) { // allow a few ticks to ensure the next OCR1A not missed tc->TC_CHANNEL[channel].TC_RA = (unsigned int)usToTicks(REFRESH_INTERVAL); } else { tc->TC_CHANNEL[channel].TC_RA = 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 } }
static void handle_interrupts(timer16_Sequence_t timer) { if( Channel[timer] == 0 ){ if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ){ digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // pulse this channel0 high if activated } } else if( Channel[timer] >= 1 ){ if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ){ digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // pulse this channel high if activated } if( SERVO_INDEX(timer,Channel[timer]-1) < ServoCount && SERVO(timer,Channel[timer]-1).Pin.isActive == true ){ digitalWrite( SERVO(timer,Channel[timer]-1).Pin.nbr,LOW); // pulse this channel low if activated } } Channel[timer]++; // increment to the next channel if( SERVO_INDEX(timer,Channel[timer]) < ServoCount+1 && Channel[timer] < SERVOS_PER_TIMER){ SRV_TDRxx = SERVO(timer,Channel[timer]).ticks; totalTicks += SERVO(timer,Channel[timer]).ticks; } else{ // finished all channels so wait for the refresh period to expire before starting over if( totalTicks + 4 < usToTicks(REFRESH_INTERVAL)){ // allow a few ticks to ensure the next OCR1A not missed SRV_TDRxx = (unsigned int)usToTicks(REFRESH_INTERVAL); } else{ SRV_TDRxx = 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 totalTicks = 0; } }
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) { 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 == true ) { servo_t &servo = SERVO(timer,Channel[timer]); servo.line_driver->lineWrite(servo.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) { *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated { servo_t &servo = SERVO(timer,Channel[timer]); servo.line_driver->lineWrite(servo.Pin.nbr,HIGH); // its 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 < (usToTicks(REFRESH_INTERVAL) + 4) ) // allow a few ticks to ensure the next OCR1A not missed *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); 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 } }
void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PR) { if( channel[timer] < 0 ) *TMRn = 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 == true ) digitalWrite( 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) { *PR = *TMRn + SERVO(timer,channel[timer]).ticks; if(SERVO(timer,channel[timer]).Pin.isActive == true) // check if activated digitalWrite( SERVO(timer,channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high } else { *TMRn = channel[timer] * usToTicks(MAX_PULSE_WIDTH); // finished all channels so wait for the refresh period to expire before starting over if( (unsigned)*TMRn < (usToTicks(REFRESH_INTERVAL) + 4) ) // allow a few ticks to ensure the next OCR1A not missed *PR = (unsigned int)usToTicks(REFRESH_INTERVAL); else *PR = *TMRn + 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 } }
//************************************************************************ // Note: PIC32 timers (TMRn) reset to zero when they match PRn. void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PRn) { static uint32_t AccumulatedTicks[3] = {0,0,0}; // Store the number of ticks since the first rising edge for this timer // Test for invalid timer number if (timer >= 3) { return; } // If this value is -1, then we have just finished with the time from the end of the last pulse // and need to start with the first servo index on this timer again. if ( channel[timer] < 0 ) { AccumulatedTicks[timer] = 0; // Clear the accumulated time for this timer on first rising edge } else { // If this is not the first pulse, then set the old pin low if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && SERVO(timer,channel[timer]).Pin.isActive == true ) { digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated } } channel[timer]++; // increment to the next channel // If we have not run out of channels (on this timer), if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && channel[timer] < SERVOS_PER_TIMER) { // Then set the time we want to fire next (the width for this channel) *PRn = SERVO(timer,channel[timer]).ticks; // Set this channel's pin high if its active if (SERVO(timer,channel[timer]).Pin.isActive == true) // check if activated { digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high } AccumulatedTicks[timer] += *PRn; // Add the time we are about to spend on this channel } else { // Otherwise, finished all channels so next fire needs to be at REFRESH_INTERVAL - AccumulatedTicks if (AccumulatedTicks[timer] < (usToTicks(REFRESH_INTERVAL) - 4)) { *PRn = (unsigned int)(usToTicks(REFRESH_INTERVAL)) - AccumulatedTicks[timer]; } else { *PRn = *TMRn + 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 } }
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) { 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 == true ) digitalWrite( 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) { // Extension for slowmove if (SERVO(timer,Channel[timer]).speed) { // Increment ticks by speed until we reach the target. // When the target is reached, speed is set to 0 to disable that code. if (SERVO(timer,Channel[timer]).target > SERVO(timer,Channel[timer]).ticks) { SERVO(timer,Channel[timer]).ticks += SERVO(timer,Channel[timer]).speed; if (SERVO(timer,Channel[timer]).target <= SERVO(timer,Channel[timer]).ticks) { SERVO(timer,Channel[timer]).ticks = SERVO(timer,Channel[timer]).target; SERVO(timer,Channel[timer]).speed = 0; } } else { SERVO(timer,Channel[timer]).ticks -= SERVO(timer,Channel[timer]).speed; if (SERVO(timer,Channel[timer]).target >= SERVO(timer,Channel[timer]).ticks) { SERVO(timer,Channel[timer]).ticks = SERVO(timer,Channel[timer]).target; SERVO(timer,Channel[timer]).speed = 0; } } } // End of Extension for slowmove //original *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its 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); 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 } }
//------------------------------------------------------------------------------ // Interrupt handler template method that takes a class that implements // a standard set of methods for the timer abstraction //------------------------------------------------------------------------------ template <class T> void Servo_Handler(T* timer) { noInterrupts(); uint8_t servoIndex; // clear interrupt timer->ResetInterrupt(); if (timer->isEndOfCycle()) { timer->StartCycle(); } else { servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel()); if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) { // pulse this channel low if activated digitalWrite(s_servos[servoIndex].info.pin, LOW); } timer->nextChannel(); } servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel()); if (servoIndex < s_servoCount && timer->getCurrentChannel() < SERVOS_PER_TIMER) { timer->SetPulseCompare(timer->usToTicks(s_servos[servoIndex].usPulse) - c_CycleCompensation); if (s_servos[servoIndex].info.isActive) { // check if activated digitalWrite(s_servos[servoIndex].info.pin, HIGH); // its an active channel so pulse it high } } else { // finished all channels so wait for the refresh period to expire before starting over // allow a few ticks to ensure the next match is not missed uint32_t refreshCompare = timer->usToTicks(REFRESH_INTERVAL); if ((timer->GetCycleCount() + c_CycleCompensation * 2) < refreshCompare) { timer->SetCycleCompare(refreshCompare - c_CycleCompensation); } else { // at least REFRESH_INTERVAL has elapsed timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2); } timer->setEndOfCycle(); } interrupts(); }
// returns true if any servo is active on this timer static boolean isTimerActive(ServoTimerSequence timerId) { for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++) { if (s_servos[SERVO_INDEX(timerId, channel)].info.isActive) { return true; } } return false; }
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) { register servo_t *pservo; if( Channel[timer] < 0 ) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer else{ pservo = &SERVO(timer,Channel[timer]); if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && pservo->Pin.isActive == TRUE ) { digitalWrite( pservo->Pin.nbr,LOW); // pulse this channel low if activated // See if we are in a timed move, if so update the move for the next time through... if (pservo->ticksDelta > 0) { pservo->ticks +=pservo->ticksDelta; if (pservo->ticks >= pservo->ticksNew) { pservo->ticks = pservo->ticksNew; pservo->ticksDelta = 0; } } else if (pservo->ticksDelta < 0) { pservo->ticks +=pservo->ticksDelta; if (pservo->ticks <= pservo->ticksNew) { pservo->ticks = pservo->ticksNew; pservo->ticksDelta = 0; } } } } Channel[timer]++; // increment to the next channel if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { pservo = &SERVO(timer,Channel[timer]); *OCRnA = *TCNTn + pservo->ticks; if(pservo->Pin.isActive == TRUE) // check if activated digitalWrite( pservo->Pin.nbr,HIGH); // its 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); 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 } }
void ServoHandler(int timer) { if (currentServoIndex[timer] < 0) { // Write compare register _timer->CCMP = 0; } else { if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated } } // Select the next servo controlled by this timer currentServoIndex[timer]++; if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) { if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high } // Get the counter value uint16_t tcCounterValue = 0; //_timer->CCMP; _timer->CCMP = (uint16_t) (tcCounterValue + SERVO(timer, currentServoIndex[timer]).ticks); } else { // finished all channels so wait for the refresh period to expire before starting over // Get the counter value uint16_t tcCounterValue = _timer->CCMP; if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) { // allow a few ticks to ensure the next OCR1A not missed _timer->CCMP = (uint16_t) usToTicks(REFRESH_INTERVAL); } else { _timer->CCMP = (uint16_t) (tcCounterValue + 4UL); // at least REFRESH_INTERVAL has elapsed } currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel } /* Clear flag */ _timer->INTFLAGS = TCB_CAPT_bm; }
static void Servo_Handler(T* timer) { uint8_t servoIndex; // clear interrupt timer->ResetInterrupt(); if (timer->isEndOfCycle()) { timer->StartCycle(); } else { servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel()); if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) { // pulse this channel low if activated digitalWrite(s_servos[servoIndex].info.pin, LOW); if (s_servos[servoIndex].info.isDetaching) { s_servos[servoIndex].info.isActive = false; s_servos[servoIndex].info.isDetaching = false; } } timer->nextChannel(); } servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel()); if (servoIndex < s_servoCount && timer->getCurrentChannel() < SERVOS_PER_TIMER) { timer->SetPulseCompare(timer->usToTicks(s_servos[servoIndex].usPulse) - c_CycleCompensation); if (s_servos[servoIndex].info.isActive) { if (s_servos[servoIndex].info.isDetaching) { // it was active, reset state and leave low s_servos[servoIndex].info.isActive = false; s_servos[servoIndex].info.isDetaching = false; } else { // its an active channel so pulse it high digitalWrite(s_servos[servoIndex].info.pin, HIGH); } } } else { if (!isTimerActive(timer->timerId())) { // no active running channels on this timer, stop the ISR finISR(timer->timerId()); } else { // finished all channels so wait for the refresh period to expire before starting over // allow a few ticks to ensure the next match is not missed uint32_t refreshCompare = timer->usToTicks(REFRESH_INTERVAL); if ((timer->GetCycleCount() + c_CycleCompensation * 2) < refreshCompare) { timer->SetCycleCompare(refreshCompare - c_CycleCompensation); } else { // at least REFRESH_INTERVAL has elapsed timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2); } } timer->setEndOfCycle(); } }