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@4:373219702af9, 2012-08-02 (annotated)

Committer:
UCSBRobotics
Date:
Thu Aug 02 02:53:08 2012 +0000
Revision:
4:373219702af9
Parent:
2:19f995979c6a
Child:
6:6af888a91e23
Updated servo.cpp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCSBRobotics 2:19f995979c6a 1 #include "Servo.h"
UCSBRobotics 2:19f995979c6a 2 #include "mbed.h"
UCSBRobotics 2:19f995979c6a 3
UCSBRobotics 2:19f995979c6a 4
UCSBRobotics 2:19f995979c6a 5 // The next line determines the maximum number of servo objects
UCSBRobotics 2:19f995979c6a 6 // that can be successfully created.
UCSBRobotics 2:19f995979c6a 7 // By default, this is set to 26, as there are only 26 unique
UCSBRobotics 2:19f995979c6a 8 // DigitalOut pins provided by the mbed.
UCSBRobotics 2:19f995979c6a 9 Servo *Servo::servos[26];
UCSBRobotics 2:19f995979c6a 10 unsigned int Servo::numServos = 0;
UCSBRobotics 4:373219702af9 11 Ticker Servo::refreshTicker;
UCSBRobotics 2:19f995979c6a 12
UCSBRobotics 2:19f995979c6a 13
UCSBRobotics 2:19f995979c6a 14
UCSBRobotics 2:19f995979c6a 15 // The pin must be specified when the object is initialized, but
UCSBRobotics 2:19f995979c6a 16 // the initial pulse width can be omitted, giving a default of
UCSBRobotics 2:19f995979c6a 17 // 1500 us.
UCSBRobotics 2:19f995979c6a 18 // This should be at about half the range of most servos.
UCSBRobotics 2:19f995979c6a 19 Servo::Servo(PinName pin, unsigned int width) : signalPin(pin)
UCSBRobotics 2:19f995979c6a 20 {
UCSBRobotics 2:19f995979c6a 21 pulseWidth = width;
UCSBRobotics 2:19f995979c6a 22
UCSBRobotics 2:19f995979c6a 23 if (numServos == 0)
UCSBRobotics 2:19f995979c6a 24 {
UCSBRobotics 4:373219702af9 25 // Start the ticker that refreshes all servos
UCSBRobotics 4:373219702af9 26 refreshTicker.attach_us(&Servo::refresh, period);
UCSBRobotics 2:19f995979c6a 27 }
UCSBRobotics 2:19f995979c6a 28
UCSBRobotics 2:19f995979c6a 29 servos[numServos++] = this;
UCSBRobotics 2:19f995979c6a 30 }
UCSBRobotics 2:19f995979c6a 31
UCSBRobotics 2:19f995979c6a 32
UCSBRobotics 2:19f995979c6a 33
UCSBRobotics 2:19f995979c6a 34 void Servo::write(int width)
UCSBRobotics 2:19f995979c6a 35 {
UCSBRobotics 2:19f995979c6a 36 // Make sure that the pulse width is less than the refresh period
UCSBRobotics 2:19f995979c6a 37 pulseWidth = width < period ? width : period;
UCSBRobotics 2:19f995979c6a 38 }
UCSBRobotics 2:19f995979c6a 39
UCSBRobotics 2:19f995979c6a 40
UCSBRobotics 2:19f995979c6a 41
UCSBRobotics 2:19f995979c6a 42 int Servo::read()
UCSBRobotics 2:19f995979c6a 43 {
UCSBRobotics 2:19f995979c6a 44 return pulseWidth;
UCSBRobotics 2:19f995979c6a 45 }
UCSBRobotics 2:19f995979c6a 46
UCSBRobotics 2:19f995979c6a 47
UCSBRobotics 2:19f995979c6a 48
UCSBRobotics 2:19f995979c6a 49 void Servo::operator=(int width)
UCSBRobotics 2:19f995979c6a 50 {
UCSBRobotics 2:19f995979c6a 51 write(width);
UCSBRobotics 2:19f995979c6a 52 }
UCSBRobotics 2:19f995979c6a 53
UCSBRobotics 2:19f995979c6a 54
UCSBRobotics 2:19f995979c6a 55
UCSBRobotics 2:19f995979c6a 56 Servo::operator int()
UCSBRobotics 2:19f995979c6a 57 {
UCSBRobotics 2:19f995979c6a 58 return read();
UCSBRobotics 2:19f995979c6a 59 }
UCSBRobotics 2:19f995979c6a 60
UCSBRobotics 2:19f995979c6a 61
UCSBRobotics 2:19f995979c6a 62
UCSBRobotics 2:19f995979c6a 63 void Servo::refresh()
UCSBRobotics 4:373219702af9 64 {
UCSBRobotics 2:19f995979c6a 65 // Start all of the individual servo width timeouts and write a logical 1 to their signal pins
UCSBRobotics 2:19f995979c6a 66 for (int i = 0; i < numServos; i++)
UCSBRobotics 2:19f995979c6a 67 {
UCSBRobotics 2:19f995979c6a 68 servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth);
UCSBRobotics 2:19f995979c6a 69 Servo::servos[i]->signalPin.write(1);
UCSBRobotics 2:19f995979c6a 70 }
UCSBRobotics 2:19f995979c6a 71 }
UCSBRobotics 2:19f995979c6a 72
UCSBRobotics 2:19f995979c6a 73
UCSBRobotics 2:19f995979c6a 74
UCSBRobotics 2:19f995979c6a 75 void Servo::timeout()
UCSBRobotics 2:19f995979c6a 76 {
UCSBRobotics 2:19f995979c6a 77 // Write a logical zero to the servo's signal pin
UCSBRobotics 2:19f995979c6a 78 signalPin = 0;
UCSBRobotics 2:19f995979c6a 79 }