00001 /* mbed R/C Servo
00002  * Copyright (c) 2007-2009 sford, cstyles
00003  * Released under the MIT License: http://mbed.org/license/mit
00004  */
00005 
00006 #include "Servo.h"
00007 #include "mbed.h"
00008 
00009 static float clamp(float value, float min, float max) {
00010     if(value < min) {
00011         return min;
00012     } else if(value > max) {
00013         return max;
00014     } else {
00015         return value;
00016     }
00017 }
00018 
00019 Servo::Servo(PinName pin) : _pwm(pin) {
00020     calibrate();
00021     write(0.5);
00022 }
00023 
00024 void Servo::write(float percent) {
00025     float offset = _range * 2.0 * (percent - 0.5);
00026     _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
00027     _p = clamp(percent, 0.0, 1.0);
00028 }
00029 
00030 void Servo::position(float degrees) {
00031     float offset = _range * (degrees / _degrees);
00032     _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
00033 }
00034 
00035 void Servo::calibrate(float range, float degrees) {
00036     _range = range;
00037     _degrees = degrees;
00038 }
00039 
00040 float Servo::read() {
00041     return _p;
00042 }
00043 
00044 Servo& Servo::operator= (float percent) { 
00045     write(percent);
00046     return *this;
00047 }
00048 
00049 Servo& Servo::operator= (Servo& rhs) {
00050     write(rhs.read());
00051     return *this;
00052 }
00053 
00054 Servo::operator float() {
00055     return read();
00056 }