arduino/esp8266
1
0
Fork 0
mirror of https://github.com/esp8266/Arduino.git synced 2025-04-21 10:26:06 +03:00
Code

Merge pull request #1231 from Makuna/ServoDetachFixes

Browse Source 
Fix detach and attach
This commit is contained in:
Ivan Grokhotkov 2015-12-18 08:45:37 +03:00
commit fa485cd88f
1 changed files with 59 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/Servo/src/esp8266/Servo.cpp
View File

@ -26,9 +26,12 @@
const uint32_t c_CycleCompensation = 4; // compensation us to trim adjust for digitalWrite delays 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 { 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 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 { struct ServoState {
@ -76,18 +79,37 @@ static void Servo_Handler(T* timer)
if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) { if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) {
// pulse this channel low if activated // pulse this channel low if activated
digitalWrite(s_servos[servoIndex].info.pin, LOW); 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(); timer->nextChannel();
} }
servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel()); 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); timer->SetPulseCompare(timer->usToTicks(s_servos[servoIndex].usPulse) - c_CycleCompensation);
if (s_servos[servoIndex].info.isActive) { // check if activated if (s_servos[servoIndex].info.isActive) {
digitalWrite(s_servos[servoIndex].info.pin, HIGH); // its an active channel so pulse it high 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 { else {
// finished all channels so wait for the refresh period to expire before starting over // finished all channels so wait for the refresh period to expire before starting over
@ -100,6 +122,7 @@ static void Servo_Handler(T* timer)
// at least REFRESH_INTERVAL has elapsed // at least REFRESH_INTERVAL has elapsed
timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2); timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2);
} }
}
timer->setEndOfCycle(); timer->setEndOfCycle();
} }
@ -166,6 +189,10 @@ Servo::Servo()
// set default _minUs and _maxUs incase write() is called before attach() // set default _minUs and _maxUs incase write() is called before attach()
_minUs = MIN_PULSE_WIDTH; _minUs = MIN_PULSE_WIDTH;
_maxUs = MAX_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 { else {
_servoIndex = INVALID_SERVO; // too many servos _servoIndex = INVALID_SERVO; // too many servos
@ -182,9 +209,11 @@ uint8_t Servo::attach(int pin, int minUs, int maxUs)
ServoTimerSequence timerId; ServoTimerSequence timerId;
if (_servoIndex < MAX_SERVOS) { if (_servoIndex < MAX_SERVOS) {
if (s_servos[_servoIndex].info.pin == INVALID_PIN) {
pinMode(pin, OUTPUT); // set servo pin to output pinMode(pin, OUTPUT); // set servo pin to output
digitalWrite(pin, LOW); digitalWrite(pin, LOW);
s_servos[_servoIndex].info.pin = pin; s_servos[_servoIndex].info.pin = pin;
}
// keep the min and max within 200-3000 us, these are extreme // keep the min and max within 200-3000 us, these are extreme
// ranges and should support extreme servos while maintaining // 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)) { if (!isTimerActive(timerId)) {
initISR(timerId); initISR(timerId);
} }
s_servos[_servoIndex].info.isDetaching = false;
s_servos[_servoIndex].info.isActive = true; // this must be set after the check for isTimerActive s_servos[_servoIndex].info.isActive = true; // this must be set after the check for isTimerActive
} }
return _servoIndex; return _servoIndex;
@ -206,10 +236,8 @@ void Servo::detach()
{ {
ServoTimerSequence timerId; ServoTimerSequence timerId;
s_servos[_servoIndex].info.isActive = false; if (s_servos[_servoIndex].info.isActive) {
timerId = SERVO_INDEX_TO_TIMER(_servoIndex); s_servos[_servoIndex].info.isDetaching = true;
if (!isTimerActive(timerId)) {
finISR(timerId);
} }
} }