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@15:55221f9de707, 2013-01-17 (annotated)
- Committer:
- pclary
- Date:
- Thu Jan 17 18:32:18 2013 +0000
- Revision:
- 15:55221f9de707
- Parent:
- 14:0eff26aa0d17
Added ability to disable servos and select whether they start enabled or disabled.
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 | 15:55221f9de707 | 20 | Servo::Servo(PinName pin, bool start) : 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; |
pclary | 15:55221f9de707 | 36 | |
pclary | 15:55221f9de707 | 37 | enabled = start; |
UCSBRobotics | 7:ff85ac12e11b | 38 | } |
UCSBRobotics | 7:ff85ac12e11b | 39 | |
UCSBRobotics | 7:ff85ac12e11b | 40 | |
UCSBRobotics | 7:ff85ac12e11b | 41 | |
pclary | 13:896410a1b594 | 42 | bool Servo::calibrate(unsigned int plus45, unsigned int minus45, float upperLimit, float lowerLimit) |
pclary | 9:6bfea9af4dcb | 43 | { |
pclary | 9:6bfea9af4dcb | 44 | // Check if given parameters are valid |
pclary | 10:f0df63e99a96 | 45 | if (upperLimit > lowerLimit) |
pclary | 9:6bfea9af4dcb | 46 | { |
pclary | 13:896410a1b594 | 47 | center = (plus45 + minus45) / 2; |
pclary | 13:896410a1b594 | 48 | usPerDegree = ((int)plus45 - (int)center) / 45.0f; |
pclary | 9:6bfea9af4dcb | 49 | this->upperLimit = upperLimit; |
pclary | 9:6bfea9af4dcb | 50 | this->lowerLimit = lowerLimit; |
pclary | 9:6bfea9af4dcb | 51 | return true; |
pclary | 9:6bfea9af4dcb | 52 | } |
pclary | 9:6bfea9af4dcb | 53 | else |
pclary | 9:6bfea9af4dcb | 54 | { |
pclary | 9:6bfea9af4dcb | 55 | return false; |
pclary | 9:6bfea9af4dcb | 56 | } |
pclary | 9:6bfea9af4dcb | 57 | } |
pclary | 9:6bfea9af4dcb | 58 | |
pclary | 9:6bfea9af4dcb | 59 | |
pclary | 9:6bfea9af4dcb | 60 | |
pclary | 9:6bfea9af4dcb | 61 | void Servo::write(float degrees) |
pclary | 9:6bfea9af4dcb | 62 | { |
pclary | 9:6bfea9af4dcb | 63 | // Limit to the valid angle range |
pclary | 9:6bfea9af4dcb | 64 | degrees = (degrees > upperLimit ? upperLimit : (degrees < lowerLimit ? lowerLimit : degrees)); |
pclary | 9:6bfea9af4dcb | 65 | |
pclary | 9:6bfea9af4dcb | 66 | pulseWidth = center + (int)(degrees * usPerDegree); |
pclary | 9:6bfea9af4dcb | 67 | } |
pclary | 9:6bfea9af4dcb | 68 | |
pclary | 9:6bfea9af4dcb | 69 | |
pclary | 9:6bfea9af4dcb | 70 | |
pclary | 9:6bfea9af4dcb | 71 | void Servo::writeWidth(unsigned int width) |
UCSBRobotics | 7:ff85ac12e11b | 72 | { |
UCSBRobotics | 7:ff85ac12e11b | 73 | // Make sure that the pulse width is less than the refresh period |
UCSBRobotics | 7:ff85ac12e11b | 74 | pulseWidth = width < period ? width : period; |
UCSBRobotics | 7:ff85ac12e11b | 75 | } |
UCSBRobotics | 7:ff85ac12e11b | 76 | |
UCSBRobotics | 7:ff85ac12e11b | 77 | |
UCSBRobotics | 7:ff85ac12e11b | 78 | |
pclary | 9:6bfea9af4dcb | 79 | float Servo::read() |
pclary | 9:6bfea9af4dcb | 80 | { |
pclary | 9:6bfea9af4dcb | 81 | return ((int)pulseWidth - (int)center) / usPerDegree; |
pclary | 9:6bfea9af4dcb | 82 | } |
pclary | 9:6bfea9af4dcb | 83 | |
pclary | 9:6bfea9af4dcb | 84 | |
pclary | 9:6bfea9af4dcb | 85 | |
pclary | 9:6bfea9af4dcb | 86 | int Servo::readWidth() |
UCSBRobotics | 7:ff85ac12e11b | 87 | { |
UCSBRobotics | 7:ff85ac12e11b | 88 | return pulseWidth; |
UCSBRobotics | 7:ff85ac12e11b | 89 | } |
UCSBRobotics | 7:ff85ac12e11b | 90 | |
UCSBRobotics | 7:ff85ac12e11b | 91 | |
UCSBRobotics | 7:ff85ac12e11b | 92 | |
pclary | 15:55221f9de707 | 93 | void Servo::disable() |
pclary | 15:55221f9de707 | 94 | { |
pclary | 15:55221f9de707 | 95 | enabled = false; |
pclary | 15:55221f9de707 | 96 | } |
pclary | 15:55221f9de707 | 97 | |
pclary | 15:55221f9de707 | 98 | |
pclary | 15:55221f9de707 | 99 | |
pclary | 15:55221f9de707 | 100 | void Servo::enable() |
pclary | 15:55221f9de707 | 101 | { |
pclary | 15:55221f9de707 | 102 | enabled = true; |
pclary | 15:55221f9de707 | 103 | } |
pclary | 15:55221f9de707 | 104 | |
pclary | 15:55221f9de707 | 105 | |
pclary | 15:55221f9de707 | 106 | |
pclary | 9:6bfea9af4dcb | 107 | void Servo::operator=(float degrees) |
UCSBRobotics | 7:ff85ac12e11b | 108 | { |
pclary | 9:6bfea9af4dcb | 109 | write(degrees); |
pclary | 9:6bfea9af4dcb | 110 | } |
pclary | 9:6bfea9af4dcb | 111 | |
pclary | 9:6bfea9af4dcb | 112 | |
pclary | 9:6bfea9af4dcb | 113 | |
pclary | 9:6bfea9af4dcb | 114 | Servo::operator float() |
pclary | 9:6bfea9af4dcb | 115 | { |
pclary | 9:6bfea9af4dcb | 116 | return read(); |
UCSBRobotics | 7:ff85ac12e11b | 117 | } |
UCSBRobotics | 7:ff85ac12e11b | 118 | |
UCSBRobotics | 7:ff85ac12e11b | 119 | |
UCSBRobotics | 7:ff85ac12e11b | 120 | |
UCSBRobotics | 7:ff85ac12e11b | 121 | Servo::operator int() |
UCSBRobotics | 7:ff85ac12e11b | 122 | { |
pclary | 9:6bfea9af4dcb | 123 | return readWidth(); |
UCSBRobotics | 7:ff85ac12e11b | 124 | } |
UCSBRobotics | 7:ff85ac12e11b | 125 | |
UCSBRobotics | 7:ff85ac12e11b | 126 | |
UCSBRobotics | 7:ff85ac12e11b | 127 | |
UCSBRobotics | 7:ff85ac12e11b | 128 | void Servo::refresh() |
UCSBRobotics | 7:ff85ac12e11b | 129 | { |
UCSBRobotics | 7:ff85ac12e11b | 130 | // Start all of the individual servo width timeouts and write a logical 1 to their signal pins |
pclary | 12:92dc63d24bf8 | 131 | for (unsigned int i = 0; i < numServos; i++) |
UCSBRobotics | 7:ff85ac12e11b | 132 | { |
pclary | 15:55221f9de707 | 133 | if (servos[i]->enabled) |
pclary | 8:131785ed96fb | 134 | { |
pclary | 8:131785ed96fb | 135 | servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth); |
pclary | 8:131785ed96fb | 136 | Servo::servos[i]->signalPin.write(1); |
pclary | 8:131785ed96fb | 137 | } |
UCSBRobotics | 7:ff85ac12e11b | 138 | } |
UCSBRobotics | 7:ff85ac12e11b | 139 | } |
UCSBRobotics | 7:ff85ac12e11b | 140 | |
UCSBRobotics | 7:ff85ac12e11b | 141 | |
UCSBRobotics | 7:ff85ac12e11b | 142 | |
UCSBRobotics | 7:ff85ac12e11b | 143 | void Servo::timeout() |
UCSBRobotics | 7:ff85ac12e11b | 144 | { |
UCSBRobotics | 7:ff85ac12e11b | 145 | // Write a logical zero to the servo's signal pin |
UCSBRobotics | 7:ff85ac12e11b | 146 | signalPin = 0; |
UCSBRobotics | 2:19f995979c6a | 147 | } |