Revision 23:bc05a104be10, committed 2019-08-06

Comitter:

dima285

Date:

Tue Aug 06 14:13:55 2019 +0000

Branch:

svoe

Parent:

22:14e85f2068c7

Commit message:

06.08.19

Changed in this revision

--- a/common.h	Tue Jul 23 12:55:23 2019 +0000
+++ b/common.h	Tue Aug 06 14:13:55 2019 +0000
@@ -19,6 +19,8 @@
 float const m_inert = 5e-3;//SI
 float const half_axis = 0.08;//SI
 float const ppr = 3200;//pulses per revolution
+float const deg5_rad = 0.08726646; //5/360*2*pi; //5 gradusov v radianah
+float const pi2_byte = 40.743665431; //256/2/pi;
 
 struct coord{
     float x; //m //vpered
@@ -30,6 +32,8 @@
     float accel;  
     float eps;
     bool dir;
+    int echo_cm;
+    float echo_azimuth;
 };
 
 coord current;
@@ -73,4 +77,9 @@
 #define GO 2
 #define ROTATE 1
 
+bool scan_360_flag;
+int scan_360_counter;
+int array_360_cm[100]; //full scan array
+float array_360_rad[100];
+int echo_delay_counter;
 

--- a/echo.h	Tue Jul 23 12:55:23 2019 +0000
+++ b/echo.h	Tue Aug 06 14:13:55 2019 +0000
@@ -15,6 +15,7 @@
 int max_dist_angle;
 int front_dist;
 
+
 Timer echo_timer;
 InterruptIn echo(PB_12);
 DigitalOut sonar_triger(PB_13);
@@ -48,6 +49,10 @@
     //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle[tmstep] < 500) ? obstacle[tmstep]/2 : 250);}
     for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((corrected_obstacle[tmstep] < 500) ? corrected_obstacle[tmstep]/2 : 250);}
     }
+    
+void transmit_360(){
+    
+    }
 
 void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ??
    serva(-90);
@@ -71,9 +76,136 @@
     serva(echo_current_step*5-90);
     echo_start(); //the last to exit ticker interrupt ASAP
     }
+    void correct_obstacle(){ //gabariti
+    for(int point = 0;point <= 12;point ++){
+        gabarit_obstacle[point] = obstacle[point];
+        }
+    for(int point = 0;point <= 12;point ++){
+        for(int test_point = 0;test_point <= 12;test_point ++){
+            switch(abs(test_point - point)){
+                case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break;
+                case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break;
+                case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break;
+                case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break;
+                }
+            }
+        }
+    /*for(int i = 0;i <= 12;i++){
+       obstacle[i] = corrected_obstacle[i];
+        }*/
+     for(int point = 0;point <= 12;point ++){
+        corrected_obstacle[point] = 0;
+        float tm = 0;
+        for(int test_point = 0;test_point <= 12;test_point ++){
+            corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
+            tm += 1.0/(abs(point-test_point)+1);
+            }
+        corrected_obstacle[point] /= tm;
+        }
+    }
+    
+void correct_obstacle_360(){ //gabariti
+    int tmtp;
+    for(int point = 0;point <= 71;point ++){
+        gabarit_obstacle[point] = obstacle[point];
+        }
+    for(int point = 0;point <= 71;point ++){
+        for(int test_point = point - 12;test_point <= point + 12 ;test_point ++){
+            if (test_point < 0)  tmtp = 72 + test_point; else tmtp = test_point; 
+            if((obstacle[tmtp] < (half_axis + 0.05) * 100 / sin(abs(tmtp - point) * deg5_rad) && (obstacle[tmtp] < obstacle[point]))){
+                gabarit_obstacle[point] = cos(abs(tmtp - point) * deg5_rad) * obstacle[tmtp];
+                }
+            }
+        }
+
+     for(int point = 0;point <= 71;point ++){
+        corrected_obstacle[point] = 0;
+        float tm = 0;
+        for(int test_point = 0;test_point <= 71;test_point ++){
+            corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
+            tm += 1.0/(abs(point-test_point)+1);
+            }
+        corrected_obstacle[point] /= tm;
+        }
+    }
+    
+    
+void analyze_obstacle(){
+    min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6];
+    for(int point = 2;point <= 10;point ++){
+        if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;}
+        if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;}
+        }
+    if (max_dist > 300) max_dist = 300;
+    if (min_dist > 10) {
+        float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5;
+        target.azimuth = current.azimuth + (max_dist_angle)/57.3; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi;
+        target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200);
+        target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200);
+        target.path = current.path + tm;
+        timeout_counter = 250;
+        }
+    else if(min_dist > 0){
+        target.azimuth = current.azimuth + (min_dist_angle-15)/57.3 ; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi;
+        target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200);
+        target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200);
+        target.path = current.path - 0.2;
+        timeout_counter = 250;
+        }
+    delta.path = target.path - current.path;
+    delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi;
+    delta.x = target.x - current.x;
+    delta.y = target.y - current.y;
+    if (delta.path > 0) target.dir = 1; else target.dir = 0;
+    //wifi.printf("!***==========***!\r\n");
+}   
+
+void analize_obstacle_360(){
+    min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[0];
+    for(int point = 0;point <= 71;point ++){
+        if((corrected_obstacle[point] < min_dist) && (corrected_obstacle[point] > 0)) {min_dist = corrected_obstacle[point]; min_dist_angle = array_360_rad[point];}
+        if(corrected_obstacle[point] > max_dist) {max_dist=corrected_obstacle[point]; max_dist_angle = array_360_rad[point];}
+        }
+    if (max_dist < 30){/*vse ploho*/}
+    target.azimuth = max_dist_angle;
+    target.path = current.path + (max_dist - 30) / 100.0;
+    }
    
-void echo_full_scan(int steps){ //scan by wheels (calculate speed)
+    
+void rt_scan(){ //realtime scan
+    int tm_echo;
+    int tm_glaz;
+    
+    tm_glaz = glaz.read()/10;//cm                              //read glaz
+    glaz.stopContinuous();
+    tm_echo = echo_cm;                                          //read echo
+    if ((tm_glaz > tm_echo) && (tm_echo < 100))                  //echo/glaz selection
+       current.echo_cm = tm_echo; 
+    else current.echo_cm = tm_glaz;
+    
+    glaz.startContinuous(100);                                   //start new measurement 
+    echo_start();
     
+    switch (motion_mode){                                         //set new coord
+        case STOP:{break;}
+        case GO:{break;}
+        case ROTATE:{                                               //SCAN 360
+            if (scan_360_flag == 1) {
+                array_360_cm[scan_360_counter] = current.echo_cm;   //save current data
+                array_360_rad[scan_360_counter] = current.azimuth;
+                target.azimuth = current.azimuth + deg5_rad;                     //set new azimuth
+                wifi.putc(current.echo_cm/2);
+                wifi.putc(current.azimuth * pi2_byte);
+                if (scan_360_counter++ > 70) {                      //increment counter
+                    scan_360_counter = 0;                              //finish scan
+                    scan_360_flag = 0;
+                    correct_obstacle_360();
+                    analize_obstacle_360();
+                    }                                 
+                
+                } 
+            break;}
+        }
     }
 
 void glaz_init(){

--- a/main.cpp	Tue Jul 23 12:55:23 2019 +0000
+++ b/main.cpp	Tue Aug 06 14:13:55 2019 +0000
@@ -27,10 +27,12 @@
     serva_counter = 0;
     
     //wait(20);
+   
     
     while(1){
+        //myled = 1; //SMErT' !!!!!!! 
         while(realtime_flag == 0){}
-        myled = !myled;
+        //myled = 0;
         //myled = 0; //inverse connection
         gyro.read(&gx,&gy,&gz,&ax,&ay,&az); //doesn't work in interrupt // reading - 500 uS 
         ax_filter();
@@ -39,6 +41,7 @@
         //balance_coord(); 
         balance_motion_PID();    
         if(external_command != 0) command_process(); 
+        if (echo_delay_counter++ > 2) {echo_delay_counter = 0; rt_scan();}      //echo every 3 csycles
         free_walk();//motion
         //motion_1D();
         

--- a/motion.h	Tue Jul 23 12:55:23 2019 +0000
+++ b/motion.h	Tue Aug 06 14:13:55 2019 +0000
@@ -7,6 +7,8 @@
 bool infinite_flag = 0;
 int fail_counter = 0;
 
+
+
 /*void go(float distance_m, bool brake = 1){
     target.path = current.path + distance_m; 
 //    wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed);
@@ -15,64 +17,7 @@
 //        wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
     }*/
     
-void correct_obstacle(){ //gabariti
-    for(int point = 0;point <= 12;point ++){
-        gabarit_obstacle[point] = obstacle[point];
-        }
-    for(int point = 0;point <= 12;point ++){
-        for(int test_point = 0;test_point <= 12;test_point ++){
-            switch(abs(test_point - point)){
-                case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break;
-                case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break;
-                case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break;
-                case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break;
-                }
-            }
-        }
-    /*for(int i = 0;i <= 12;i++){
-       obstacle[i] = corrected_obstacle[i];
-        }*/
-     for(int point = 0;point <= 12;point ++){
-        corrected_obstacle[point] = 0;
-        float tm = 0;
-        for(int test_point = 0;test_point <= 12;test_point ++){
-            corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
-            tm += 1.0/(abs(point-test_point)+1);
-            }
-        corrected_obstacle[point] /= tm;
-        }
-    }
-    
-    
-void analyze_obstacle(){
-    min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6];
-    for(int point = 2;point <= 10;point ++){
-        if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;}
-        if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;}
-        }
-    if (max_dist > 300) max_dist = 300;
-    if (min_dist > 10) {
-        float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5;
-        target.azimuth = current.azimuth + (max_dist_angle)/57.3; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi;
-        target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200);
-        target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200);
-        target.path = current.path + tm;
-        timeout_counter = 250;
-        }
-    else if(min_dist > 0){
-        target.azimuth = current.azimuth + (min_dist_angle-15)/57.3 ; if (current.azimuth > pi) current.azimuth -= 2*pi; if (current.azimuth < -pi) current.azimuth += 2*pi;
-        target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200);
-        target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200);
-        target.path = current.path - 0.2;
-        timeout_counter = 250;
-        }
-    delta.path = target.path - current.path;
-    delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi;
-    delta.x = target.x - current.x;
-    delta.y = target.y - current.y;
-    if (delta.path > 0) target.dir = 1; else target.dir = 0;
-    //wifi.printf("!***==========***!\r\n");
-}   
+
     
 void vstavai(){
     //motor_speed[0] = 0;
@@ -91,7 +36,30 @@
     }
     
 void free_walk(){
-    if (motion_mode == GO){
+    
+    switch (motion_mode){
+        case ROTATE:{
+            if(coord_ready && !scan_360_flag) motion_mode = GO;
+            break;}
+        case GO:{
+            if (timeout_counter-- == 0) {
+                target.path = current.path - 0.2;
+                timeout_counter = 250;
+            }
+            if (current.echo_cm < 30) target.path = current.path + current.echo_cm/100.0 - 0.15;
+            if (coord_ready) motion_mode = STOP;
+            break;}
+        case STOP:{
+            wifi.putc(current.x * 20); wifi.putc(current.y * 20);
+            scan_360_counter = 0;
+            scan_360_flag = 1;
+            motion_mode = ROTATE;
+            break;}
+        }
+    
+    
+    
+    /*if (motion_mode == GO){
         if (timeout_counter-- == 0) {
             target.path = current.path - 0.2;
             timeout_counter = 250;
@@ -128,7 +96,7 @@
                     }
                 }
             delay_counter = 7;
-            }
+            }*/
     
     }
     

--- a/sound.h	Tue Jul 23 12:55:23 2019 +0000
+++ b/sound.h	Tue Aug 06 14:13:55 2019 +0000
@@ -29,7 +29,7 @@
 
 void voice_command_process(){ 
     if(sound_in.readable()){
-        if (myled == 0 ) myled = 1;else myled = 0;
+        //if (myled == 0 ) myled = 1;else myled = 0;
         char tmc = sound_in.getc();
         wifi.putc(tmc);
         if (tmc == 0xAA) sound_in_receive_counter = 0; else sound_in_receive_counter++; if(sound_in_receive_counter > 15) sound_in_receive_counter = 15;