fasdf gfaha / Servo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }