File content as of revision 23:bc05a104be10:

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 transmit_360(){
    
    }

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 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 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(){
    glaz.setTimeout(500);
    if (!glaz.init())
    {
        wifi.printf("Failed to detect and initialize sensor!");
    }
    glaz.setDistanceMode(VL53L1X::Long);
    glaz.setMeasurementTimingBudget(50000);
    //glaz.startContinuous(50);
    }