Bayley Wang
/
foc-ed_in_the_bot_compact
robot
PwmIn/PwmIn.cpp
- Committer:
- bwang
- Date:
- 2017-05-04
- Revision:
- 150:08c13bfc7417
- Parent:
- 141:ccd21d7fbd1a
- Child:
- 151:5bbb15351798
File content as of revision 150:08c13bfc7417:
#include "mbed.h" #include "PwmIn.h" #include "MathHelpers.h" PwmIn::PwmIn(PinName pin, int usec_min, int usec_max, int usec_crazy_low, int usec_crazy_hi) { 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; this->usec_crazy_low = usec_crazy_low; this->usec_crazy_hi = usec_crazy_hi; usecs = usec_min; blocked = false; enabled = false; risen = false; } bool PwmIn::get_enabled() { return enabled; } void PwmIn::handle_rise() { if (!enabled) { enabled = true; usecs = usec_min; } risen = true; timer.stop(); timer.reset(); timer.start(); } void PwmIn::handle_fall() { int usecs_new = timer.read_us(); timer.stop(); timer.reset(); timer.start(); if (blocked) { blocked = false; risen = false; return; } //if (usecs_new <= usec_crazy_hi && usecs_new >= usec_crazy_low && risen) usecs = usecs_new; if (risen) usecs = usecs_new; risen = false; } float PwmIn::get_throttle() { if(timer.read_us() > 40000) { enabled = false; usecs = usec_min; } if(!enabled) return -1; return constrain(map((float)usecs, usec_min, usec_max, 0, 1), 0, 1); }