Jexus Chuang / Servo

another clone of servo library

Servo.cpp@0:c24d2ead5c71, 2020-01-09 (annotated)

Committer:
yungsung
Date:
Thu Jan 09 19:18:56 2020 +0000
Revision:
0:c24d2ead5c71
done

Who changed what in which revision?

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