FINAL VERSION with comments
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Servo.cpp@23:ff73ee119244, 2019-10-31 (annotated)
- Committer:
- Floris_Hoek
- Date:
- Thu Oct 31 09:48:53 2019 +0000
- Revision:
- 23:ff73ee119244
final V1
Who changed what in which revision?
User | Revision | Line number | New 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 | } |