echo.h

00001 int obstacle_glaz[100];//13 steps [-90;+90] by 15 deg
00002 int obstacle_echo[100];//13 steps [-90;+90] by 15 deg
00003 int obstacle[100];//13 steps [-90;+90] by 15 deg
00004 int corrected_obstacle [100];
00005 int gabarit_obstacle [100];
00006 int echo_current_step;//[0;36], (step=18) => 0 deg
00007 int echo_old_step;
00008 int echo_old_old_step;
00009 int echo_current_dir = -1;
00010 int echo_cm;
00011 float tmf;
00012 int min_dist;
00013 int min_dist_angle;
00014 int max_dist;
00015 int max_dist_angle;
00016 int front_dist;
00017 
00018 
00019 Timer echo_timer;
00020 InterruptIn echo(PB_12);
00021 DigitalOut sonar_triger(PB_13);
00022 
00023 void echo_begin(){ //begin pulsewidth measurement
00024     echo_timer.reset();
00025     }
00026     
00027 void echo_end(){ //end pulsewidth measurement
00028     echo_cm = echo_timer.read_us()/58;
00029     }
00030 
00031 void echo_start(){
00032     sonar_triger = 1;
00033     tmf=log(1.0);//delay 5us
00034     tmf=log(2.0);//delay 5us
00035     sonar_triger = 0;//sonar start
00036     }
00037     
00038 void echo_init(){
00039     echo.rise(&echo_begin);
00040     echo.fall(&echo_end);
00041     echo_timer.start();
00042     }
00043     
00044 void   echo_transmit(int steps_number)  { //printf is not recommended for interrupts
00045     int tmstep;
00046     wifi.putc(0xff); //sync symbol
00047     for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((gabarit_obstacle[tmstep] < 500) ? gabarit_obstacle[tmstep]/2 : 250);}
00048     //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle_echo[tmstep] < 500) ? obstacle_echo[tmstep]/2 : 250);}
00049     //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle[tmstep] < 500) ? obstacle[tmstep]/2 : 250);}
00050     for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((corrected_obstacle[tmstep] < 500) ? corrected_obstacle[tmstep]/2 : 250);}
00051     }
00052     
00053 void transmit_360(){
00054     
00055     }
00056 
00057 void echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ??
00058    serva(-90);
00059    wait_ms(300);
00060    for(int point = 0;point <= 12;point ++){
00061             serva(point*15-90);
00062             wait_ms(50);
00063             echo_start();
00064             wait_ms(50);
00065             obstacle[point] = echo_cm;}      
00066    }
00067    
00068 void echo_step(bool transmit = 0){
00069     obstacle[echo_old_old_step] = echo_cm;
00070     //echo_transmit(echo_current_step);
00071     echo_old_old_step = echo_old_step;
00072     echo_old_step = echo_current_step;
00073     echo_current_step += echo_current_dir;
00074     if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; /*echo_transmit(37);*/} 
00075     if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; /*echo_transmit(37);*/} 
00076     serva(echo_current_step*5-90);
00077     echo_start(); //the last to exit ticker interrupt ASAP
00078     }
00079     void correct_obstacle(){ //gabariti
00080     for(int point = 0;point <= 12;point ++){
00081         gabarit_obstacle[point] = obstacle[point];
00082         }
00083     for(int point = 0;point <= 12;point ++){
00084         for(int test_point = 0;test_point <= 12;test_point ++){
00085             switch(abs(test_point - point)){
00086                 case 1: if((obstacle[test_point] < 60) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = obstacle[test_point]; break;
00087                 case 2: if((obstacle[test_point] < 30) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.85 * obstacle[test_point];break;
00088                 case 3: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.7 * obstacle[test_point];break;
00089                 case 4: if((obstacle[test_point] < 18) && (obstacle[test_point] < obstacle[point])) gabarit_obstacle[point] = 0.5 * obstacle[test_point];break;
00090                 }
00091             }
00092         }
00093     /*for(int i = 0;i <= 12;i++){
00094        obstacle[i] = corrected_obstacle[i];
00095         }*/
00096      for(int point = 0;point <= 12;point ++){
00097         corrected_obstacle[point] = 0;
00098         float tm = 0;
00099         for(int test_point = 0;test_point <= 12;test_point ++){
00100             corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
00101             tm += 1.0/(abs(point-test_point)+1);
00102             }
00103         corrected_obstacle[point] /= tm;
00104         }
00105     }
00106     
00107 void correct_obstacle_360(){ //gabariti
00108     int tmtp;
00109     for(int point = 0;point <= 71;point ++){
00110         gabarit_obstacle[point] = obstacle[point];
00111         }
00112     for(int point = 0;point <= 71;point ++){
00113         for(int test_point = point - 12;test_point <= point + 12 ;test_point ++){
00114             if (test_point < 0)  tmtp = 72 + test_point; else tmtp = test_point; 
00115             if((obstacle[tmtp] < (half_axis + 0.05) * 100 / sin(abs(tmtp - point) * deg5_rad) && (obstacle[tmtp] < obstacle[point]))){
00116                 gabarit_obstacle[point] = cos(abs(tmtp - point) * deg5_rad) * obstacle[tmtp];
00117                 }
00118             }
00119         }
00120 
00121      for(int point = 0;point <= 71;point ++){
00122         corrected_obstacle[point] = 0;
00123         float tm = 0;
00124         for(int test_point = 0;test_point <= 71;test_point ++){
00125             corrected_obstacle[point] += gabarit_obstacle[test_point]/(abs(point-test_point)+1);
00126             tm += 1.0/(abs(point-test_point)+1);
00127             }
00128         corrected_obstacle[point] /= tm;
00129         }
00130     }
00131     
00132     
00133 void analyze_obstacle(){
00134     min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[6];
00135     for(int point = 2;point <= 10;point ++){
00136         if((corrected_obstacle[point]<min_dist) && (corrected_obstacle[point] > 0)) {min_dist=corrected_obstacle[point];min_dist_angle = point*15-90;}
00137         if(corrected_obstacle[point]>max_dist) {max_dist=corrected_obstacle[point];max_dist_angle = point*15-90;}
00138         }
00139     if (max_dist > 300) max_dist = 300;
00140     if (min_dist > 10) {
00141         float tm = float(max_dist - 30) / 100; //if (tm > 0.5) tm = 0.5;
00142         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;
00143         target.x = current.x + cos(target.azimuth) * tm;//(max_dist - 200);
00144         target.y = current.y + sin(target.azimuth) * tm;//(max_dist - 200);
00145         target.path = current.path + tm;
00146         timeout_counter = 250;
00147         }
00148     else if(min_dist > 0){
00149         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;
00150         target.x = current.x - 0.01*(cos(target.azimuth) * 20);//(max_dist - 200);
00151         target.y = current.y - 0.01*(sin(target.azimuth) * 20);//(max_dist - 200);
00152         target.path = current.path - 0.2;
00153         timeout_counter = 250;
00154         }
00155     delta.path = target.path - current.path;
00156     delta.azimuth = target.azimuth - current.azimuth; if(delta.azimuth > pi) delta.azimuth -= 2*pi; if(delta.azimuth < -pi) delta.azimuth += 2*pi;
00157     delta.x = target.x - current.x;
00158     delta.y = target.y - current.y;
00159     if (delta.path > 0) target.dir = 1; else target.dir = 0;
00160     //wifi.printf("!***==========***!\r\n");
00161 }   
00162 
00163 void analize_obstacle_360(){
00164     min_dist = 400; max_dist = 0; front_dist = corrected_obstacle[0];
00165     for(int point = 0;point <= 71;point ++){
00166         if((corrected_obstacle[point] < min_dist) && (corrected_obstacle[point] > 0)) {min_dist = corrected_obstacle[point]; min_dist_angle = array_360_rad[point];}
00167         if(corrected_obstacle[point] > max_dist) {max_dist=corrected_obstacle[point]; max_dist_angle = array_360_rad[point];}
00168         }
00169     if (max_dist < 30){/*vse ploho*/}
00170     target.azimuth = max_dist_angle;
00171     target.path = current.path + (max_dist - 30) / 100.0;
00172     }
00173    
00174     
00175 void rt_scan(){ //realtime scan
00176     int tm_echo;
00177     int tm_glaz;
00178     
00179     tm_glaz = glaz.read()/10;//cm                              //read glaz
00180     glaz.stopContinuous();
00181     tm_echo = echo_cm;                                          //read echo
00182     if ((tm_glaz > tm_echo) && (tm_echo < 100))                  //echo/glaz selection
00183        current.echo_cm = tm_echo; 
00184     else current.echo_cm = tm_glaz;
00185     
00186     glaz.startContinuous(100);                                   //start new measurement 
00187     echo_start();
00188     
00189     switch (motion_mode){                                         //set new coord
00190         case STOP:{break;}
00191         case GO:{break;}
00192         case ROTATE:{                                               //SCAN 360
00193             if (scan_360_flag == 1) {
00194                 array_360_cm[scan_360_counter] = current.echo_cm;   //save current data
00195                 array_360_rad[scan_360_counter] = current.azimuth;
00196                 target.azimuth = current.azimuth + deg5_rad;                     //set new azimuth
00197                 wifi.putc(current.echo_cm/2);
00198                 wifi.putc(current.azimuth * pi2_byte);
00199                 if (scan_360_counter++ > 70) {                      //increment counter
00200                     scan_360_counter = 0;                              //finish scan
00201                     scan_360_flag = 0;
00202                     correct_obstacle_360();
00203                     analize_obstacle_360();
00204                     }                                 
00205                 
00206                 } 
00207             break;}
00208         }
00209     }
00210 
00211 void glaz_init(){
00212     glaz.setTimeout(500);
00213     if (!glaz.init())
00214     {
00215         wifi.printf("Failed to detect and initialize sensor!");
00216     }
00217     glaz.setDistanceMode(VL53L1X::Long);
00218     glaz.setMeasurementTimingBudget(50000);
00219     //glaz.startContinuous(50);
00220     }