kusano kiyoshige / servo

first lib

Dependents:   17robo_fuzi 17robo_tokyo_kaede

servo.cpp@0:41758331f8d1, 2017-09-24 (annotated)

Committer:
echo_piyo
Date:
Sun Sep 24 06:19:24 2017 +0000
Revision:
0:41758331f8d1
(??)???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:41758331f8d1 1
echo_piyo 0:41758331f8d1 2 #include "servo.h"
echo_piyo 0:41758331f8d1 3 #include "mbed.h"
echo_piyo 0:41758331f8d1 4
echo_piyo 0:41758331f8d1 5 static float clamp(float value, float min, float max) {
echo_piyo 0:41758331f8d1 6 if(value < min) {
echo_piyo 0:41758331f8d1 7 return min;
echo_piyo 0:41758331f8d1 8 } else if(value > max) {
echo_piyo 0:41758331f8d1 9 return max;
echo_piyo 0:41758331f8d1 10 } else {
echo_piyo 0:41758331f8d1 11 return value;
echo_piyo 0:41758331f8d1 12 }
echo_piyo 0:41758331f8d1 13 }
echo_piyo 0:41758331f8d1 14
echo_piyo 0:41758331f8d1 15 void Servo::setRange(float maxrange, float minrange){
echo_piyo 0:41758331f8d1 16 maxRange = maxrange;
echo_piyo 0:41758331f8d1 17 minRange = minrange;
echo_piyo 0:41758331f8d1 18 }
echo_piyo 0:41758331f8d1 19
echo_piyo 0:41758331f8d1 20 void Servo::setMax(){
echo_piyo 0:41758331f8d1 21 write(maxRange);
echo_piyo 0:41758331f8d1 22 }
echo_piyo 0:41758331f8d1 23
echo_piyo 0:41758331f8d1 24 void Servo::setMin(){
echo_piyo 0:41758331f8d1 25 write(minRange);
echo_piyo 0:41758331f8d1 26 }
echo_piyo 0:41758331f8d1 27
echo_piyo 0:41758331f8d1 28 Servo::Servo(PinName pin) : _pwm(pin) {
echo_piyo 0:41758331f8d1 29 calibrate();
echo_piyo 0:41758331f8d1 30 write(0.5);
echo_piyo 0:41758331f8d1 31 }
echo_piyo 0:41758331f8d1 32
echo_piyo 0:41758331f8d1 33 void Servo::write(float percent) {
echo_piyo 0:41758331f8d1 34 float offset = _range * 2.0 * (percent - 0.5);
echo_piyo 0:41758331f8d1 35 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
echo_piyo 0:41758331f8d1 36 _p = clamp(percent, 0.0, 1.0);
echo_piyo 0:41758331f8d1 37 }
echo_piyo 0:41758331f8d1 38
echo_piyo 0:41758331f8d1 39 void Servo::position(float degrees) {
echo_piyo 0:41758331f8d1 40 float offset = _range * (degrees / _degrees);
echo_piyo 0:41758331f8d1 41 _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
echo_piyo 0:41758331f8d1 42 }
echo_piyo 0:41758331f8d1 43
echo_piyo 0:41758331f8d1 44 void Servo::calibrate(float range, float degrees) {
echo_piyo 0:41758331f8d1 45 _range = range;
echo_piyo 0:41758331f8d1 46 _degrees = degrees;
echo_piyo 0:41758331f8d1 47 }
echo_piyo 0:41758331f8d1 48
echo_piyo 0:41758331f8d1 49 float Servo::read() {
echo_piyo 0:41758331f8d1 50 return _p;
echo_piyo 0:41758331f8d1 51 }
echo_piyo 0:41758331f8d1 52
echo_piyo 0:41758331f8d1 53 Servo& Servo::operator= (float percent) {
echo_piyo 0:41758331f8d1 54 write(percent);
echo_piyo 0:41758331f8d1 55 return *this;
echo_piyo 0:41758331f8d1 56 }
echo_piyo 0:41758331f8d1 57
echo_piyo 0:41758331f8d1 58 Servo& Servo::operator= (Servo& rhs) {
echo_piyo 0:41758331f8d1 59 write(rhs.read());
echo_piyo 0:41758331f8d1 60 return *this;
echo_piyo 0:41758331f8d1 61 }
echo_piyo 0:41758331f8d1 62
echo_piyo 0:41758331f8d1 63 Servo::operator float() {
echo_piyo 0:41758331f8d1 64 return read();
echo_piyo 0:41758331f8d1 65 }