Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

FINAL VERSION with comments

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

Servo.cpp@27:e704fdc41e87, 2019-11-04 (annotated)

Committer:
Floris_Hoek
Date:
Mon Nov 04 14:47:17 2019 +0000
Revision:
27:e704fdc41e87
Parent:
23:ff73ee119244 
FINAL VERSION V1

Who changed what in which revision?

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