Ikarashi Kota / Servo_softpwm

ds3218

Dependencies:   SoftPWM

Dependents:   NHK2021_ikarashiSV_code_withservo 2021NHK_B_syudo 2021NHK_B_main

Servo.cpp@6:39d683a34a55, 2021-10-18 (annotated)

Committer:
ikarashikota
Date:
Mon Oct 18 14:19:48 2021 +0000
Revision:
6:39d683a34a55
Parent:
5:ef8f4fe45587 
:-)

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"
highfieldsnj 4:9cc96122f399 26 #include "SoftPWM.h"
simon 0:24148c673250 27
simon 0:24148c673250 28 static float clamp(float value, float min, float max) {
simon 0:24148c673250 29 if(value < min) {
simon 0:24148c673250 30 return min;
simon 0:24148c673250 31 } else if(value > max) {
simon 0:24148c673250 32 return max;
simon 0:24148c673250 33 } else {
simon 0:24148c673250 34 return value;
simon 0:24148c673250 35 }
simon 0:24148c673250 36 }
simon 0:24148c673250 37
simon 0:24148c673250 38 Servo::Servo(PinName pin) : _pwm(pin) {
highfieldsnj 5:ef8f4fe45587 39 _pwm.period_ms(20);
simon 0:24148c673250 40 calibrate();
simon 0:24148c673250 41 write(0.5);
simon 0:24148c673250 42 }
simon 0:24148c673250 43
simon 0:24148c673250 44 void Servo::write(float percent) {
simon 0:24148c673250 45 float offset = _range * 2.0 * (percent - 0.5);
simon 0:24148c673250 46 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
simon 0:24148c673250 47 _p = clamp(percent, 0.0, 1.0);
simon 0:24148c673250 48 }
simon 0:24148c673250 49
simon 0:24148c673250 50 void Servo::position(float degrees) {
simon 0:24148c673250 51 float offset = _range * (degrees / _degrees);
simon 0:24148c673250 52 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
simon 0:24148c673250 53 }
simon 0:24148c673250 54
simon 0:24148c673250 55 void Servo::calibrate(float range, float degrees) {
simon 0:24148c673250 56 _range = range;
simon 0:24148c673250 57 _degrees = degrees;
simon 0:24148c673250 58 }
simon 0:24148c673250 59
simon 0:24148c673250 60 float Servo::read() {
simon 0:24148c673250 61 return _p;
simon 0:24148c673250 62 }
simon 0:24148c673250 63
simon 0:24148c673250 64 Servo& Servo::operator= (float percent) {
simon 0:24148c673250 65 write(percent);
simon 0:24148c673250 66 return *this;
simon 0:24148c673250 67 }
simon 0:24148c673250 68
simon 0:24148c673250 69 Servo& Servo::operator= (Servo& rhs) {
simon 0:24148c673250 70 write(rhs.read());
simon 0:24148c673250 71 return *this;
simon 0:24148c673250 72 }
simon 0:24148c673250 73
simon 0:24148c673250 74 Servo::operator float() {
simon 0:24148c673250 75 return read();
simon 0:24148c673250 76 }