Diff: main.cpp

Revision:

28:5aece9593681

Parent:

27:c15170a5cd3d

Child:

29:78419e287e62

--- a/main.cpp	Thu Oct 31 11:23:49 2019 +0000
+++ b/main.cpp	Mon Nov 04 11:42:06 2019 +0000
@@ -499,6 +499,7 @@
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
     float In1=0.25f*0.49/3.0f+0.2f*(pow(x_pos,2)+pow(y_pos,2));
+    float r1=0.1+sqrt(pow(x_pos,2)+pow(y_pos,2));
     //I action 
     error_integral1 = error_integral1 + err * T;
     u_i = Ki * error_integral1;
@@ -514,7 +515,7 @@
     u_d = Kd * filtered_error_derivativex;
     error_prevx = err;
     
-    u = In1*8.0f*(u_p + u_i+u_d);
+    u = r1/3.0f*(u_p + u_i+u_d);
     if(filtered_error_derivativex>0.3)
     {
         u=0;
@@ -528,6 +529,7 @@
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
     float In2=0.25f*0.49/3.0f+0.2f*(pow(0.35-x_pos,2)+pow(y_pos,2));
+    float r2=0.1+sqrt(pow(0.35f-x_pos,2)+pow(y_pos,2));
     //I action 
     error_integral2 = error_integral2 + err * T;
     u_i = Ki * error_integral2;
@@ -545,7 +547,7 @@
     error_prevy = err;
     
     
-    u = In2*8.0f*(u_p + u_i+u_d);
+    u = r2/3.0f*(u_p + u_i+u_d);
     return u;
     }
 void set_refxy(void)
@@ -663,6 +665,9 @@
     {
         demo_done=true;
         wait(10);   
+        demo_done=false;
+        demo_subpart=1;
+        state_time.reset();
     }
     else
     {