motion.h

00001                                      //3 main parameters of the movement:
00002 //float accel (defined in motor.h)
00003 float speed = 1;//cm/s
00004 float radius = 100;
00005 bool tupic = 0;
00006 //int corrected_obstacle [100];
00007 bool infinite_flag = 0;
00008 int fail_counter = 0;
00009 
00010 
00011 
00012 /*void go(float distance_m, bool brake = 1){
00013     target.path = current.path + distance_m; 
00014 //    wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,distance_m,current_speed);
00015 //    while(abs(target.path - current.path) > 0.01){
00016 //        gyro_process();
00017 //        wifi.printf("%.2f %.2f %.2f %.2f;",target_path,current_path,target_path,current_speed);}
00018     }*/
00019     
00020 
00021     
00022 void vstavai(){
00023     //motor_speed[0] = 0;
00024     //motor_speed[1] = 0;
00025     fall_timer = -20;
00026     if (ax < 0){
00027         set_motor_speed(-0.2,-0.2);
00028         wait(1);
00029     }
00030     set_motor_speed(0,0);
00031     target.x = current.x;
00032     target.y = current.y;
00033     target.path = current.path;
00034     target.azimuth = current.azimuth;
00035     fail_counter++;
00036     }
00037     
00038 void free_walk(){
00039     
00040     switch (motion_mode){
00041         case ROTATE:{
00042             if(coord_ready && !scan_360_flag) motion_mode = GO;
00043             break;}
00044         case GO:{
00045             if (timeout_counter-- == 0) {
00046                 target.path = current.path - 0.2;
00047                 timeout_counter = 250;
00048             }
00049             if (current.echo_cm < 30) target.path = current.path + current.echo_cm/100.0 - 0.15;
00050             if (coord_ready) motion_mode = STOP;
00051             break;}
00052         case STOP:{
00053             wifi.putc(current.x * 20); wifi.putc(current.y * 20);
00054             scan_360_counter = 0;
00055             scan_360_flag = 1;
00056             motion_mode = ROTATE;
00057             break;}
00058         }
00059     
00060     
00061     
00062     /*if (motion_mode == GO){
00063         if (timeout_counter-- == 0) {
00064             target.path = current.path - 0.2;
00065             timeout_counter = 250;
00066             }
00067         }
00068         
00069     if (delay_counter-- == 3) {  
00070             glaz.startContinuous(100);
00071             echo_start();
00072             }
00073         if (delay_counter == 0) {
00074             if(motion_mode > STOP) {if(coord_ready) {motion_mode++;if (motion_mode > GO) motion_mode = STOP; delay2_counter = 15;}}
00075             else{
00076                 if (delay2_counter > 0) delay2_counter--;//delay before full stop and scan
00077                 else{   
00078                     obstacle_glaz[serva_counter] = glaz.read()/10;//cm //Function
00079                     glaz.stopContinuous();
00080                     obstacle_echo[serva_counter] = echo_cm;
00081                     if ((obstacle_glaz[serva_counter] > obstacle_echo[serva_counter]) && (obstacle_echo[serva_counter] < 100))
00082                         obstacle[serva_counter] = obstacle_echo[serva_counter]; 
00083                     else obstacle[serva_counter] = obstacle_glaz[serva_counter];
00084                     if(serva_counter < 12)serva(-90 + (serva_counter++)*15); 
00085                     else{
00086                         serva_counter = 0; 
00087                         serva(-90);
00088                         correct_obstacle();
00089                         analyze_obstacle();
00090                         echo_transmit(13);
00091                         //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);             
00092                         motion_mode = ROTATE;
00093                         //motion_timeout = 250; 
00094                         //wait(2);
00095                         }    
00096                     }
00097                 }
00098             delay_counter = 7;
00099             }*/
00100     
00101     }
00102     
00103 void motion_1D(){
00104     static int k;
00105     if (k++ > 2400) k = 0; else { target.path = traj_1D[int(k/200)];}
00106     }
00107     
00108     /*if (max_dist < 0.3) tupic = 1;
00109     if (tupic == 1) {
00110         target.azimuth += pi * (rand() % 10 + 5)/10; if (target.azimuth > pi) target.azimuth -= 2*pi;
00111         target.x = current.x;
00112         target.y = current.y;
00113         }     */
00114     
00115 /*    
00116 void go_no_wait(int cm, bool brake = 1){
00117     if(cm != 0) {
00118         stop_flag = 0; infinite_flag = 0;
00119         motor_enable = 1;
00120         target_path =old_path + cm; old_path = target_path;
00121         if(cm > 0) path_dir = 1; else path_dir = 0;
00122         angle_task = 0;
00123         motor_busy = 1;
00124         //while(motor_busy == 1){proc_counter++;}
00125         }
00126     }
00127 
00128 void turn(int deg, bool brake = 1){
00129     if(deg != 0) {
00130         stop_flag = 0; infinite_flag = 0;
00131         motor_enable = 1;
00132         target_angle = old_angle + deg; old_angle = target_angle;
00133         if(deg > 0) angle_dir = 1; else angle_dir = 0;
00134         angle_task = 1;   
00135         motor_busy = 1;
00136         while(motor_busy == 1){proc_counter++;}
00137         old_path = current_path;
00138         }   
00139     }
00140     
00141 void stop(){
00142     motor_enable = 1;
00143     stop_flag = 1;
00144     motor_busy = 1;
00145     while(motor_busy == 1){}
00146     target_path = current_path;
00147     //motor_enable = 0;
00148     }
00149        
00150 void dance(float dance_speed,float dance_accel){
00151     speed = dance_speed;
00152     accel=dance_accel;
00153     radius=20;
00154     
00155     go(25);
00156     go(-50);
00157     go(25);
00158     turn(90);
00159     speed = -dance_speed;
00160     turn(-180);   
00161     speed = dance_speed;
00162     turn(90);
00163     turn(-360);
00164     turn(360); 
00165     
00166     go(20);
00167     speed = -dance_speed;
00168     turn(-90); 
00169     speed = dance_speed;
00170     turn(-90); 
00171     speed = -dance_speed;
00172     turn(-90); 
00173     speed = dance_speed;
00174     turn(-90); 
00175     go(-20);
00176     
00177     radius =0;
00178     turn(720);
00179     turn(-720);
00180     }
00181     */
00182