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