File content as of revision 22:14e85f2068c7:

int obstacle_glaz[100];//13 steps [-90;+90] by 15 deg
int obstacle_echo[100];//13 steps [-90;+90] by 15 deg
int obstacle[100];//13 steps [-90;+90] by 15 deg
int corrected_obstacle [100];
int gabarit_obstacle [100];
int echo_current_step;//[0;36], (step=18) => 0 deg
int echo_old_step;
int echo_old_old_step;
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(){ //begin pulsewidth measurement
    echo_timer.reset();
    }
    
void echo_end(){ //end pulsewidth measurement
    echo_cm = echo_timer.read_us()/58;
    }

void echo_start(){
    sonar_triger = 1;
    tmf=log(1.0);//delay 5us
    tmf=log(2.0);//delay 5us
    sonar_triger = 0;//sonar start
    }
    
void echo_init(){
    echo.rise(&echo_begin);
    echo.fall(&echo_end);
    echo_timer.start();
    }
    
void   echo_transmit(int steps_number)  { //printf is not recommended for interrupts
    int tmstep;
    wifi.putc(0xff); //sync symbol
    for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((gabarit_obstacle[tmstep] < 500) ? gabarit_obstacle[tmstep]/2 : 250);}
    //for(tmstep = 0; tmstep < steps_number; tmstep ++){wifi.putc((obstacle_echo[tmstep] < 500) ? obstacle_echo[tmstep]/2 : 250);}
    //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 echo_scan(){ // rewrite with argument: n or step //will not work in interrupt ??
   serva(-90);
   wait_ms(300);
   for(int point = 0;point <= 12;point ++){
            serva(point*15-90);
            wait_ms(50);
            echo_start();
            wait_ms(50);
            obstacle[point] = echo_cm;}      
   }
   
void echo_step(bool transmit = 0){
    obstacle[echo_old_old_step] = echo_cm;
    //echo_transmit(echo_current_step);
    echo_old_old_step = echo_old_step;
    echo_old_step = echo_current_step;
    echo_current_step += echo_current_dir;
    if (echo_current_step > 36) {echo_current_step = 35 ; echo_current_dir = -1; /*echo_transmit(37);*/} 
    if (echo_current_step < 0) {echo_current_step = 1 ; echo_current_dir = 1; /*echo_transmit(37);*/} 
    serva(echo_current_step*5-90);
    echo_start(); //the last to exit ticker interrupt ASAP
    }
   
void echo_full_scan(int steps){ //scan by wheels (calculate speed)
    
    }

void glaz_init(){
    glaz.setTimeout(500);
    if (!glaz.init())
    {
        wifi.printf("Failed to detect and initialize sensor!");
    }
    glaz.setDistanceMode(VL53L1X::Long);
    glaz.setMeasurementTimingBudget(50000);
    //glaz.startContinuous(50);
    }