A simple library for driving RC servos without using the mbed's PWM functions. This allows the mbed to drive as many servos as there are DigitalOut pins, and additionally allows for the PWM functions to be used at a different frequency than the 50Hz used for servos.
Diff: Servo.cpp
- Revision:
- 9:6bfea9af4dcb
- Parent:
- 8:131785ed96fb
- Child:
- 10:f0df63e99a96
diff -r 131785ed96fb -r 6bfea9af4dcb Servo.cpp --- a/Servo.cpp Wed Oct 03 07:28:03 2012 +0000 +++ b/Servo.cpp Sun Oct 21 00:03:06 2012 +0000 @@ -17,9 +17,12 @@ // the initial pulse width can be omitted, giving a default of // 1500 us. // This should be at about half the range of most servos. -Servo::Servo(PinName pin, unsigned int width) : signalPin(pin) +Servo::Servo(PinName pin) : signalPin(pin) { - pulseWidth = width; + // Set default calibration + calibrate(2000, 1000, 60.f, -60.f); + + pulseWidth = center; if (numServos == 0) { @@ -34,7 +37,36 @@ -void Servo::write(int width) +bool Servo::calibrate(unsigned int plus60, unsigned int minus60, float upperLimit, float lowerLimit) +{ + // Check if given parameters are valid + if (plus60 > minus60 && upperLimit > lowerLimit) + { + center = (plus60 + minus60) / 2; + usPerDegree = (plus60 - center) / 60.f; + this->upperLimit = upperLimit; + this->lowerLimit = lowerLimit; + return true; + } + else + { + return false; + } +} + + + +void Servo::write(float degrees) +{ + // Limit to the valid angle range + degrees = (degrees > upperLimit ? upperLimit : (degrees < lowerLimit ? lowerLimit : degrees)); + + pulseWidth = center + (int)(degrees * usPerDegree); +} + + + +void Servo::writeWidth(unsigned int width) { // Make sure that the pulse width is less than the refresh period pulseWidth = width < period ? width : period; @@ -42,23 +74,37 @@ -int Servo::read() +float Servo::read() +{ + return ((int)pulseWidth - (int)center) / usPerDegree; +} + + + +int Servo::readWidth() { return pulseWidth; } -void Servo::operator=(int width) +void Servo::operator=(float degrees) { - write(width); + write(degrees); +} + + + +Servo::operator float() +{ + return read(); } Servo::operator int() { - return read(); + return readWidth(); }