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@8:131785ed96fb, 2012-10-03 (annotated)

Committer:
pclary
Date:
Wed Oct 03 07:28:03 2012 +0000
Revision:
8:131785ed96fb
Parent:
7:ff85ac12e11b
Child:
9:6bfea9af4dcb
Stopped PWM when pulseWidth is 0

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.
UCSBRobotics 7:ff85ac12e11b 20 Servo::Servo(PinName pin, unsigned int width) : signalPin(pin)
UCSBRobotics 7:ff85ac12e11b 21 {
UCSBRobotics 7:ff85ac12e11b 22 pulseWidth = width;
UCSBRobotics 7:ff85ac12e11b 23
UCSBRobotics 7:ff85ac12e11b 24 if (numServos == 0)
UCSBRobotics 7:ff85ac12e11b 25 {
UCSBRobotics 7:ff85ac12e11b 26 refreshTicker = new Ticker();
UCSBRobotics 7:ff85ac12e11b 27
UCSBRobotics 7:ff85ac12e11b 28 // Start the ticker that refreshes all servos
UCSBRobotics 7:ff85ac12e11b 29 refreshTicker->attach_us(&Servo::refresh, period);
UCSBRobotics 7:ff85ac12e11b 30 }
UCSBRobotics 7:ff85ac12e11b 31
UCSBRobotics 7:ff85ac12e11b 32 servos[numServos++] = this;
UCSBRobotics 7:ff85ac12e11b 33 }
UCSBRobotics 7:ff85ac12e11b 34
UCSBRobotics 7:ff85ac12e11b 35
UCSBRobotics 7:ff85ac12e11b 36
UCSBRobotics 7:ff85ac12e11b 37 void Servo::write(int width)
UCSBRobotics 7:ff85ac12e11b 38 {
UCSBRobotics 7:ff85ac12e11b 39 // Make sure that the pulse width is less than the refresh period
UCSBRobotics 7:ff85ac12e11b 40 pulseWidth = width < period ? width : period;
UCSBRobotics 7:ff85ac12e11b 41 }
UCSBRobotics 7:ff85ac12e11b 42
UCSBRobotics 7:ff85ac12e11b 43
UCSBRobotics 7:ff85ac12e11b 44
UCSBRobotics 7:ff85ac12e11b 45 int Servo::read()
UCSBRobotics 7:ff85ac12e11b 46 {
UCSBRobotics 7:ff85ac12e11b 47 return pulseWidth;
UCSBRobotics 7:ff85ac12e11b 48 }
UCSBRobotics 7:ff85ac12e11b 49
UCSBRobotics 7:ff85ac12e11b 50
UCSBRobotics 7:ff85ac12e11b 51
UCSBRobotics 7:ff85ac12e11b 52 void Servo::operator=(int width)
UCSBRobotics 7:ff85ac12e11b 53 {
UCSBRobotics 7:ff85ac12e11b 54 write(width);
UCSBRobotics 7:ff85ac12e11b 55 }
UCSBRobotics 7:ff85ac12e11b 56
UCSBRobotics 7:ff85ac12e11b 57
UCSBRobotics 7:ff85ac12e11b 58
UCSBRobotics 7:ff85ac12e11b 59 Servo::operator int()
UCSBRobotics 7:ff85ac12e11b 60 {
UCSBRobotics 7:ff85ac12e11b 61 return read();
UCSBRobotics 7:ff85ac12e11b 62 }
UCSBRobotics 7:ff85ac12e11b 63
UCSBRobotics 7:ff85ac12e11b 64
UCSBRobotics 7:ff85ac12e11b 65
UCSBRobotics 7:ff85ac12e11b 66 void Servo::refresh()
UCSBRobotics 7:ff85ac12e11b 67 {
UCSBRobotics 7:ff85ac12e11b 68 // Start all of the individual servo width timeouts and write a logical 1 to their signal pins
UCSBRobotics 7:ff85ac12e11b 69 for (int i = 0; i < numServos; i++)
UCSBRobotics 7:ff85ac12e11b 70 {
pclary 8:131785ed96fb 71 if (servos[i]->pulseWidth > 0)
pclary 8:131785ed96fb 72 {
pclary 8:131785ed96fb 73 servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth);
pclary 8:131785ed96fb 74 Servo::servos[i]->signalPin.write(1);
pclary 8:131785ed96fb 75 }
UCSBRobotics 7:ff85ac12e11b 76 }
UCSBRobotics 7:ff85ac12e11b 77 }
UCSBRobotics 7:ff85ac12e11b 78
UCSBRobotics 7:ff85ac12e11b 79
UCSBRobotics 7:ff85ac12e11b 80
UCSBRobotics 7:ff85ac12e11b 81 void Servo::timeout()
UCSBRobotics 7:ff85ac12e11b 82 {
UCSBRobotics 7:ff85ac12e11b 83 // Write a logical zero to the servo's signal pin
UCSBRobotics 7:ff85ac12e11b 84 signalPin = 0;
UCSBRobotics 2:19f995979c6a 85 }