Diff: realtime.h

Revision:

15:960b922433d1

Parent:

14:e12d0fdc3a48

Child:

17:bd6b6ac89e0e

--- a/realtime.h	Sun Nov 18 13:33:28 2018 +0000
+++ b/realtime.h	Sat Dec 01 14:25:04 2018 +0000
@@ -1,63 +1,60 @@
 Ticker rt_ticker;
 bool realtime_flag;
-/*float balance_prop = 0.8;
-float balance_diff = 1.2;
-float x_prop = 40;
-float x_diff = 25;
-float azimuth_prop = 0.8;
-float azimuth_diff = 0.65;*/
 
-void balance()
-{
-    float a;
-    float eps;
-    float azimuth_to_target;
-    float normal_error;
-    float tan_error;
-    
-    float delta_x = (target.x - current.x);
-    if (delta_x == 0) delta_x = 0.0001;
+void balance_coord(){
+    float delta_x = (target.x - current.x); if (delta_x == 0) delta_x = 0.0001;
     float delta_y = (target.y - current.y);
-    float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y);
-    if (delta_x < 0) {delta_s = -delta_s;}
+    float delta_s = sqrt(delta_x*delta_x + delta_y*delta_y); //always positive        //if (delta_x < 0) {delta_s = -delta_s;}
+    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;}
     
-    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;}
-    float delta_phi_0 = azimuth_to_target - current.azimuth;                                              //target error
-    float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s);       //sever error
-    float delta_phi = coord_accuracy/(delta_s + coord_accuracy) * 0.3 * delta_phi_1 + delta_s/(delta_s + coord_accuracy) * delta_phi_0;  //ZNAK 
+    float delta_phi_0 = azimuth_to_target - current.azimuth;                                                                                                    //azimuth: target error
+    if (delta_phi_0 < -pi) delta_phi_0 += 2*pi; if (delta_phi_0 > pi) delta_phi_0 -= 2*pi;
+    
+    float normal_error  = delta_s * sin(delta_phi_0);
+    float tan_error = delta_s * cos(delta_phi_0);
     
-    normal_error  = delta_s * sin(delta_phi);
-    tan_error = delta_s * cos(delta_phi);
-    
-    
-    if (abs(current.speed) > max_speed) a = 0 ; else  a = x_prop * tan_error - x_diff * current.speed; 
+    float a = x_prop * tan_error - x_diff * current.speed;                                                                                                      //Motion PID
+    if ((current.speed > max_speed) && (a > 0)) a = 0 ; if ((current.speed < -max_speed) && (a < 0)) a = 0 ; 
     if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel;
-    float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step; 
+
+    float delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step;                                                                                         //Balance PID
        
-    //if (abs(normal_error) < coord_accuracy) {eps = 0;current.omega = 0;} else 
-    eps = azimuth_prop * delta_phi - azimuth_diff * current.omega; //gz * azimuth_diff;
+    float delta_phi_1 = (target.azimuth - current.azimuth);//* (current.speed * t_step / delta_s);                                                              //azimuth: sever error
+    if (delta_phi_1 < -pi) delta_phi_1 += 2*pi; if (delta_phi_1 > pi) delta_phi_1 -= 2*pi;
+    if (delta_phi_1 < -pi/2) delta_phi_1 = -pi/2; if (delta_phi_1 > pi/2) delta_phi_1 = pi/2;
+
+    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)
+//    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;   
+    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 
+    if (delta_phi > pi/2) delta_phi -= pi; if (delta_phi < -pi/2) delta_phi += pi;    
+
+    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
     float delta_omega = eps * t_step;
     
-    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); 
-    
-    set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);
+    //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); 
     
-    if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy))  ready = 1; else ready = 0;
+    set_motor_speed(motor_speed[0] - delta_v + delta_omega * half_axis, motor_speed[1] - delta_v - delta_omega * half_axis);                                    //Set motor
     
-   // wifi.printf("%.2f %.2f %.2f %.2f;",a,gy,ax,delta_v*100);
-   
+    if ((abs(normal_error) < coord_accuracy) && (abs(tan_error) < coord_accuracy/5))  coord_ready = 1; else coord_ready = 0;
 }
 
+void balance_motion(){
+    float delta_s, v, a, delta_v;
+    
+    delta_s = target.path - current.path;
+    if(delta_s>0){v=sqrt(2*max_accel*delta_s); if(v>max_speed) v=max_speed;} else {v=-sqrt(-2*max_accel*delta_s); if(v<-max_speed) v=-max_speed;}               //Canonic trajectory with brake
+    a = x_prop * (v - current.speed); if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel;
+    delta_v = (balance_prop*ax + balance_diff * gy + a) * t_step;                                                                                              //Balance PID
+    v = current.speed - delta_v;                                                                                      
+    set_motor_speed(v*(radius+half_axis)/radius, v*(radius-half_axis)/radius);                                                                                 //Set motor
+}
 
-void realtime(){ //35uS - (3.5mS @ 115200) - (500uS @ 921600) (w/o sin,cos)
-/*    float path_error = target_path-current_path;
-    float target_speed = 0.1*path_error;//1m/s at 10cm
-    float linear_acceleration = 1*(target_speed-current_speed);*/    
-
+void realtime(){ 
+    static int fall_timer;
     
-    realtime_flag = 1; 
-}
+    time_sec += t_step;
+    fall_timer++; if(abs(ax)<1) fall_timer=0; if(fall_timer>50) fall_flag=1; else fall_flag=0;     //fall check
+    realtime_flag = 1; }
 
 void realtime_init(){
     rt_ticker.attach(&realtime, t_step);    }