File content as of revision 7:9e4a997ad23a:

                                     //3 main parameters of the movement:
//float accel (defined in motor.h)
float speed = 100;//cm/s
float radius = 10;

float half_width = 8.5;
float current_x, current_y;
float current_path, target_path;//cm
float current_angle, target_angle;//deg
float old_path = 0;
float old_angle = 0;
bool volatile motor_busy;
bool path_dir, angle_dir;
bool angle_task;//1 - angle, 0 - path
bool stop_flag = 0;
bool infinite_flag = 0;

//Timer trap_timer;

void motion_init(){
    //trap_timer.start();
    }
    
void go(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);
    }