Lowe Dantzler / Servo

Description

Dependents:   RoboPanorama

Fork of Servo by Simon Ford

Servo.cpp@4:2176f8595b34, 2012-10-12 (annotated)

Committer:
lndv3
Date:
Fri Oct 12 16:41:39 2012 +0000
Revision:
4:2176f8595b34
Parent:
2:8995c167f399 
Commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lndv3 4:2176f8595b34 1 /* mbed R/C Servo Library
lndv3 4:2176f8595b34 2 *
lndv3 4:2176f8595b34 3 * Copyright (c) 2007-2010 sford, cstyles
lndv3 4:2176f8595b34 4 *
lndv3 4:2176f8595b34 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
lndv3 4:2176f8595b34 6 * of this software and associated documentation files (the "Software"), to deal
lndv3 4:2176f8595b34 7 * in the Software without restriction, including without limitation the rights
lndv3 4:2176f8595b34 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
lndv3 4:2176f8595b34 9 * copies of the Software, and to permit persons to whom the Software is
lndv3 4:2176f8595b34 10 * furnished to do so, subject to the following conditions:
lndv3 4:2176f8595b34 11 *
lndv3 4:2176f8595b34 12 * The above copyright notice and this permission notice shall be included in
lndv3 4:2176f8595b34 13 * all copies or substantial portions of the Software.
lndv3 4:2176f8595b34 14 *
lndv3 4:2176f8595b34 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
lndv3 4:2176f8595b34 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
lndv3 4:2176f8595b34 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
lndv3 4:2176f8595b34 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
lndv3 4:2176f8595b34 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
lndv3 4:2176f8595b34 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
lndv3 4:2176f8595b34 21 * THE SOFTWARE.
lndv3 4:2176f8595b34 22 */
lndv3 4:2176f8595b34 23
lndv3 4:2176f8595b34 24 #include "Servo.h"
lndv3 4:2176f8595b34 25 #include "mbed.h"
lndv3 4:2176f8595b34 26
lndv3 4:2176f8595b34 27 static float clamp(float value, float min, float max) {
lndv3 4:2176f8595b34 28 if(value < min) {
lndv3 4:2176f8595b34 29 return min;
lndv3 4:2176f8595b34 30 } else if(value > max) {
lndv3 4:2176f8595b34 31 return max;
lndv3 4:2176f8595b34 32 } else {
lndv3 4:2176f8595b34 33 return value;
lndv3 4:2176f8595b34 34 }
lndv3 4:2176f8595b34 35 }
lndv3 4:2176f8595b34 36
lndv3 4:2176f8595b34 37 Servo::Servo(PinName pin) : _pwm(pin) {
lndv3 4:2176f8595b34 38 calibrate();
lndv3 4:2176f8595b34 39 write(0.5);
lndv3 4:2176f8595b34 40 }
lndv3 4:2176f8595b34 41
lndv3 4:2176f8595b34 42 void Servo::write(float percent) {
lndv3 4:2176f8595b34 43 float offset = _range * 2.0 * (percent - 0.5);
lndv3 4:2176f8595b34 44 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
lndv3 4:2176f8595b34 45 _p = clamp(percent, 0.0, 1.0);
lndv3 4:2176f8595b34 46 }
lndv3 4:2176f8595b34 47
lndv3 4:2176f8595b34 48 void Servo::position(float degrees) {
lndv3 4:2176f8595b34 49 float offset = _range * (degrees / _degrees);
lndv3 4:2176f8595b34 50 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
lndv3 4:2176f8595b34 51 }
lndv3 4:2176f8595b34 52
lndv3 4:2176f8595b34 53 void Servo::calibrate(float range, float degrees) {
lndv3 4:2176f8595b34 54 _range = range;
lndv3 4:2176f8595b34 55 _degrees = degrees;
lndv3 4:2176f8595b34 56 }
lndv3 4:2176f8595b34 57
lndv3 4:2176f8595b34 58 float Servo::read() {
lndv3 4:2176f8595b34 59 return _p;
lndv3 4:2176f8595b34 60 }
lndv3 4:2176f8595b34 61
lndv3 4:2176f8595b34 62 Servo& Servo::operator= (float percent) {
lndv3 4:2176f8595b34 63 write(percent);
lndv3 4:2176f8595b34 64 return *this;
lndv3 4:2176f8595b34 65 }
lndv3 4:2176f8595b34 66
lndv3 4:2176f8595b34 67 Servo& Servo::operator= (Servo& rhs) {
lndv3 4:2176f8595b34 68 write(rhs.read());
lndv3 4:2176f8595b34 69 return *this;
lndv3 4:2176f8595b34 70 }
lndv3 4:2176f8595b34 71
lndv3 4:2176f8595b34 72 Servo::operator float() {
lndv3 4:2176f8595b34 73 return read();
lndv3 4:2176f8595b34 74 }