Cooper Liu
/
Sonar_Robot
main.cpp@0:9352a72a9d85, 2012-03-14 (annotated)
- Committer:
- xiaxia686
- Date:
- Wed Mar 14 17:05:44 2012 +0000
- Revision:
- 0:9352a72a9d85
code for sonar localisation on robot side
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
xiaxia686 | 0:9352a72a9d85 | 1 | #include "mbed.h" |
xiaxia686 | 0:9352a72a9d85 | 2 | #include "RFSRF05.h" |
xiaxia686 | 0:9352a72a9d85 | 3 | |
xiaxia686 | 0:9352a72a9d85 | 4 | DigitalOut Led_1(LED1); |
xiaxia686 | 0:9352a72a9d85 | 5 | DigitalOut Led_2(LED2); |
xiaxia686 | 0:9352a72a9d85 | 6 | |
xiaxia686 | 0:9352a72a9d85 | 7 | RFSRF05 my_srf(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9); |
xiaxia686 | 0:9352a72a9d85 | 8 | |
xiaxia686 | 0:9352a72a9d85 | 9 | Serial pc(USBTX, USBRX); // tx, rx |
xiaxia686 | 0:9352a72a9d85 | 10 | |
xiaxia686 | 0:9352a72a9d85 | 11 | float Distance[3],Distance_Shortest,Distance_Largest; |
xiaxia686 | 0:9352a72a9d85 | 12 | volatile char SendByte; |
xiaxia686 | 0:9352a72a9d85 | 13 | |
xiaxia686 | 0:9352a72a9d85 | 14 | int main() { |
xiaxia686 | 0:9352a72a9d85 | 15 | pc.printf("Robot Side Side \n"); |
xiaxia686 | 0:9352a72a9d85 | 16 | while (1) { |
xiaxia686 | 0:9352a72a9d85 | 17 | Led_2 = 1; |
xiaxia686 | 0:9352a72a9d85 | 18 | wait_ms(100); |
xiaxia686 | 0:9352a72a9d85 | 19 | Distance[0] = my_srf.read0(); |
xiaxia686 | 0:9352a72a9d85 | 20 | Distance[1] = my_srf.read1(); |
xiaxia686 | 0:9352a72a9d85 | 21 | Distance[2] = my_srf.read2(); |
xiaxia686 | 0:9352a72a9d85 | 22 | pc.printf("%.1f, %.1f, %.1f \n",Distance[0],Distance[1],Distance[2]); |
xiaxia686 | 0:9352a72a9d85 | 23 | // pc.printf("Distance0 = %.1f\n",Distance[0]); |
xiaxia686 | 0:9352a72a9d85 | 24 | // pc.printf("Distance1 = %.1f\n",Distance[1]); |
xiaxia686 | 0:9352a72a9d85 | 25 | // pc.printf("Distance2 = %.1f\n",Distance[2]); |
xiaxia686 | 0:9352a72a9d85 | 26 | // pc.printf("\n\r"); |
xiaxia686 | 0:9352a72a9d85 | 27 | Led_2 = 0; |
xiaxia686 | 0:9352a72a9d85 | 28 | wait_ms(50); |
xiaxia686 | 0:9352a72a9d85 | 29 | |
xiaxia686 | 0:9352a72a9d85 | 30 | |
xiaxia686 | 0:9352a72a9d85 | 31 | |
xiaxia686 | 0:9352a72a9d85 | 32 | } |
xiaxia686 | 0:9352a72a9d85 | 33 | } |