From 2425ee34134ac90923341323a8754ae0ca9c3787 Mon Sep 17 00:00:00 2001 From: Makuna Date: Wed, 16 Dec 2015 15:17:53 -0800 Subject: [PATCH] Fix detach and attach detaching could leave a running timer which would cause issues. Further, detaching could truncate the last pulse causing a invalid movement. This change moves detaching into the ISR so it can maintain the last pulse and not start any more timers if not needed. --- libraries/Servo/src/esp8266/Servo.cpp | 90 ++++++++++++++++++--------- 1 file changed, 59 insertions(+), 31 deletions(-) diff --git a/libraries/Servo/src/esp8266/Servo.cpp b/libraries/Servo/src/esp8266/Servo.cpp index 637265cd3..5c5b1c7ea 100644 --- a/libraries/Servo/src/esp8266/Servo.cpp +++ b/libraries/Servo/src/esp8266/Servo.cpp @@ -1,20 +1,20 @@ /* - Copyright (c) 2015 Michael C. Miller. All right reserved. +Copyright (c) 2015 Michael C. Miller. All right reserved. - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - */ +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ #if defined(ESP8266) @@ -26,9 +26,12 @@ const uint32_t c_CycleCompensation = 4; // compensation us to trim adjust for digitalWrite delays +#define INVALID_PIN 63 // flag indicating never attached servo + struct ServoInfo { - uint8_t pin : 6; // a pin number from 0 to 63 + uint8_t pin : 6; // a pin number from 0 to 62, 63 reserved uint8_t isActive : 1; // true if this channel is enabled, pin not pulsed if false + uint8_t isDetaching : 1; // true if this channel is being detached, maintains pulse integrity }; struct ServoState { @@ -76,29 +79,49 @@ static void Servo_Handler(T* timer) 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) { + 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 + 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 { - // 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); + if (!isTimerActive(timer->timerId())) { + // no active running channels on this timer, stop the ISR + finISR(timer->timerId()); } else { - // at least REFRESH_INTERVAL has elapsed - timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2); + // 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(); @@ -166,6 +189,10 @@ Servo::Servo() // set default _minUs and _maxUs incase write() is called before attach() _minUs = MIN_PULSE_WIDTH; _maxUs = MAX_PULSE_WIDTH; + + s_servos[_servoIndex].info.isActive = false; + s_servos[_servoIndex].info.isDetaching = false; + s_servos[_servoIndex].info.pin = INVALID_PIN; } else { _servoIndex = INVALID_SERVO; // too many servos @@ -182,9 +209,11 @@ uint8_t Servo::attach(int pin, int minUs, int maxUs) ServoTimerSequence timerId; if (_servoIndex < MAX_SERVOS) { - pinMode(pin, OUTPUT); // set servo pin to output - digitalWrite(pin, LOW); - s_servos[_servoIndex].info.pin = pin; + if (s_servos[_servoIndex].info.pin == INVALID_PIN) { + pinMode(pin, OUTPUT); // set servo pin to output + digitalWrite(pin, LOW); + s_servos[_servoIndex].info.pin = pin; + } // keep the min and max within 200-3000 us, these are extreme // ranges and should support extreme servos while maintaining @@ -197,6 +226,7 @@ uint8_t Servo::attach(int pin, int minUs, int maxUs) if (!isTimerActive(timerId)) { initISR(timerId); } + s_servos[_servoIndex].info.isDetaching = false; s_servos[_servoIndex].info.isActive = true; // this must be set after the check for isTimerActive } return _servoIndex; @@ -206,10 +236,8 @@ void Servo::detach() { ServoTimerSequence timerId; - s_servos[_servoIndex].info.isActive = false; - timerId = SERVO_INDEX_TO_TIMER(_servoIndex); - if (!isTimerActive(timerId)) { - finISR(timerId); + if (s_servos[_servoIndex].info.isActive) { + s_servos[_servoIndex].info.isDetaching = true; } }