Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: PwmIn/PwmIn.cpp
- Revision:
- 18:3863ca45cf26
- Parent:
- 16:f283d6032fe5
- Child:
- 19:a6cf15f89f3d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmIn/PwmIn.cpp Mon Oct 31 06:53:28 2016 +0000 @@ -0,0 +1,56 @@ +#include "PwmIn.h" +#include "mbed.h" + +float map(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +float constrain(float in, float min, float max) +{ + if(in > max) return max; + if(in < min) return min; + return in; +} + +PwmIn::PwmIn(PinName pin, int usec_min, int usec_max) +{ + int_in = new InterruptIn(pin); + dig_in = new DigitalIn(pin); + int_in->rise(this, &PwmIn::handle_rise); + int_in->fall(this, &PwmIn::handle_fall); + this->usec_min = usec_min; + this->usec_max = usec_max; +} + + +bool PwmIn::get_enabled() +{ + return enabled; +} + +void PwmIn::handle_rise() +{ + enabled = true; + timer.stop(); + timer.reset(); + timer.start(); + was_on = true; +} + +void PwmIn::handle_fall() +{ + was_on = false; + usecs = timer.read_us(); + timer.stop(); + timer.reset(); + timer.start(); +} + +float PwmIn::get_throttle() +{ + if(timer.read_us() > 40000) enabled = false; + if(!enabled) return -1; + return constrain(map((float)usecs, usec_min, usec_max, 0, 1), 0, 1); +} +