Skip to content

Commit

Permalink
Merge pull request esp8266#1231 from Makuna/ServoDetachFixes
Browse files Browse the repository at this point in the history 
Fix detach and attach
  • Loading branch information
@igrr
igrr committed Dec 18, 2015
2 parents 695583e + 2425ee3 commit fa485cd
Showing 1 changed file with 59 additions and 31 deletions.
90 changes: 59 additions & 31 deletions libraries/Servo/src/esp8266/Servo.cpp
View file
Original file line number Diff line number Diff line change
@@ -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)

Expand All @@ -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 {
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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;
Expand All @@ -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;
}
}

Expand Down

0 comments on commit fa485cd

Please sign in to comment.