Bayley Wang
robot
PwmIn/PwmIn.cpp@151:5bbb15351798, 2017-05-04 (annotated)
- Committer:
- bwang
- Date:
- Thu May 04 14:34:34 2017 +0000
- Revision:
- 151:5bbb15351798
- Parent:
- 150:08c13bfc7417
- Child:
- 155:7c6005933d4c
05/04/2017 10:33 - removed throttle_crazy checks as these lead to dangerous behavior; beginnings of new filter code in main.cpp
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| bwang | 19:a6cf15f89f3d | 1 | #include "mbed.h" |
| bwang | 18:3863ca45cf26 | 2 | #include "PwmIn.h" |
| bwang | 19:a6cf15f89f3d | 3 | #include "MathHelpers.h" |
| dicarloj | 13:41d102a53caf | 4 | |
| bwang | 151:5bbb15351798 | 5 | PwmIn::PwmIn(PinName pin, int usec_min, int usec_max) |
| dicarloj | 13:41d102a53caf | 6 | { |
| dicarloj | 13:41d102a53caf | 7 | int_in = new InterruptIn(pin); |
| dicarloj | 13:41d102a53caf | 8 | dig_in = new DigitalIn(pin); |
| bwang | 16:f283d6032fe5 | 9 | int_in->rise(this, &PwmIn::handle_rise); |
| bwang | 16:f283d6032fe5 | 10 | int_in->fall(this, &PwmIn::handle_fall); |
| dicarloj | 13:41d102a53caf | 11 | this->usec_min = usec_min; |
| dicarloj | 13:41d102a53caf | 12 | this->usec_max = usec_max; |
| bwang | 75:591556ce033d | 13 | |
| bwang | 75:591556ce033d | 14 | usecs = usec_min; |
| bwang | 75:591556ce033d | 15 | blocked = false; |
| bwang | 75:591556ce033d | 16 | enabled = false; |
| bwang | 92:a9dac72d8cac | 17 | risen = false; |
| dicarloj | 13:41d102a53caf | 18 | } |
| dicarloj | 13:41d102a53caf | 19 | |
| dicarloj | 13:41d102a53caf | 20 | |
| bwang | 16:f283d6032fe5 | 21 | bool PwmIn::get_enabled() |
| dicarloj | 13:41d102a53caf | 22 | { |
| dicarloj | 13:41d102a53caf | 23 | return enabled; |
| dicarloj | 13:41d102a53caf | 24 | } |
| dicarloj | 13:41d102a53caf | 25 | |
| bwang | 16:f283d6032fe5 | 26 | void PwmIn::handle_rise() |
| dicarloj | 13:41d102a53caf | 27 | { |
| bwang | 137:54c47ef47bbb | 28 | if (!enabled) { |
| bwang | 137:54c47ef47bbb | 29 | enabled = true; |
| bwang | 138:7ff70cef8db3 | 30 | usecs = usec_min; |
| bwang | 137:54c47ef47bbb | 31 | } |
| bwang | 92:a9dac72d8cac | 32 | risen = true; |
| bwang | 75:591556ce033d | 33 | |
| dicarloj | 13:41d102a53caf | 34 | timer.stop(); |
| dicarloj | 13:41d102a53caf | 35 | timer.reset(); |
| dicarloj | 13:41d102a53caf | 36 | timer.start(); |
| dicarloj | 13:41d102a53caf | 37 | } |
| dicarloj | 13:41d102a53caf | 38 | |
| bwang | 16:f283d6032fe5 | 39 | void PwmIn::handle_fall() |
| dicarloj | 13:41d102a53caf | 40 | { |
| bwang | 75:591556ce033d | 41 | int usecs_new = timer.read_us(); |
| dicarloj | 13:41d102a53caf | 42 | timer.stop(); |
| dicarloj | 13:41d102a53caf | 43 | timer.reset(); |
| dicarloj | 13:41d102a53caf | 44 | timer.start(); |
| bwang | 75:591556ce033d | 45 | |
| bwang | 75:591556ce033d | 46 | if (blocked) { |
| bwang | 75:591556ce033d | 47 | blocked = false; |
| bwang | 92:a9dac72d8cac | 48 | risen = false; |
| bwang | 75:591556ce033d | 49 | return; |
| bwang | 75:591556ce033d | 50 | } |
| bwang | 75:591556ce033d | 51 | |
| bwang | 150:08c13bfc7417 | 52 | if (risen) usecs = usecs_new; |
| bwang | 92:a9dac72d8cac | 53 | risen = false; |
| dicarloj | 13:41d102a53caf | 54 | } |
| dicarloj | 13:41d102a53caf | 55 | |
| bwang | 16:f283d6032fe5 | 56 | float PwmIn::get_throttle() |
| dicarloj | 13:41d102a53caf | 57 | { |
| bwang | 140:170411d158b5 | 58 | if(timer.read_us() > 40000) { |
| bwang | 140:170411d158b5 | 59 | enabled = false; |
| bwang | 141:ccd21d7fbd1a | 60 | usecs = usec_min; |
| bwang | 140:170411d158b5 | 61 | } |
| dicarloj | 13:41d102a53caf | 62 | if(!enabled) return -1; |
| dicarloj | 13:41d102a53caf | 63 | return constrain(map((float)usecs, usec_min, usec_max, 0, 1), 0, 1); |
| dicarloj | 13:41d102a53caf | 64 | } |
| dicarloj | 13:41d102a53caf | 65 |