Diff: echo.h

Revision:

0:e9488589a8ee

Child:

1:e2a6e523bf1f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/echo.h	Tue May 02 16:50:18 2017 +0000
@@ -0,0 +1,66 @@
+int obstacle[20];//13 steps [-90;+90] by 15 deg
+int echo_current_step;//[0;12], step = 6 => 0 deg
+int echo_current_dir = -1;
+int echo_cm;
+float tmf;
+int min_dist;
+int min_dist_angle;
+int max_dist;
+int max_dist_angle;
+int front_dist;
+
+
+
+Timer echo_timer;
+InterruptIn echo(PB_12);
+DigitalOut sonar_triger(PB_13);
+
+void echo_begin(){
+    echo_timer.reset();
+    }
+    
+void echo_end(){
+    echo_cm = echo_timer.read_us()/58;
+    }
+
+void search(){
+   serva(-80);
+   wait_ms(200);
+   for(int point = 0;point <= 12;point ++){
+            serva(point*15-90);
+            wait_ms(100);
+            obstacle[point]=echo_cm;}      
+   }
+   
+void echo_start(){
+    sonar_triger = 1;
+    tmf=log(1.0);//delay 5us
+    tmf=log(1.0);//delay 5us
+    //for(int i = 0; i<2000; i++) {tmi++;}
+    sonar_triger = 0;//sonar start
+    }
+    
+void echo_init(){
+    echo.rise(&echo_begin);
+    echo.fall(&echo_end);
+    echo_timer.start();
+    }
+    
+void echo_step(){
+    obstacle[echo_current_step]=echo_cm;
+    echo_current_step += echo_current_dir;
+    if (echo_current_step > 12) {echo_current_step = 11 ; echo_current_dir = -1;} 
+    if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1;} 
+    serva(echo_current_step*15-90);
+    echo_start();
+    }
+    
+bool analyze_obstacle(){
+    min_dist = 400;max_dist = 0; front_dist = 400;
+    for(int point = 0;point <= 12;point ++){
+        if(obstacle[point]<min_dist) {min_dist=obstacle[point];min_dist_angle = point*15-90;}
+        if(obstacle[point]>max_dist) {max_dist=obstacle[point];max_dist_angle = point*15-90;}
+        if((obstacle[point]*abs(point-6)*3<15)&&(obstacle[point]<front_dist)) {front_dist=obstacle[point];}
+        }
+    if(front_dist < 20) return(1); else return(0);
+    }
\ No newline at end of file