Delta for AGV
Dependencies: mbed HC-SR04 ros_lib_kinetic
main.cpp@5:889566fb3a85, 2015-12-07 (annotated)
- Committer:
- tbjazic
- Date:
- Mon Dec 07 11:45:44 2015 +0000
- Revision:
- 5:889566fb3a85
- Parent:
- 4:052ac3f5c938
Example of filtering the distance with PT1 filter.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tbjazic | 0:ebee649c5b1b | 1 | #include "mbed.h" |
tbjazic | 2:f86b1e3609b3 | 2 | #include "HCSR04.h" |
tbjazic | 4:052ac3f5c938 | 3 | #include "AutomationElements.h" |
tbjazic | 0:ebee649c5b1b | 4 | |
tbjazic | 3:3297ea6e3ae1 | 5 | Serial pc(USBTX, USBRX); |
tbjazic | 4:052ac3f5c938 | 6 | HCSR04 sensor(p5, p7); |
tbjazic | 4:052ac3f5c938 | 7 | float sampleTime = 0.1; |
tbjazic | 5:889566fb3a85 | 8 | PT1 filter(1, 2, sampleTime); // filter with a time constant of 2 seconds |
tbjazic | 5:889566fb3a85 | 9 | float distance = 0; |
tbjazic | 5:889566fb3a85 | 10 | float filteredDistance = 0; |
tbjazic | 5:889566fb3a85 | 11 | Timer timer; |
tbjazic | 4:052ac3f5c938 | 12 | |
tbjazic | 5:889566fb3a85 | 13 | int main() { |
tbjazic | 5:889566fb3a85 | 14 | sensor.setRanges(4, 400); |
tbjazic | 5:889566fb3a85 | 15 | pc.printf("Minimum sensor range = %g cm\n\rMaximum sensor range = %g cm\n\r", sensor.getMinRange(), sensor.getMaxRange()); |
tbjazic | 5:889566fb3a85 | 16 | pc.printf("Sensor: Filtered:\n\r"); |
tbjazic | 5:889566fb3a85 | 17 | |
tbjazic | 5:889566fb3a85 | 18 | timer.start(); |
tbjazic | 4:052ac3f5c938 | 19 | distance = sensor.getDistance_mm(); |
tbjazic | 5:889566fb3a85 | 20 | timer.stop(); |
tbjazic | 5:889566fb3a85 | 21 | int time1 = timer.read_us(); |
tbjazic | 5:889566fb3a85 | 22 | pc.printf("Time to get distance: %d us \n\r", time1); |
tbjazic | 5:889566fb3a85 | 23 | timer.reset(); |
tbjazic | 5:889566fb3a85 | 24 | |
tbjazic | 5:889566fb3a85 | 25 | timer.start(); |
tbjazic | 4:052ac3f5c938 | 26 | filter.in(distance); |
tbjazic | 4:052ac3f5c938 | 27 | filteredDistance = filter.out(); |
tbjazic | 5:889566fb3a85 | 28 | timer.stop(); |
tbjazic | 5:889566fb3a85 | 29 | int time2 = timer.read_us(); |
tbjazic | 5:889566fb3a85 | 30 | pc.printf("Time to filter result: %d us \n\r", time2); |
tbjazic | 5:889566fb3a85 | 31 | timer.reset(); |
tbjazic | 5:889566fb3a85 | 32 | |
tbjazic | 5:889566fb3a85 | 33 | timer.start(); |
tbjazic | 4:052ac3f5c938 | 34 | pc.printf("%7.1f mm %7.1f mm \r", distance, filteredDistance); |
tbjazic | 5:889566fb3a85 | 35 | timer.stop(); |
tbjazic | 5:889566fb3a85 | 36 | int time3 = timer.read_us(); |
tbjazic | 5:889566fb3a85 | 37 | pc.printf("Time to printf one line: %d us \n\r", time3); |
tbjazic | 5:889566fb3a85 | 38 | timer.reset(); |
tbjazic | 5:889566fb3a85 | 39 | |
tbjazic | 4:052ac3f5c938 | 40 | pc.printf("Sensor: Filtered:\n\r"); |
tbjazic | 4:052ac3f5c938 | 41 | |
tbjazic | 0:ebee649c5b1b | 42 | while(1) { |
tbjazic | 5:889566fb3a85 | 43 | distance = sensor.getDistance_mm(); |
tbjazic | 5:889566fb3a85 | 44 | filter.in(distance); |
tbjazic | 5:889566fb3a85 | 45 | filteredDistance = filter.out(); |
tbjazic | 5:889566fb3a85 | 46 | pc.printf("%7.1f mm %7.1f mm \r", distance, filteredDistance); |
tbjazic | 5:889566fb3a85 | 47 | wait_us(sampleTime*1e6 - time1 - time2 - time3); |
tbjazic | 0:ebee649c5b1b | 48 | } |
tbjazic | 0:ebee649c5b1b | 49 | } |