Pedro Enrique Vidal Peñas / Servo

SERVO PARA QUADCOPTER

Dependents:   quadcopter servo1 Elevator

Servo.cpp@0:5490c22f727d, 2011-02-22 (annotated)

Committer:
cr0n0s20
Date:
Tue Feb 22 22:07:28 2011 +0000
Revision:
0:5490c22f727d
1.0

Who changed what in which revision?

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