A class to control a model R/C servo, using a PwmOut

Dependencies:   tsi_sensor

Dependents:   AccelExample

Fork of Servo by Simon Ford

Committer:
simon
Date:
Mon Nov 16 18:24:16 2009 +0000
Revision:
0:24148c673250
Child:
2:8995c167f399

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:24148c673250 1 /* mbed R/C Servo
simon 0:24148c673250 2 * Copyright (c) 2007-2009 sford, cstyles
simon 0:24148c673250 3 * Released under the MIT License: http://mbed.org/license/mit
simon 0:24148c673250 4 */
simon 0:24148c673250 5
simon 0:24148c673250 6 #include "Servo.h"
simon 0:24148c673250 7 #include "mbed.h"
simon 0:24148c673250 8
simon 0:24148c673250 9 static float clamp(float value, float min, float max) {
simon 0:24148c673250 10 if(value < min) {
simon 0:24148c673250 11 return min;
simon 0:24148c673250 12 } else if(value > max) {
simon 0:24148c673250 13 return max;
simon 0:24148c673250 14 } else {
simon 0:24148c673250 15 return value;
simon 0:24148c673250 16 }
simon 0:24148c673250 17 }
simon 0:24148c673250 18
simon 0:24148c673250 19 Servo::Servo(PinName pin) : _pwm(pin) {
simon 0:24148c673250 20 calibrate();
simon 0:24148c673250 21 write(0.5);
simon 0:24148c673250 22 }
simon 0:24148c673250 23
simon 0:24148c673250 24 void Servo::write(float percent) {
simon 0:24148c673250 25 float offset = _range * 2.0 * (percent - 0.5);
simon 0:24148c673250 26 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
simon 0:24148c673250 27 _p = clamp(percent, 0.0, 1.0);
simon 0:24148c673250 28 }
simon 0:24148c673250 29
simon 0:24148c673250 30 void Servo::position(float degrees) {
simon 0:24148c673250 31 float offset = _range * (degrees / _degrees);
simon 0:24148c673250 32 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
simon 0:24148c673250 33 }
simon 0:24148c673250 34
simon 0:24148c673250 35 void Servo::calibrate(float range, float degrees) {
simon 0:24148c673250 36 _range = range;
simon 0:24148c673250 37 _degrees = degrees;
simon 0:24148c673250 38 }
simon 0:24148c673250 39
simon 0:24148c673250 40 float Servo::read() {
simon 0:24148c673250 41 return _p;
simon 0:24148c673250 42 }
simon 0:24148c673250 43
simon 0:24148c673250 44 Servo& Servo::operator= (float percent) {
simon 0:24148c673250 45 write(percent);
simon 0:24148c673250 46 return *this;
simon 0:24148c673250 47 }
simon 0:24148c673250 48
simon 0:24148c673250 49 Servo& Servo::operator= (Servo& rhs) {
simon 0:24148c673250 50 write(rhs.read());
simon 0:24148c673250 51 return *this;
simon 0:24148c673250 52 }
simon 0:24148c673250 53
simon 0:24148c673250 54 Servo::operator float() {
simon 0:24148c673250 55 return read();
simon 0:24148c673250 56 }