Diff: realtime.h

Branch:

svoe

Revision:

22:14e85f2068c7

Parent:

20:e73f49ba5001

--- a/realtime.h	Sat Mar 23 16:39:46 2019 +0000
+++ b/realtime.h	Tue Jul 23 12:55:23 2019 +0000
@@ -79,6 +79,7 @@
         }
     theor.accel += sign_da * da;
      //wifi.printf("%d, %f\r\n",sign_da,da);
+    balance_integral += ax;
     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
@@ -88,7 +89,10 @@
 void balance_motion_PID(){
     float tma;
     int sign_de; //eps
-    
+    float tm_lim;//ogran skorosti (uskor ot skorosti)
+        
+    x_integral += (target.path - current.path);
+    if (abs(current.speed) > 0.05) x_integral = 0;    
     switch(motion_mode){
         case GO:{
             sign_de = 0;
@@ -96,13 +100,16 @@
             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
+
+                tma = x_prop * path_error - x_diff * current.speed + x_int * x_integral;
+                if (current.speed * tma > 0) tm_lim = max.accel*(max.speed - abs(current.speed))/max.speed; else tm_lim = max.accel; //ogran skor
+                tma = atan(tma/tm_lim*pi/2) * tm_lim * 2 / pi;                                                                                                 //Motion PID
                 coord_ready = 0;}
             break;}
         case STOP:{
-            tma = x_prop * (target.path - current.path) - x_diff * current.speed; 
-            tma = atan(tma);                                                                                                     //Motion PID
+            tma = x_prop * (target.path - current.path) - x_diff * current.speed ;//+ x_int * x_integral; 
+            if (current.speed * tma > 0) tm_lim = max.accel*(max.speed - abs(current.speed))/max.speed; else tm_lim = max.accel; //ogran skor
+            tma = atan(tma/tm_lim*pi/2) * tm_lim * 2 / pi;  //ogran uskor                                                                                                   //Motion PID
             sign_de = 0;
             break;
             }
@@ -118,10 +125,11 @@
             }
         }
      //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_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]); 
+    //wifi.printf("%f, %f, %f, %f %f, %f\r\n",current.path,theor.path,theor.speed,theor.accel,target.path,motor_speed[0]); 
+    wifi.printf("%f %f %f %f;",current.speed,tma,ax,gy); 
 }