Tomotsugu Ogawa / Servo

サーボ制御

Dependents:   Servo

Servo.cpp@3:36b69a7ced07, 2010-09-02 (annotated)

Committer:
simon
Date:
Thu Sep 02 17:34:43 2010 +0000
Revision:
3:36b69a7ced07
Parent:
2:8995c167f399 
Published as library, with new doxygen autodoc

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);
simon 0:24148c673250 46 }
simon 0:24148c673250 47
simon 0:24148c673250 48 void Servo::position(float degrees) {
simon 0:24148c673250 49 float offset = _range * (degrees / _degrees);
simon 0:24148c673250 50 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
simon 0:24148c673250 51 }
simon 0:24148c673250 52
simon 0:24148c673250 53 void Servo::calibrate(float range, float degrees) {
simon 0:24148c673250 54 _range = range;
simon 0:24148c673250 55 _degrees = degrees;
simon 0:24148c673250 56 }
simon 0:24148c673250 57
simon 0:24148c673250 58 float Servo::read() {
simon 0:24148c673250 59 return _p;
simon 0:24148c673250 60 }
simon 0:24148c673250 61
simon 0:24148c673250 62 Servo& Servo::operator= (float percent) {
simon 0:24148c673250 63 write(percent);
simon 0:24148c673250 64 return *this;
simon 0:24148c673250 65 }
simon 0:24148c673250 66
simon 0:24148c673250 67 Servo& Servo::operator= (Servo& rhs) {
simon 0:24148c673250 68 write(rhs.read());
simon 0:24148c673250 69 return *this;
simon 0:24148c673250 70 }
simon 0:24148c673250 71
simon 0:24148c673250 72 Servo::operator float() {
simon 0:24148c673250 73 return read();
simon 0:24148c673250 74 }