Diff: main.cpp

Revision:

26:88639564e3af

Parent:

25:2cc29fa67345

Child:

27:c15170a5cd3d

--- a/main.cpp	Tue Oct 29 14:37:55 2019 +0000
+++ b/main.cpp	Wed Oct 30 11:34:26 2019 +0000
@@ -48,6 +48,12 @@
 states currentState=Waiting;
 volatile bool motor1_calibrated=false;
 
+volatile bool motorx_on=false;
+volatile float signx=0.075;
+volatile bool motory_on=false;
+volatile float signy=0.075;
+
+volatile bool demo_done=false;
 volatile float x_ref=0.175;
 volatile float y_ref=0.175;
 static float v_refx; //reference speeds 
@@ -66,9 +72,9 @@
 //PID controller variables
 bool q = true;
 float error_prev;
-static float Kp = 24.6;
-static float Ki = 28.3;
-static float Kd = 20;
+volatile float Kp = 24.6;
+volatile float Ki = 28.3;
+volatile float Kd = 0.1;
 float u;
 float u_p;
 float u_i;
@@ -235,12 +241,8 @@
 
 void set_v_refxy(void)
 {
-   static bool motorx_on=false;
-   static float signx=0.075;
    float Q0;
    Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
-   static bool motory_on=false;
-   static float signy=0.075;
    float Q1;
    Q1=2.0f*(P1-(EMG_min1+EMG_max1)/2.0f)/(EMG_max1-EMG_min1);
    if (Q0>-0.2f && !ignore_peaksx)
@@ -323,8 +325,6 @@
 
 void left_fake_emg(void)
 {
-    static bool motorx_on=false;
-    static float signx=0.075;
     if (!ignore_peaksx)
    {
         if (motorx_on)
@@ -358,8 +358,6 @@
 
 void right_fake_emg(void)
 {
-    static bool motory_on=false;
-    static float signy=0.075;
     if (!ignore_peaksy)
    {
         if (motory_on)
@@ -495,7 +493,9 @@
 float PID1(float err){
     //P action
     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));
     //I action 
     error_integral1 = error_integral1 + err * T;
     u_i = Ki * error_integral1;
@@ -511,7 +511,7 @@
     u_d = Kd * filtered_error_derivativex;
     error_prevx = err;
     
-    u = u_p + u_i+u_d;
+    u = r1/0.3f*(u_p + u_i+u_d);
     if(filtered_error_derivativex>0.3)
     {
         u=0;
@@ -522,7 +522,9 @@
 float PID2(float err){
     //P action
     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));
     //I action 
     error_integral2 = error_integral2 + err * T;
     u_i = Ki * error_integral2;
@@ -540,13 +542,19 @@
     error_prevy = err;
     
     
-    u = u_p + u_i+u_d;
+    u = r2/0.3f*(u_p + u_i+u_d);
     return u;
     }
 void set_refxy(void)
 {
     float t=state_time.read();
     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;
     /* Rectangle
     if(tfloor%12==0||tfloor%12==1||tfloor%12==2)
     {
@@ -575,73 +583,82 @@
     */
     if(tfloor<3)
     {
-        x_ref=0.025;
-        y_ref=0.1;
+        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);
     }
-    else if(tfloor>=3&&tfloor<=8)
+    else if(demo_subpart==1&&y_pos<0.4)
     {
         x_ref=0.025;
-        y_ref=0.1+0.05*(t-3);
+        y_ref=min(0.41f,y_pos+v_demo);
     }
-    else if(tfloor==9)
+    else if(demo_subpart==2&&x_pos<0.075)
     {
-        x_ref=0.025+0.05*(t-9);
-        y_ref=0.4;
+        x_ref=0.085;
+        y_ref=y_pos;
     }
-    else if(tfloor>=10&&tfloor<=15)
+    else if(demo_subpart==3&&y_pos>0.1)
     {
         x_ref=0.075;
-        y_ref=0.4-0.05*(t-10);
+        y_ref=max(0.09f,y_pos-v_demo);
     }
-    else if(tfloor==16)
+    else if(demo_subpart==4&&x_pos<0.125)
     {
-        x_ref=0.075+0.05*(t-16);
-        y_ref=0.1;
+        x_ref=0.135;
+        y_ref=y_pos;
     }
-    else if(tfloor>=17&&tfloor<=22)
+    else if(demo_subpart==5&&y_pos<0.4)
     {
         x_ref=0.125;
-        y_ref=0.1+0.05*(t-17);
+        y_ref=min(0.41f,y_pos+v_demo);
     }
-    else if(tfloor==23)
+    else if(demo_subpart==6&&x_pos<0.175)
     {
-        x_ref=0.125+0.05*(t-23);
-        y_ref=0.4;
+        x_ref=0.185;
+        y_ref=y_pos;
     }
-    else if(tfloor>=24&&tfloor<=29)
+    else if(demo_subpart==7&&y_pos>0.1)
     {
         x_ref=0.175;
-        y_ref=0.4-0.05*(t-24);
+        y_ref=max(0.09f,y_pos-v_demo);
     }
-    else if(tfloor==30)
+    else if(demo_subpart==8&&x_pos<0.225)
     {
-        x_ref=0.175+0.05*(t-25);
-        y_ref=0.1;
+        x_ref=0.235;
+        y_ref=y_pos;
     }
-    else if(tfloor>=31&&tfloor<=36)
+    else if(demo_subpart==9&&y_pos<0.4)
     {
         x_ref=0.225;
-        y_ref=0.1+0.05*(t-31);
+        y_ref=min(0.41f,y_pos+v_demo);
     }
-    else if(tfloor==37)
+    else if(demo_subpart==10&&x_pos<0.275)
     {
-        x_ref=0.225+0.05*(t-37);
-        y_ref=0.4;
+        x_ref=0.285;
+        y_ref=y_pos;
     }
-    else if(tfloor>=38&&tfloor<=43)
+    else if(demo_subpart==11&&y_pos>0.1)
     {
         x_ref=0.275;
-        y_ref=0.4-0.05*(t-38);
+        y_ref=max(0.09f,y_pos-v_demo);
     }
-    else if(tfloor==44)
+    else if(demo_subpart==12&&x_pos<0.325)
     {
-        x_ref=0.275+0.05*(t-44);
-        y_ref=0.1;
+        x_ref=0.335;
+        y_ref=y_pos;
     }
-    else if(tfloor>=45&&tfloor<=50)
+    else if(demo_subpart==13&&y_pos<0.4)
     {
         x_ref=0.325;
-        y_ref=0.1+0.05*(t-45);
+        y_ref=min(0.41f,y_pos+v_demo);
+    }
+    else if(demo_subpart==14)
+    {
+        
+        wait(10);   
+    }
+    else
+    {
+        demo_subpart+=1;
     }
     //float asdfgh=x_ref;
     //x_ref=y_ref-0.075;
@@ -707,7 +724,7 @@
             break;
         case Operating:
             read_emg();
-            //set_v_refxy();
+            set_v_refxy();
             set_ref_fake_emg();
             setPWM_controller();
             ledred=1;
@@ -715,8 +732,16 @@
             ledblue=1;
             break;
         case Demo:
-            set_refxy();
-            setPWM_controller();
+            if(!demo_done)
+            {
+                set_refxy();
+                setPWM_controller();
+            }
+            else
+            {
+                motor1_pwm.write(0.0f);
+                motor2_pwm.write(0.0f);
+            }
             ledred=0;
             ledgreen=0;
             ledblue=0;
@@ -737,6 +762,22 @@
     currentState=Failure;
 }
 
+void return_home()
+{
+    theta1_ref=pi/4.0f;
+    theta2_ref=pi/4.0f;
+    theta1_old=pi/4.0f;
+    theta2_old=pi/4.0f;
+    v_refx=0;
+    EMG_peakx.attach(unignore_peaksx,0.8);
+    ignore_peaksx=true;
+    motorx_on=false;
+    v_refy=0;
+    EMG_peaky.attach(unignore_peaksy,0.8);
+    ignore_peaksy=true;
+    motory_on=false;
+}
+
 void button_press(void) 
 //Press button to change state
 {
@@ -775,14 +816,14 @@
             currentState=Demo;
             Kp=24.6;
             Ki=28.3;
-            Kd=20;
+            Kd=0.1;
             wait(1);
             break;
         case Demo:
             currentState=Operating;
             Kp=15;
             Ki=38.3;
-            Kd=20;
+            Kd=10;
             wait(1);
             break;
     }
@@ -798,9 +839,9 @@
     button.fall(button_press);
     left_button.fall(left_fake_emg);
     left_button.rise(left_fake_emg);
-    right_button.fall(right_fake_emg);
-    right_button.rise(right_fake_emg);
-    int frequency_pwm=10000;
+    right_button.fall(return_home);
+    right_button.rise(return_home);
+    int frequency_pwm=21000;
     motor1_pwm.period(1.0/frequency_pwm);
     motor2_pwm.period(1.0/frequency_pwm);
     state_time.start();