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@2:19f995979c6a, 2012-08-01 (annotated)

Committer:
UCSBRobotics
Date:
Wed Aug 01 22:28:33 2012 +0000
Revision:
2:19f995979c6a
Child:
4:373219702af9
Child:
5:94cdc85bf1ae
Copied from PWMTest

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 2:19f995979c6a 11 Timeout Servo::refreshTimeout;
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 2:19f995979c6a 25 // Start the timeout that refreshes all servos
UCSBRobotics 2:19f995979c6a 26 refreshTimeout.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 2:19f995979c6a 64 {
UCSBRobotics 2:19f995979c6a 65 // Restart the refresh timeout
UCSBRobotics 2:19f995979c6a 66 refreshTimeout.attach_us(&Servo::refresh, period);
UCSBRobotics 2:19f995979c6a 67
UCSBRobotics 2:19f995979c6a 68 // Start all of the individual servo width timeouts and write a logical 1 to their signal pins
UCSBRobotics 2:19f995979c6a 69 for (int i = 0; i < numServos; i++)
UCSBRobotics 2:19f995979c6a 70 {
UCSBRobotics 2:19f995979c6a 71 servos[i]->servoTimeout.attach_us(servos[i], &Servo::timeout, servos[i]->pulseWidth);
UCSBRobotics 2:19f995979c6a 72 Servo::servos[i]->signalPin.write(1);
UCSBRobotics 2:19f995979c6a 73 }
UCSBRobotics 2:19f995979c6a 74 }
UCSBRobotics 2:19f995979c6a 75
UCSBRobotics 2:19f995979c6a 76
UCSBRobotics 2:19f995979c6a 77
UCSBRobotics 2:19f995979c6a 78 void Servo::timeout()
UCSBRobotics 2:19f995979c6a 79 {
UCSBRobotics 2:19f995979c6a 80 // Write a logical zero to the servo's signal pin
UCSBRobotics 2:19f995979c6a 81 signalPin = 0;
UCSBRobotics 2:19f995979c6a 82 }