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@7:ff85ac12e11b, 2012-08-30 (annotated)

Committer:
UCSBRobotics
Date:
Thu Aug 30 03:36:03 2012 +0000
Revision:
7:ff85ac12e11b
Parent:
6:6af888a91e23
Child:
8:131785ed96fb
Fixed to allow for servos to be created with global scope.

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 {
UCSBRobotics 7:ff85ac12e11b 71 servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth);
UCSBRobotics 7:ff85ac12e11b 72 Servo::servos[i]->signalPin.write(1);
UCSBRobotics 7:ff85ac12e11b 73 }
UCSBRobotics 7:ff85ac12e11b 74 }
UCSBRobotics 7:ff85ac12e11b 75
UCSBRobotics 7:ff85ac12e11b 76
UCSBRobotics 7:ff85ac12e11b 77
UCSBRobotics 7:ff85ac12e11b 78 void Servo::timeout()
UCSBRobotics 7:ff85ac12e11b 79 {
UCSBRobotics 7:ff85ac12e11b 80 // Write a logical zero to the servo's signal pin
UCSBRobotics 7:ff85ac12e11b 81 signalPin = 0;
UCSBRobotics 2:19f995979c6a 82 }