File content as of revision 23:bc05a104be10:

                                     //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 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(){
    
    switch (motion_mode){
        case ROTATE:{
            if(coord_ready && !scan_360_flag) motion_mode = GO;
            break;}
        case GO:{
            if (timeout_counter-- == 0) {
                target.path = current.path - 0.2;
                timeout_counter = 250;
            }
            if (current.echo_cm < 30) target.path = current.path + current.echo_cm/100.0 - 0.15;
            if (coord_ready) motion_mode = STOP;
            break;}
        case STOP:{
            wifi.putc(current.x * 20); wifi.putc(current.y * 20);
            scan_360_counter = 0;
            scan_360_flag = 1;
            motion_mode = ROTATE;
            break;}
        }
    
    
    
    /*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);
    }
    */