void Servo::detach()
{
servos[this->servoIndex].Pin.isActive = false;
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if(isTimerActive(timer) == false) {
finISR(timer);
}
}
void Servo::detach()
{
ServoTimerSequence timerId;
s_servos[_servoIndex].info.isActive = false;
timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
if (!isTimerActive(timerId)) {
finISR(timerId);
}
}
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();
}
}