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@9:6bfea9af4dcb, 2012-10-21 (annotated)
- Committer:
- pclary
- Date:
- Sun Oct 21 00:03:06 2012 +0000
- Revision:
- 9:6bfea9af4dcb
- Parent:
- 8:131785ed96fb
- Child:
- 10:f0df63e99a96
Added setting angles by degrees, calibration, and angle limits.
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 | 9:6bfea9af4dcb | 43 | if (plus60 > minus60 && upperLimit > lowerLimit) |
pclary | 9:6bfea9af4dcb | 44 | { |
pclary | 9:6bfea9af4dcb | 45 | center = (plus60 + minus60) / 2; |
pclary | 9:6bfea9af4dcb | 46 | usPerDegree = (plus60 - center) / 60.f; |
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 |
UCSBRobotics | 7:ff85ac12e11b | 115 | for (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 | } |