Delta for AGV
Dependencies: mbed HC-SR04 ros_lib_kinetic
Diff: main.cpp
- Revision:
- 4:052ac3f5c938
- Parent:
- 3:3297ea6e3ae1
- Child:
- 5:889566fb3a85
- Child:
- 6:bca0839e8295
diff -r 3297ea6e3ae1 -r 052ac3f5c938 main.cpp --- a/main.cpp Mon Dec 07 09:38:35 2015 +0000 +++ b/main.cpp Mon Dec 07 10:44:07 2015 +0000 @@ -1,15 +1,32 @@ #include "mbed.h" #include "HCSR04.h" +#include "AutomationElements.h" Serial pc(USBTX, USBRX); +HCSR04 sensor(p5, p7); +float sampleTime = 0.1; +PT1 filter(1, 2, sampleTime); +Ticker ticker; +float distance; +float filteredDistance; +DigitalOut led1(LED1); + +void calc() { + led1 = 1; + distance = sensor.getDistance_mm(); + filter.in(distance); + filteredDistance = filter.out(); + pc.printf("%7.1f mm %7.1f mm \r", distance, filteredDistance); + led1 = 0; +} int main() { - HCSR04 sensor(p5, p7); sensor.setRanges(10, 110); - pc.printf("Min. range = %g cm\n\rMax. range = %g cm\n\r", - sensor.getMinRange(), sensor.getMaxRange()); + pc.printf("Minimum sensor range = %g cm\n\rMaximum sensor range = %g cm\n\r", sensor.getMinRange(), sensor.getMaxRange()); + pc.printf("Sensor: Filtered:\n\r"); + ticker.attach(&calc, sampleTime); + while(1) { - pc.printf("Distance: %5.1f mm\r", sensor.getDistance_mm()); wait_ms(500); } } \ No newline at end of file