Servo.cpp

00001 #include "Servo.h"
00002 #include "mbed.h"
00003 
00004 /**
00005  * Servo constructor
00006  *
00007  * Initialize variables, attach to a PWM-enabled pin.
00008  */
00009 Servo::Servo(PinName pwm32) : _pwm(pwm32) {
00010     _period = 0;
00011     _min = 0;
00012     _max = 0;
00013     _pos = 0;
00014     invert = 0;
00015 }
00016 
00017 /**
00018  * float read
00019  *
00020  * Read the value shat should be at the servo.
00021  */
00022 float Servo::read(void) {
00023     return _pos;
00024 }
00025 
00026 /**
00027  * void write
00028  *
00029  * Write a value to the servo
00030  *
00031  * @param float to Positional data taking values of 0.0 to 1.0.
00032  */
00033 void Servo::write(float to) {
00034     if(to < 0 || to > 1) return;
00035     _pos = to;
00036     float dest_pw;
00037     if(invert == 0) {
00038         dest_pw = to * _slope + _min;
00039     }
00040     else if(invert == 1) {
00041         dest_pw = to * -_slope + _max;
00042     }
00043     else { return; }
00044     _pwm.pulsewidth(dest_pw);
00045 }
00046 
00047 /**
00048  * void calibrate
00049  *
00050  * Set the limits of the PWM.
00051  *
00052  * @param float period Pulse period in seconds
00053  * @param float max Maximum pulse width in seconds
00054  * @param float min Minimum pulse width in seconds
00055  */
00056 void Servo::calibrate(float period, float min, float max) {
00057     _period = period;
00058     _min = min;
00059     _max = max;
00060     _slope = max - min;
00061     
00062     _pwm.period(_period);
00063 }