Diff: main.cpp

Revision:

27:c15170a5cd3d

Parent:

26:88639564e3af

Child:

28:5aece9593681

--- a/main.cpp	Wed Oct 30 11:34:26 2019 +0000
+++ b/main.cpp	Thu Oct 31 11:23:49 2019 +0000
@@ -1,3 +1,4 @@
+
 #include "mbed.h"
 #include "HIDScope.h"
 #include "QEI.h"
@@ -159,7 +160,7 @@
     if(!motor1_calibrated&&t>1.0f)
     {
         dir1=1; //???
-        motor1_pwm.write(0.75f);
+        motor1_pwm.write(0.7f);
         pulses=motor1_pos.getPulses();
         if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
         {
@@ -175,14 +176,14 @@
     }
     else if(!motor1_calibrated)
     {
-        motor2_pwm.write(0.75f);
+        motor2_pwm.write(0.7f);
         dir2=1;
     }
     else if(t>1.0f)
     {
         motor1_pwm.write(0.0f);
         dir2=1; //???
-        motor2_pwm.write(0.75f);
+        motor2_pwm.write(0.7f);
         pulses=motor2_pos.getPulses();
         if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
         {
@@ -435,7 +436,7 @@
     float theta1_dot = a*v_refx + b*v_refy;
     float theta2_dot = c*v_refx + d*v_refy;
     
-    if(currentState==Operating)
+    if(currentState==Operating||currentState==Demo)
     {
         theta1_ref = theta1_old+theta1_dot*T;
         theta2_ref = theta2_old+theta2_dot*T;
@@ -477,11 +478,13 @@
         theta1_ref=pi/4.0f;
         theta2_ref=pi/4.0f;
     }
+    /*
     else if(currentState==Demo)
     {
         theta1_ref=atan(y_ref/x_ref);
         theta2_ref=atan(y_ref/(0.35f-x_ref));
     }
+    */
     error_1 = theta1 - theta1_ref;
     error_2 = theta2 - theta2_ref;
     
@@ -495,7 +498,7 @@
     u_p = Kp * err;
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
-    float r1=0.1+sqrt(pow(x_pos,2)+pow(y_pos,2));
+    float In1=0.25f*0.49/3.0f+0.2f*(pow(x_pos,2)+pow(y_pos,2));
     //I action 
     error_integral1 = error_integral1 + err * T;
     u_i = Ki * error_integral1;
@@ -511,7 +514,7 @@
     u_d = Kd * filtered_error_derivativex;
     error_prevx = err;
     
-    u = r1/0.3f*(u_p + u_i+u_d);
+    u = In1*8.0f*(u_p + u_i+u_d);
     if(filtered_error_derivativex>0.3)
     {
         u=0;
@@ -524,7 +527,7 @@
     u_p = Kp * err;
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
-    float r2=0.1+sqrt(pow(0.35f-x_pos,2)+pow(y_pos,2));
+    float In2=0.25f*0.49/3.0f+0.2f*(pow(0.35-x_pos,2)+pow(y_pos,2));
     //I action 
     error_integral2 = error_integral2 + err * T;
     u_i = Ki * error_integral2;
@@ -542,7 +545,7 @@
     error_prevy = err;
     
     
-    u = r2/0.3f*(u_p + u_i+u_d);
+    u = In2*8.0f*(u_p + u_i+u_d);
     return u;
     }
 void set_refxy(void)
@@ -551,10 +554,8 @@
     int tfloor=floor(t)/1;
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
-    static float x_start=x_pos;
-    static float y_start=y_pos;
     static int demo_subpart=1;
-    float v_demo=0.015;
+    float v_demo=0.08;
     /* Rectangle
     if(tfloor%12==0||tfloor%12==1||tfloor%12==2)
     {
@@ -581,79 +582,86 @@
     x_ref=0.175f+0.15*cos(t/2.5f);
     y_ref=0.275f+0.15*sin(t/2.5f);
     */
-    if(tfloor<3)
+    if(tfloor<5)
     {
-        x_ref=0.025+(3.0f-t)/3.0f*(x_start-0.025);
-        y_ref=0.1+(3.0f-t)/3.0f*(y_start-0.1);
+        theta1_old=pi/4;
+        theta2_old=pi/4;
+    }
+    else if(tfloor<12)
+    {
+        x_ref=0.025+(x_pos-0.025)*(12-t)/7.0f;
+        y_ref=0.1+(y_pos-0.1)*(12-t)/7.0f;
+        theta1_old=atan(y_ref/x_ref);
+        theta2_old=atan(y_ref/(0.35f-x_ref));
     }
     else if(demo_subpart==1&&y_pos<0.4)
     {
-        x_ref=0.025;
-        y_ref=min(0.41f,y_pos+v_demo);
+        v_refx=0;
+        v_refy=v_demo;
     }
     else if(demo_subpart==2&&x_pos<0.075)
     {
-        x_ref=0.085;
-        y_ref=y_pos;
+        v_refx=v_demo;
+        v_refy=0;
     }
-    else if(demo_subpart==3&&y_pos>0.1)
+    else if(demo_subpart==3&&y_pos>0.05)
     {
-        x_ref=0.075;
-        y_ref=max(0.09f,y_pos-v_demo);
+        v_refx=0;
+        v_refy=-v_demo;
     }
     else if(demo_subpart==4&&x_pos<0.125)
     {
-        x_ref=0.135;
-        y_ref=y_pos;
+        v_refx=v_demo;
+        v_refy=0;
     }
-    else if(demo_subpart==5&&y_pos<0.4)
+    else if(demo_subpart==5&&y_pos<0.05)
     {
-        x_ref=0.125;
-        y_ref=min(0.41f,y_pos+v_demo);
+        v_refx=0;
+        v_refy=v_demo;
     }
     else if(demo_subpart==6&&x_pos<0.175)
     {
-        x_ref=0.185;
-        y_ref=y_pos;
+        v_refx=v_demo;
+        v_refy=0;
     }
-    else if(demo_subpart==7&&y_pos>0.1)
+    else if(demo_subpart==7&&y_pos>0.05)
     {
-        x_ref=0.175;
-        y_ref=max(0.09f,y_pos-v_demo);
+        v_refx=0;
+        v_refy=-v_demo;
     }
     else if(demo_subpart==8&&x_pos<0.225)
     {
-        x_ref=0.235;
-        y_ref=y_pos;
+        v_refx=v_demo;
+        v_refy=0;
     }
     else if(demo_subpart==9&&y_pos<0.4)
     {
-        x_ref=0.225;
-        y_ref=min(0.41f,y_pos+v_demo);
+        v_refx=0;
+        v_refy=v_demo;
     }
     else if(demo_subpart==10&&x_pos<0.275)
     {
-        x_ref=0.285;
-        y_ref=y_pos;
+        v_refx=v_demo;
+        v_refy=0;
     }
-    else if(demo_subpart==11&&y_pos>0.1)
+    else if(demo_subpart==11&&y_pos>0.05)
     {
-        x_ref=0.275;
-        y_ref=max(0.09f,y_pos-v_demo);
+        v_refx=0;
+        v_refy=-v_demo;
     }
     else if(demo_subpart==12&&x_pos<0.325)
     {
-        x_ref=0.335;
-        y_ref=y_pos;
+        v_refx=v_demo;
+        v_refy=0;
     }
     else if(demo_subpart==13&&y_pos<0.4)
     {
-        x_ref=0.325;
-        y_ref=min(0.41f,y_pos+v_demo);
+        v_refx=0;
+        v_refy=v_demo;
     }
     else if(demo_subpart==14)
     {
-        
+        demo_done=true;
         wait(10);   
     }
     else
@@ -814,16 +822,11 @@
             break;
         case Operating:
             currentState=Demo;
-            Kp=24.6;
-            Ki=28.3;
-            Kd=0.1;
             wait(1);
             break;
+            
         case Demo:
             currentState=Operating;
-            Kp=15;
-            Ki=38.3;
-            Kd=10;
             wait(1);
             break;
     }