zhaw_st16b_pes2_10 / ServoArm

cac

Fork of Servo by Simon Ford

ServoArm.cpp@5:fd67069d66e6, 2017-05-03 (annotated)

Committer:
beacon
Date:
Wed May 03 13:54:43 2017 +0000
Revision:
5:fd67069d66e6
Parent:
4:62c9bdc760b4 
THIIIIS;

Who changed what in which revision?

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