Diff: realtime.h

Revision:

20:e73f49ba5001

Parent:

19:2fe650d29823

Child:

22:14e85f2068c7

--- a/realtime.h	Sun Feb 24 11:02:56 2019 +0000
+++ b/realtime.h	Sat Mar 23 16:38:44 2019 +0000
@@ -15,8 +15,8 @@
     float tan_error = delta_s * cos(delta_phi_0);
     
     float a = x_prop * tan_error - x_diff * current.speed;                                                                                                      //Motion PID
-    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); 
-    if (a > max_accel) a = max_accel ; if (a < -max_accel) a = -max_accel;
+    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); 
+    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;                                                                                         //Balance PID
        
@@ -29,7 +29,7 @@
     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 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); 
@@ -40,16 +40,92 @@
 }
 
 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
+    int sign_da; //signum of da
+    int sign_de; //eps
+    switch(motion_mode){
+        case GO:{
+            sign_de = 0;
+            if ((abs(theor.path) < max.speed*max.speed/max.accel) || (abs(target.path - theor.path) < max.speed*max.speed/max.accel)){
+                //if (abs(theor.speed) < max.speed/2) sign_da = 1; else sign_da = -1;}   //Razgon/Tormoz
+                if (abs(theor.path) < max.speed*max.speed/max.accel/6) sign_da = 1; else sign_da = -1; }
+            else sign_da = 0; //ezda       
+            if (target.dir == 0) sign_da = -sign_da;
+            
+            theor.speed += (theor.accel + sign_da * da) * t_step; //for next step
+            theor.path += theor.speed * t_step; 
+            if (((target.path < theor.path) && (target.dir == 1)) || ((target.path > theor.path) && (target.dir == 0))) {coord_ready = 1;sign_da = 0;} 
+            else coord_ready = 0;
+            break;}
+        case STOP:{
+            //float tma = x_prop * (target.path - current.path) - x_diff * current.speed;                                                                                                      //Motion PID
+            sign_da = 0;//tma/da;  
+            sign_de = 0;
+            theor.speed = 0;
+            theor.accel = 0;
+            break;
+            }
+        case ROTATE:{
+            sign_da = 0;
+            float tm_az = target.azimuth - current.azimuth; if (tm_az < - pi) tm_az += 2*pi; if (tm_az > pi) tm_az -= 2*pi;
+            if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1;
+            if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0;
+                motor_speed[0] = (motor_speed[0] + motor_speed[1])/2;
+                motor_speed[1] = motor_speed[0];
+            } 
+            else coord_ready = 0;
+            break;
+            }
+        }
+    theor.accel += sign_da * da;
+     //wifi.printf("%d, %f\r\n",sign_da,da);
+    float delta_v = (balance_prop*ax + balance_diff * gy + theor.accel) * t_step;         //Balance PID
+    float delta_omega = max.eps * sign_de * t_step;
+    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("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); 
 }
 
+void balance_motion_PID(){
+    float tma;
+    int sign_de; //eps
+    
+    switch(motion_mode){
+        case GO:{
+            sign_de = 0;
+            float path_error = (target.path - current.path); 
+            if (abs(path_error) < coord_accuracy) {coord_ready = 1;tma = 0;}
+            else {
+                //if (path_error > 0.04) path_error = 0.04;  if (path_error < -0.04) path_error = -0.04;
+                tma = x_prop * path_error - x_diff * current.speed;    
+                tma = atan(tma) ;//* max.accel;                                                                                                 //Motion PID
+                coord_ready = 0;}
+            break;}
+        case STOP:{
+            tma = x_prop * (target.path - current.path) - x_diff * current.speed; 
+            tma = atan(tma);                                                                                                     //Motion PID
+            sign_de = 0;
+            break;
+            }
+        case ROTATE:{
+            float tm_az = target.azimuth - current.azimuth; if (tm_az < - pi) tm_az += 2*pi; if (tm_az > pi) tm_az -= 2*pi;
+            if (tm_az < delta.azimuth/2) sign_de = -1; else sign_de = 1;
+            if (abs(tm_az) < 0.1) {coord_ready = 1;sign_de = 0;
+                motor_speed[0] = (motor_speed[0] + motor_speed[1])/2;
+                motor_speed[1] = motor_speed[0];
+            } 
+            else coord_ready = 0;
+            break;
+            }
+        }
+     //wifi.printf("%d, %f\r\n",sign_da,da);
+    float delta_v = (balance_prop*ax + balance_diff * gy + tma) * t_step;         //Balance PID
+    float delta_omega = max.eps * sign_de * t_step;
+    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("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); 
+}
+
+
+
 void realtime(){ 
     time_sec += t_step;
     fall_timer++; if(abs(ax)<3) fall_timer=0; if(fall_timer>50) fall_flag=1; else fall_flag=0;     //fall check