yashirou shimogamo / Mbed 2 deprecated BIRD2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos SPI_MX25R

Control/Servo/Servo.cpp@0:d6402bcd58f7, 2018-12-01 (annotated)

Committer:
shimogamo
Date:
Sat Dec 01 15:10:42 2018 +0000
Revision:
0:d6402bcd58f7
BIRD2018????????????

Who changed what in which revision?

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