File content as of revision 0:e9488589a8ee:

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);
    }