File content as of revision 22:14e85f2068c7:

                                     //3 main parameters of the movement:
//float accel (defined in motor.h)
float speed = 1;//cm/s
float radius = 100;
bool tupic = 0;
//int corrected_obstacle [100];
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);
//    while(abs(target.path - current.path) > 0.01){
//        gyro_process();
//        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;
    //motor_speed[1] = 0;
    fall_timer = -20;
    if (ax < 0){
        set_motor_speed(-0.2,-0.2);
        wait(1);
    }
    set_motor_speed(0,0);
    target.x = current.x;
    target.y = current.y;
    target.path = current.path;
    target.azimuth = current.azimuth;
    fail_counter++;
    }
    
void free_walk(){
    if (motion_mode == GO){
        if (timeout_counter-- == 0) {
            target.path = current.path - 0.2;
            timeout_counter = 250;
            }
        }
        
    if (delay_counter-- == 3) {  
            glaz.startContinuous(100);
            echo_start();
            }
        if (delay_counter == 0) {
            if(motion_mode > STOP) {if(coord_ready) {motion_mode++;if (motion_mode > GO) motion_mode = STOP; delay2_counter = 15;}}
            else{
                if (delay2_counter > 0) delay2_counter--;//delay before full stop and scan
                else{   
                    obstacle_glaz[serva_counter] = glaz.read()/10;//cm //Function
                    glaz.stopContinuous();
                    obstacle_echo[serva_counter] = echo_cm;
                    if ((obstacle_glaz[serva_counter] > obstacle_echo[serva_counter]) && (obstacle_echo[serva_counter] < 100))
                        obstacle[serva_counter] = obstacle_echo[serva_counter]; 
                    else obstacle[serva_counter] = obstacle_glaz[serva_counter];
                    if(serva_counter < 12)serva(-90 + (serva_counter++)*15); 
                    else{
                        serva_counter = 0; 
                        serva(-90);
                        correct_obstacle();
                        analyze_obstacle();
                        echo_transmit(13);
                        //wifi.putc(current.x * 10); wifi.putc(current.y * 10); wifi.putc(current.azimuth * 40); wifi.putc(target.x * 10); wifi.putc(target.y * 10); wifi.putc(target.azimuth * 40);             
                        motion_mode = ROTATE;
                        //motion_timeout = 250; 
                        //wait(2);
                        }    
                    }
                }
            delay_counter = 7;
            }
    
    }
    
void motion_1D(){
    static int k;
    if (k++ > 2400) k = 0; else { target.path = traj_1D[int(k/200)];}
    }
    
    /*if (max_dist < 0.3) tupic = 1;
    if (tupic == 1) {
        target.azimuth += pi * (rand() % 10 + 5)/10; if (target.azimuth > pi) target.azimuth -= 2*pi;
        target.x = current.x;
        target.y = current.y;
        }     */
    
/*    
void go_no_wait(int cm, bool brake = 1){
    if(cm != 0) {
        stop_flag = 0; infinite_flag = 0;
        motor_enable = 1;
        target_path =old_path + cm; old_path = target_path;
        if(cm > 0) path_dir = 1; else path_dir = 0;
        angle_task = 0;
        motor_busy = 1;
        //while(motor_busy == 1){proc_counter++;}
        }
    }

void turn(int deg, bool brake = 1){
    if(deg != 0) {
        stop_flag = 0; infinite_flag = 0;
        motor_enable = 1;
        target_angle = old_angle + deg; old_angle = target_angle;
        if(deg > 0) angle_dir = 1; else angle_dir = 0;
        angle_task = 1;   
        motor_busy = 1;
        while(motor_busy == 1){proc_counter++;}
        old_path = current_path;
        }   
    }
    
void stop(){
    motor_enable = 1;
    stop_flag = 1;
    motor_busy = 1;
    while(motor_busy == 1){}
    target_path = current_path;
    //motor_enable = 0;
    }
       
void dance(float dance_speed,float dance_accel){
    speed = dance_speed;
    accel=dance_accel;
    radius=20;
    
    go(25);
    go(-50);
    go(25);
    turn(90);
    speed = -dance_speed;
    turn(-180);   
    speed = dance_speed;
    turn(90);
    turn(-360);
    turn(360); 
    
    go(20);
    speed = -dance_speed;
    turn(-90); 
    speed = dance_speed;
    turn(-90); 
    speed = -dance_speed;
    turn(-90); 
    speed = dance_speed;
    turn(-90); 
    go(-20);
    
    radius =0;
    turn(720);
    turn(-720);
    }
    */