Dominik Święch / Ser

A class to control a model R/C servo, using a PwmOut

Fork of Servo by Simon Ford

Servo.cpp@4:fc2e5d57af37, 2016-07-07 (annotated)

Committer:
yruiewyrui3
Date:
Thu Jul 07 12:55:07 2016 +0000
Revision:
4:fc2e5d57af37
Parent:
2:8995c167f399 
.

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();
simon 0:24148c673250 39 write(0.5);
simon 0:24148c673250 40 }
simon 0:24148c673250 41
simon 0:24148c673250 42 void Servo::write(float percent) {
simon 0:24148c673250 43 float offset = _range * 2.0 * (percent - 0.5);
simon 0:24148c673250 44 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
simon 0:24148c673250 45 _p = clamp(percent, 0.0, 1.0);
yruiewyrui3 4:fc2e5d57af37 46
simon 0:24148c673250 47 }
simon 0:24148c673250 48
simon 0:24148c673250 49 void Servo::position(float degrees) {
yruiewyrui3 4:fc2e5d57af37 50 // float offset = _range * (degrees / _degrees);
yruiewyrui3 4:fc2e5d57af37 51 if(degrees>0){
yruiewyrui3 4:fc2e5d57af37 52
yruiewyrui3 4:fc2e5d57af37 53 kat= 0.00145+ fabs(degrees)*0.00001111;
yruiewyrui3 4:fc2e5d57af37 54 _pwm.pulsewidth(kat);
yruiewyrui3 4:fc2e5d57af37 55
yruiewyrui3 4:fc2e5d57af37 56 }
yruiewyrui3 4:fc2e5d57af37 57
yruiewyrui3 4:fc2e5d57af37 58 else if (degrees==0){
yruiewyrui3 4:fc2e5d57af37 59 kat=0.00145;
yruiewyrui3 4:fc2e5d57af37 60 _pwm.pulsewidth(kat);
yruiewyrui3 4:fc2e5d57af37 61 }
yruiewyrui3 4:fc2e5d57af37 62
yruiewyrui3 4:fc2e5d57af37 63 else {
yruiewyrui3 4:fc2e5d57af37 64 kat= 0.00145- fabs(degrees)*0.00001111;
yruiewyrui3 4:fc2e5d57af37 65 _pwm.pulsewidth(kat);
yruiewyrui3 4:fc2e5d57af37 66 }
yruiewyrui3 4:fc2e5d57af37 67
yruiewyrui3 4:fc2e5d57af37 68 //_pwm.pulsewidth(0.00245);
yruiewyrui3 4:fc2e5d57af37 69 //k=(0.0015 + clamp(offset, -_range, 0.002 +_range));
yruiewyrui3 4:fc2e5d57af37 70
simon 0:24148c673250 71 }
simon 0:24148c673250 72
simon 0:24148c673250 73 void Servo::calibrate(float range, float degrees) {
simon 0:24148c673250 74 _range = range;
simon 0:24148c673250 75 _degrees = degrees;
simon 0:24148c673250 76 }
simon 0:24148c673250 77
simon 0:24148c673250 78 float Servo::read() {
simon 0:24148c673250 79 return _p;
simon 0:24148c673250 80 }
simon 0:24148c673250 81
simon 0:24148c673250 82 Servo& Servo::operator= (float percent) {
simon 0:24148c673250 83 write(percent);
simon 0:24148c673250 84 return *this;
simon 0:24148c673250 85 }
simon 0:24148c673250 86
simon 0:24148c673250 87 Servo& Servo::operator= (Servo& rhs) {
simon 0:24148c673250 88 write(rhs.read());
simon 0:24148c673250 89 return *this;
simon 0:24148c673250 90 }
simon 0:24148c673250 91
simon 0:24148c673250 92 Servo::operator float() {
simon 0:24148c673250 93 return read();
simon 0:24148c673250 94 }