zhang chenhong / Servo

78678

Dependents:   IDW01M1_Cloud_IBM

Servo.cpp@4:cbc67535aa7b, 2020-11-04 (annotated)

Committer:
fishzhang
Date:
Wed Nov 04 02:49:15 2020 +0000
Revision:
4:cbc67535aa7b
Parent:
2:8995c167f399 
1

Who changed what in which revision?

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