EE192 Team 4 / Servo

Faster servo

Fork of Servo by Simon Ford

Servo.cpp@4:d66a8456658a, 2016-04-22 (annotated)

Committer:
vsutardja
Date:
Fri Apr 22 19:36:24 2016 +0000
Revision:
4:d66a8456658a
Parent:
2:8995c167f399 
Increased frequency

Who changed what in which revision?

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