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.
Servo.cpp@12:92dc63d24bf8, 2012-11-03 (annotated)
- Committer:
- pclary
- Date:
- Sat Nov 03 19:10:29 2012 +0000
- Revision:
- 12:92dc63d24bf8
- Parent:
- 10:f0df63e99a96
- Child:
- 14:0eff26aa0d17
Fixed GCC warning
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCSBRobotics | 7:ff85ac12e11b | 1 | #include "Servo.h" |
UCSBRobotics | 7:ff85ac12e11b | 2 | #include "mbed.h" |
UCSBRobotics | 7:ff85ac12e11b | 3 | |
UCSBRobotics | 7:ff85ac12e11b | 4 | |
UCSBRobotics | 7:ff85ac12e11b | 5 | |
UCSBRobotics | 7:ff85ac12e11b | 6 | // The next line determines the maximum number of servo objects |
UCSBRobotics | 7:ff85ac12e11b | 7 | // that can be successfully created. |
UCSBRobotics | 7:ff85ac12e11b | 8 | // By default, this is set to 26, as there are only 26 unique |
UCSBRobotics | 7:ff85ac12e11b | 9 | // DigitalOut pins provided by the mbed. |
UCSBRobotics | 7:ff85ac12e11b | 10 | Servo *Servo::servos[26]; |
UCSBRobotics | 7:ff85ac12e11b | 11 | unsigned int Servo::numServos = 0; |
UCSBRobotics | 7:ff85ac12e11b | 12 | Ticker* Servo::refreshTicker; |
UCSBRobotics | 7:ff85ac12e11b | 13 | |
UCSBRobotics | 7:ff85ac12e11b | 14 | |
UCSBRobotics | 7:ff85ac12e11b | 15 | |
UCSBRobotics | 7:ff85ac12e11b | 16 | // The pin must be specified when the object is initialized, but |
UCSBRobotics | 7:ff85ac12e11b | 17 | // the initial pulse width can be omitted, giving a default of |
UCSBRobotics | 7:ff85ac12e11b | 18 | // 1500 us. |
UCSBRobotics | 7:ff85ac12e11b | 19 | // This should be at about half the range of most servos. |
pclary | 9:6bfea9af4dcb | 20 | Servo::Servo(PinName pin) : signalPin(pin) |
UCSBRobotics | 7:ff85ac12e11b | 21 | { |
pclary | 9:6bfea9af4dcb | 22 | // Set default calibration |
pclary | 9:6bfea9af4dcb | 23 | calibrate(2000, 1000, 60.f, -60.f); |
pclary | 9:6bfea9af4dcb | 24 | |
pclary | 9:6bfea9af4dcb | 25 | pulseWidth = center; |
UCSBRobotics | 7:ff85ac12e11b | 26 | |
UCSBRobotics | 7:ff85ac12e11b | 27 | if (numServos == 0) |
UCSBRobotics | 7:ff85ac12e11b | 28 | { |
UCSBRobotics | 7:ff85ac12e11b | 29 | refreshTicker = new Ticker(); |
UCSBRobotics | 7:ff85ac12e11b | 30 | |
UCSBRobotics | 7:ff85ac12e11b | 31 | // Start the ticker that refreshes all servos |
UCSBRobotics | 7:ff85ac12e11b | 32 | refreshTicker->attach_us(&Servo::refresh, period); |
UCSBRobotics | 7:ff85ac12e11b | 33 | } |
UCSBRobotics | 7:ff85ac12e11b | 34 | |
UCSBRobotics | 7:ff85ac12e11b | 35 | servos[numServos++] = this; |
UCSBRobotics | 7:ff85ac12e11b | 36 | } |
UCSBRobotics | 7:ff85ac12e11b | 37 | |
UCSBRobotics | 7:ff85ac12e11b | 38 | |
UCSBRobotics | 7:ff85ac12e11b | 39 | |
pclary | 9:6bfea9af4dcb | 40 | bool Servo::calibrate(unsigned int plus60, unsigned int minus60, float upperLimit, float lowerLimit) |
pclary | 9:6bfea9af4dcb | 41 | { |
pclary | 9:6bfea9af4dcb | 42 | // Check if given parameters are valid |
pclary | 10:f0df63e99a96 | 43 | if (upperLimit > lowerLimit) |
pclary | 9:6bfea9af4dcb | 44 | { |
pclary | 9:6bfea9af4dcb | 45 | center = (plus60 + minus60) / 2; |
pclary | 10:f0df63e99a96 | 46 | usPerDegree = ((int)plus60 - (int)center) / 60.0f; |
pclary | 9:6bfea9af4dcb | 47 | this->upperLimit = upperLimit; |
pclary | 9:6bfea9af4dcb | 48 | this->lowerLimit = lowerLimit; |
pclary | 9:6bfea9af4dcb | 49 | return true; |
pclary | 9:6bfea9af4dcb | 50 | } |
pclary | 9:6bfea9af4dcb | 51 | else |
pclary | 9:6bfea9af4dcb | 52 | { |
pclary | 9:6bfea9af4dcb | 53 | return false; |
pclary | 9:6bfea9af4dcb | 54 | } |
pclary | 9:6bfea9af4dcb | 55 | } |
pclary | 9:6bfea9af4dcb | 56 | |
pclary | 9:6bfea9af4dcb | 57 | |
pclary | 9:6bfea9af4dcb | 58 | |
pclary | 9:6bfea9af4dcb | 59 | void Servo::write(float degrees) |
pclary | 9:6bfea9af4dcb | 60 | { |
pclary | 9:6bfea9af4dcb | 61 | // Limit to the valid angle range |
pclary | 9:6bfea9af4dcb | 62 | degrees = (degrees > upperLimit ? upperLimit : (degrees < lowerLimit ? lowerLimit : degrees)); |
pclary | 9:6bfea9af4dcb | 63 | |
pclary | 9:6bfea9af4dcb | 64 | pulseWidth = center + (int)(degrees * usPerDegree); |
pclary | 9:6bfea9af4dcb | 65 | } |
pclary | 9:6bfea9af4dcb | 66 | |
pclary | 9:6bfea9af4dcb | 67 | |
pclary | 9:6bfea9af4dcb | 68 | |
pclary | 9:6bfea9af4dcb | 69 | void Servo::writeWidth(unsigned int width) |
UCSBRobotics | 7:ff85ac12e11b | 70 | { |
UCSBRobotics | 7:ff85ac12e11b | 71 | // Make sure that the pulse width is less than the refresh period |
UCSBRobotics | 7:ff85ac12e11b | 72 | pulseWidth = width < period ? width : period; |
UCSBRobotics | 7:ff85ac12e11b | 73 | } |
UCSBRobotics | 7:ff85ac12e11b | 74 | |
UCSBRobotics | 7:ff85ac12e11b | 75 | |
UCSBRobotics | 7:ff85ac12e11b | 76 | |
pclary | 9:6bfea9af4dcb | 77 | float Servo::read() |
pclary | 9:6bfea9af4dcb | 78 | { |
pclary | 9:6bfea9af4dcb | 79 | return ((int)pulseWidth - (int)center) / usPerDegree; |
pclary | 9:6bfea9af4dcb | 80 | } |
pclary | 9:6bfea9af4dcb | 81 | |
pclary | 9:6bfea9af4dcb | 82 | |
pclary | 9:6bfea9af4dcb | 83 | |
pclary | 9:6bfea9af4dcb | 84 | int Servo::readWidth() |
UCSBRobotics | 7:ff85ac12e11b | 85 | { |
UCSBRobotics | 7:ff85ac12e11b | 86 | return pulseWidth; |
UCSBRobotics | 7:ff85ac12e11b | 87 | } |
UCSBRobotics | 7:ff85ac12e11b | 88 | |
UCSBRobotics | 7:ff85ac12e11b | 89 | |
UCSBRobotics | 7:ff85ac12e11b | 90 | |
pclary | 9:6bfea9af4dcb | 91 | void Servo::operator=(float degrees) |
UCSBRobotics | 7:ff85ac12e11b | 92 | { |
pclary | 9:6bfea9af4dcb | 93 | write(degrees); |
pclary | 9:6bfea9af4dcb | 94 | } |
pclary | 9:6bfea9af4dcb | 95 | |
pclary | 9:6bfea9af4dcb | 96 | |
pclary | 9:6bfea9af4dcb | 97 | |
pclary | 9:6bfea9af4dcb | 98 | Servo::operator float() |
pclary | 9:6bfea9af4dcb | 99 | { |
pclary | 9:6bfea9af4dcb | 100 | return read(); |
UCSBRobotics | 7:ff85ac12e11b | 101 | } |
UCSBRobotics | 7:ff85ac12e11b | 102 | |
UCSBRobotics | 7:ff85ac12e11b | 103 | |
UCSBRobotics | 7:ff85ac12e11b | 104 | |
UCSBRobotics | 7:ff85ac12e11b | 105 | Servo::operator int() |
UCSBRobotics | 7:ff85ac12e11b | 106 | { |
pclary | 9:6bfea9af4dcb | 107 | return readWidth(); |
UCSBRobotics | 7:ff85ac12e11b | 108 | } |
UCSBRobotics | 7:ff85ac12e11b | 109 | |
UCSBRobotics | 7:ff85ac12e11b | 110 | |
UCSBRobotics | 7:ff85ac12e11b | 111 | |
UCSBRobotics | 7:ff85ac12e11b | 112 | void Servo::refresh() |
UCSBRobotics | 7:ff85ac12e11b | 113 | { |
UCSBRobotics | 7:ff85ac12e11b | 114 | // Start all of the individual servo width timeouts and write a logical 1 to their signal pins |
pclary | 12:92dc63d24bf8 | 115 | for (unsigned int i = 0; i < numServos; i++) |
UCSBRobotics | 7:ff85ac12e11b | 116 | { |
pclary | 8:131785ed96fb | 117 | if (servos[i]->pulseWidth > 0) |
pclary | 8:131785ed96fb | 118 | { |
pclary | 8:131785ed96fb | 119 | servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth); |
pclary | 8:131785ed96fb | 120 | Servo::servos[i]->signalPin.write(1); |
pclary | 8:131785ed96fb | 121 | } |
UCSBRobotics | 7:ff85ac12e11b | 122 | } |
UCSBRobotics | 7:ff85ac12e11b | 123 | } |
UCSBRobotics | 7:ff85ac12e11b | 124 | |
UCSBRobotics | 7:ff85ac12e11b | 125 | |
UCSBRobotics | 7:ff85ac12e11b | 126 | |
UCSBRobotics | 7:ff85ac12e11b | 127 | void Servo::timeout() |
UCSBRobotics | 7:ff85ac12e11b | 128 | { |
UCSBRobotics | 7:ff85ac12e11b | 129 | // Write a logical zero to the servo's signal pin |
UCSBRobotics | 7:ff85ac12e11b | 130 | signalPin = 0; |
UCSBRobotics | 2:19f995979c6a | 131 | } |