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@14:0eff26aa0d17, 2013-01-09 (annotated)

Committer:
pclary
Date:
Wed Jan 09 21:08:53 2013 +0000
Revision:
14:0eff26aa0d17
Parent:
13:896410a1b594
Parent:
12:92dc63d24bf8
Child:
15:55221f9de707
Merged branches

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 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 13:896410a1b594 40 bool Servo::calibrate(unsigned int plus45, unsigned int minus45, 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 13:896410a1b594 45 center = (plus45 + minus45) / 2;
pclary 13:896410a1b594 46 usPerDegree = ((int)plus45 - (int)center) / 45.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 }