Exemple #1
0
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;
}
Exemple #2
0
  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
      }
  }
Exemple #3
0
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;
    }
}
Exemple #4
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
  }
     

}
Exemple #6
0
//************************************************************************
// 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
  }
}
Exemple #8
0
//------------------------------------------------------------------------------
// 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();
}
Exemple #9
0
// 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;
}
Exemple #10
0
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
  }
}
Exemple #11
0
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;
}
Exemple #12
0
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();
    }
}