File content as of revision 17:bd6b6ac89e0e:

                                     //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;

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(){
    for(int point = 1;point <= 12;point ++){
        for(int test_point = 1;test_point <= 12;test_point ++){
            switch(abs(test_point - point)){
                case 1: if((obstacle[test_point] < 40) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = obstacle[test_point];break;
                case 2: if((obstacle[test_point] < 20) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.85 * obstacle[test_point];break;
                case 3: if((obstacle[test_point] < 15) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.7 * obstacle[test_point];break;
                case 4: if((obstacle[test_point] < 12) && (obstacle[test_point] < obstacle[point])) corrected_obstacle[point] = 0.5 * obstacle[test_point];break;
                }
            }
        }
    /*for(int i = 0;i <= 12;i++){
       obstacle[i] = corrected_obstacle[i];
        }*/
    }
    
    
void analyze_obstacle(){
    min_dist = 400; max_dist = 0; front_dist = obstacle[6];
    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;}
        }
    target.azimuth = current.azimuth + max_dist_angle/57.3;
    target.x = current.x + 0.01*(cos(target.azimuth) * 25);//(max_dist - 200);
    target.y = current.y + 0.01*(sin(target.azimuth) * 25);//(max_dist - 200);
    /*if (max_dist < 0.3) tupic = 1;
    if (tupic == 1) {
        target.azimuth += pi * (rand() % 10 + 5)/10; if (target.azimuth > pi) target.azimuth -= 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);
    }
    */