fasdf gfaha / Servo

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?

UserRevisionLine numberNew 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 }