realtime.h

00001 Ticker rt_ticker;
00002 bool realtime_flag;
00003 
00004 
00005 void balance_coord(){
00006     float delta_x = (target.x - current.x); if (delta_x == 0) delta_x = 0.0001;
00007     float delta_y = (target.y - current.y);
00008     float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y); //always positive        //if (delta_x < 0) {delta_s = -delta_s;}
00009     float azimuth_to_target = atan(delta_y/delta_x); if (delta_x < 0){if (delta_y > 0) azimuth_to_target += pi; else azimuth_to_target -= pi;}
00010     
00011     float delta_phi_0 = azimuth_to_target - current.azimuth;                                                                                                    //azimuth: target error
00012     if (delta_phi_0 < -pi) delta_phi_0 += 2*pi; if (delta_phi_0 > pi) delta_phi_0 -= 2*pi;
00013     
00014     float normal_error  = delta_s * sin(delta_phi_0);
00015     float tan_error = delta_s * cos(delta_phi_0);
00016     
00017     float a = x_prop * tan_error - x_diff * current.speed;                                                                                                      //Motion PID
00018     if ((current.speed > max.speed) && (a > 0)) a = max.accel * (2 - current.speed/max.speed); if ((current.speed < -max.speed) && (a < 0)) a = -max.accel * (2 + current.speed/max.speed); 
00019     if (a > max.accel) a = max.accel ; if (a < -max.accel) a = -max.accel;
00020 
00021     float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step;                                                                                         //Balance PID
00022        
00023     float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s);                                                              //azimuth: sever error
00024     if (delta_phi_1 < -pi) delta_phi_1 += 2*pi; if (delta_phi_1 > pi) delta_phi_1 -= 2*pi;
00025     if (delta_phi_1 < -pi/2) delta_phi_1 = -pi/2; if (delta_phi_1 > pi/2) delta_phi_1 = pi/2;
00026 
00027     if (delta_phi_0 < -pi/2) delta_phi_0 += pi; if (delta_phi_0 > pi/2) delta_phi_0 -= pi;    //Normalization [-pi/2; pi/2] - (zadom/peredom)
00028 //    float delta_phi = cos(delta_s/coord_accuracy) * coord_accuracy/(delta_s + coord_accuracy) * 1.0 * delta_phi_1 + abs(tan_error) /*delta_s*/ /(delta_s + coord_accuracy) * delta_phi_0;   
00029     float delta_phi = cos(delta_s/coord_accuracy) * exp(-delta_s/coord_accuracy) * delta_phi_1 + (1-exp(-delta_s/coord_accuracy)) * abs(tan_error) /(coord_accuracy) * delta_phi_0;  //fusion of delta_phi_0 and delta_phi_1 + uprezhdenie 
00030     if (delta_phi > pi/2) delta_phi -= pi; if (delta_phi < -pi/2) delta_phi += pi;    
00031 
00032     float eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; if (eps > max.eps) eps = max.eps; if (eps < -max.eps) eps = -max.eps;                  //Azimuth PID
00033     float delta_omega = eps * t_step;
00034     
00035     //wifi.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f;\r\n",current.x*100, current.y*100, current.azimuth, target.x*100, azimuth_to_target, eps, delta_omega, delta_phi_0, delta_phi_1, delta_phi); 
00036     
00037     set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);                                    //Set motor
00038     
00039     if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy/2) && (abs(target.azimuth - current.azimuth) < 0.1))  coord_ready = 1; else coord_ready = 0;
00040 }
00041 
00042 void balance_motion(){
00043     
00044     int sign_da; //signum of da
00045     int sign_de; //eps
00046     switch(motion_mode){
00047         case GO:{
00048             sign_de = 0;
00049             if ((abs(theor.path) < max.speed*max.speed/max.accel) || (abs(target.path - theor.path) < max.speed*max.speed/max.accel)){
00050                 //if (abs(theor.speed) < max.speed/2) sign_da = 1; else sign_da = -1;}   //Razgon/Tormoz
00051                 if (abs(theor.path) < max.speed*max.speed/max.accel/6) sign_da = 1; else sign_da = -1; }
00052             else sign_da = 0; //ezda       
00053             if (target.dir == 0) sign_da = -sign_da;
00054             
00055             theor.speed += (theor.accel + sign_da * da) * t_step; //for next step
00056             theor.path += theor.speed * t_step; 
00057             if (((target.path < theor.path) && (target.dir == 1)) || ((target.path > theor.path) && (target.dir == 0))) {coord_ready = 1;sign_da = 0;} 
00058             else coord_ready = 0;
00059             break;}
00060         case STOP:{
00061             //float tma = x_prop * (target.path - current.path) - x_diff * current.speed;                                                                                                      //Motion PID
00062             sign_da = 0;//tma/da;  
00063             sign_de = 0;
00064             theor.speed = 0;
00065             theor.accel = 0;
00066             break;
00067             }
00068         case ROTATE:{
00069             sign_da = 0;
00070             float tm_az = target.azimuth - current.azimuth; if (tm_az < - pi) tm_az += 2*pi; if (tm_az > pi) tm_az -= 2*pi;
00071             if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1;
00072             if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0;
00073                 motor_speed[0] = (motor_speed[0] + motor_speed[1])/2;
00074                 motor_speed[1] = motor_speed[0];
00075             } 
00076             else coord_ready = 0;
00077             break;
00078             }
00079         }
00080     theor.accel += sign_da * da;
00081      //wifi.printf("%d, %f\r\n",sign_da,da);
00082     balance_integral += ax;
00083     float delta_v = (balance_prop*ax + balance_diff * gy + theor.accel) * t_step;         //Balance PID
00084     float delta_omega = max.eps * sign_de * t_step;
00085     set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);                                    //Set motor
00086     wifi.printf("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); 
00087 }
00088 
00089 void balance_motion_PID(){
00090     float tma;
00091     int sign_de; //eps
00092     float tm_lim;//ogran skorosti (uskor ot skorosti)
00093         
00094     x_integral += (target.path - current.path);
00095     if (abs(current.speed) > 0.05) x_integral = 0;    
00096     switch(motion_mode){
00097         case GO:{
00098             sign_de = 0;
00099             float path_error = (target.path - current.path); 
00100             if (abs(path_error) < coord_accuracy) {coord_ready = 1;tma = 0;}
00101             else {
00102                 //if (path_error > 0.04) path_error = 0.04;  if (path_error < -0.04) path_error = -0.04;
00103 
00104                 tma = x_prop * path_error - x_diff * current.speed + x_int * x_integral;
00105                 if (current.speed * tma > 0) tm_lim = max.accel*(max.speed - abs(current.speed))/max.speed; else tm_lim = max.accel; //ogran skor
00106                 tma = atan(tma/tm_lim*pi/2) * tm_lim * 2 / pi;                                                                                                 //Motion PID
00107                 coord_ready = 0;}
00108             break;}
00109         case STOP:{
00110             tma = x_prop * (target.path - current.path) - x_diff * current.speed ;//+ x_int * x_integral; 
00111             if (current.speed * tma > 0) tm_lim = max.accel*(max.speed - abs(current.speed))/max.speed; else tm_lim = max.accel; //ogran skor
00112             tma = atan(tma/tm_lim*pi/2) * tm_lim * 2 / pi;  //ogran uskor                                                                                                   //Motion PID
00113             sign_de = 0;
00114             break;
00115             }
00116         case ROTATE:{
00117             float tm_az = target.azimuth - current.azimuth; if (tm_az < - pi) tm_az += 2*pi; if (tm_az > pi) tm_az -= 2*pi;
00118             if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1;
00119             if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0;
00120                 motor_speed[0] = (motor_speed[0] + motor_speed[1])/2;
00121                 motor_speed[1] = motor_speed[0];
00122             } 
00123             else coord_ready = 0;
00124             break;
00125             }
00126         }
00127      //wifi.printf("%d, %f\r\n",sign_da,da);
00128     float delta_v = (balance_prop * ax + balance_diff * gy + tma) * t_step;         //Balance PID
00129     float delta_omega = max.eps * sign_de * t_step;
00130     set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);                                    //Set motor
00131     //wifi.printf("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); 
00132     wifi.printf("%f %f %f %f;",current.speed,tma,ax,gy); 
00133 }
00134 
00135 
00136 
00137 void realtime(){ 
00138     time_sec += t_step;
00139     fall_timer++; if(abs(ax)<3) fall_timer=0; if(fall_timer>50) fall_flag=1; else fall_flag=0;     //fall check
00140     realtime_flag = 1; 
00141     }
00142 
00143 void realtime_init(){
00144     rt_ticker.attach(&realtime, t_step);    }
00145 
00146 
00147