Diff: main.cpp

Revision:

25:2cc29fa67345

Parent:

24:3c9ac44890f0

Child:

26:88639564e3af

--- a/main.cpp	Fri Oct 25 12:25:48 2019 +0000
+++ b/main.cpp	Tue Oct 29 14:37:55 2019 +0000
@@ -25,12 +25,14 @@
 Timeout EMG_peaky;
 Timeout turny;
 Ticker      sample_timer;
-HIDScope    scope( 4);
+HIDScope    scope( 6);
 DigitalOut  ledred(LED_RED);
 DigitalOut  ledblue(LED_BLUE);
 DigitalOut  ledgreen(LED_GREEN);
 InterruptIn err(SW2);
 InterruptIn button(SW3);
+InterruptIn left_button(D3);
+InterruptIn right_button(D2);
 
 volatile float P0;
 volatile float P1;
@@ -58,13 +60,15 @@
 
 float error_1;
 float error_2;
+volatile float filtered_error_derivativex;
+volatile float filtered_error_derivativey;
 
 //PID controller variables
 bool q = true;
 float error_prev;
 static float Kp = 24.6;
 static float Ki = 28.3;
-static float Kd = 0.1;
+static float Kd = 20;
 float u;
 float u_p;
 float u_i;
@@ -79,6 +83,8 @@
 const double pi=3.1415926535897932384626;
 volatile float theta1;
 volatile float theta2;
+volatile float theta1_ref;
+volatile float theta2_ref;
 
 void read_emg()
 {
@@ -230,11 +236,11 @@
 void set_v_refxy(void)
 {
    static bool motorx_on=false;
-   static float signx=0.15;
+   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.15;
+   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)
@@ -261,7 +267,7 @@
         {
             signx=-1.0*signx;
             v_refx=1.0*signx;
-            EMG_peakx.attach(unignore_peaksx,1.5);
+            EMG_peakx.attach(unignore_peaksx,0.8);
             ignore_peaksx=true;
             motorx_on=true;
         } 
@@ -290,7 +296,7 @@
         {
             signy=-1.0*signy;
             v_refy=1.0*signy;
-            EMG_peaky.attach(unignore_peaksy,1.5);
+            EMG_peaky.attach(unignore_peaksy,0.8);
             ignore_peaksy=true;
             motory_on=true;
         } 
@@ -315,39 +321,157 @@
     }
 }
 
+void left_fake_emg(void)
+{
+    static bool motorx_on=false;
+    static float signx=0.075;
+    if (!ignore_peaksx)
+   {
+        if (motorx_on)
+        {
+            v_refx=0;
+            EMG_peakx.attach(unignore_peaksx,0.8);
+            turnx.attach(start_ignore_turnx,1);
+            ignore_turnx=false;
+            ignore_peaksx=true;
+            motorx_on=false;
+        }
+        else if(ignore_turnx)
+        {
+            v_refx=1.0*signx;
+            EMG_peakx.attach(unignore_peaksx,0.8);
+            turnx.attach(start_ignore_turnx,1);
+            ignore_turnx=false;
+            ignore_peaksx=true;
+            motorx_on=true;
+        }
+        else
+        {
+            signx=-1.0*signx;
+            v_refx=1.0*signx;
+            EMG_peakx.attach(unignore_peaksx,0.8);
+            ignore_peaksx=true;
+            motorx_on=true;
+        } 
+    }
+}
+
+void right_fake_emg(void)
+{
+    static bool motory_on=false;
+    static float signy=0.075;
+    if (!ignore_peaksy)
+   {
+        if (motory_on)
+        {
+            v_refy=0;
+            EMG_peaky.attach(unignore_peaksy,0.8);
+            turny.attach(start_ignore_turny,1);
+            ignore_turny=false;
+            ignore_peaksy=true;
+            motory_on=false;
+        }
+        else if(ignore_turny)
+        {
+            v_refy=1.0*signy;
+            EMG_peaky.attach(unignore_peaksy,0.8);
+            turny.attach(start_ignore_turny,1);
+            ignore_turny=false;
+            ignore_peaksy=true;
+            motory_on=true;
+        }
+        else
+        {
+            signy=-1.0*signy;
+            v_refy=1.0*signy;
+            EMG_peaky.attach(unignore_peaksy,0.8);
+            ignore_peaksy=true;
+            motory_on=true;
+        } 
+    }
+}
+
+void set_ref_fake_emg(void)
+{
+    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
+    float y_pos=tan(theta1)*x_pos;
+    if(x_pos>=0.35&&v_refx>0)
+    {
+        v_refx=0;
+    }
+    else if(x_pos<=0.0&&v_refx<0)
+    {
+        v_refx=0;
+    }
+    if(y_pos>=0.45&&v_refy>0)
+    {
+        v_refy=0;
+    }
+    else if(y_pos<=0.07&&v_refy<0)
+    {
+        v_refy=0;
+    }
+}
+
 void error(void){
-    
+    float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
+    float y_pos=tan(theta1)*x_pos;
+    float r1=sqrt(pow(x_pos,2)+pow(y_pos,2));
+    float r2=sqrt(pow(0.35-x_pos,2)+pow(y_pos,2));
+    float J11=-r1*cos(theta2)/sin(theta1+theta2);
+    float J21=r1*sin(theta2)/sin(theta1+theta2);
+    float J12=r2*cos(theta1)/sin(theta1+theta2);
+    float J22=r2*sin(theta1)/sin(theta1+theta2);
+    float a=J22/(J11*J22-J12*J21);
+    float b=-J12/(J11*J22-J12*J21);
+    float c=-J21/(J11*J22-J12*J21);
+    float d=J11/(J11*J22-J12*J21);
+    /*
     float dvd=L * (tan(theta2) * pow(tan(theta1),2) + tan(theta1) * pow(tan(theta2),2));//+ is aangepast
 
     float a =  (  -pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta1),2)  )  / dvd;             // inverse matrix components of differential x and differential y
     float b =  (   pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * tan(theta1)         )  / dvd; 
     float c =  (   pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2)  )  / dvd;
     float d =  (   pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2)  )  / dvd;
-    
+    */
     float theta1_dot = a*v_refx + b*v_refy;
     float theta2_dot = c*v_refx + d*v_refy;
     
-    float theta1_ref;
-    float theta2_ref;
     if(currentState==Operating)
     {
         theta1_ref = theta1_old+theta1_dot*T;
         theta2_ref = theta2_old+theta2_dot*T;
+        if(theta1_ref<theta1-0.2)
+        {
+            theta1_ref=theta1-0.2;
+        }
+        else if(theta1_ref>theta1+0.2)
+        {
+            theta1_ref=theta1+0.2;
+        }
+        if(theta2_ref<theta2-0.2)
+        {
+            theta2_ref=theta2-0.2;
+        }
+        else if(theta2_ref>theta2+0.2)
+        {
+            theta2_ref=theta2+0.2;
+        }
         if(theta1_ref<0.1f)
         {
             theta1_ref=0.1f;
         }
-        else if(theta1_ref<1.6f)
+        else if(theta1_ref>1.8f)
         {
-            theta1_ref=1.6f;
+            theta1_ref=1.8f;
         }
         if(theta2_ref<0.1f)
         {
             theta2_ref=0.1f;
         }
-        else if(theta2_ref<1.6f)
+        else if(theta2_ref>1.8f)
         {
-            theta2_ref=1.6f;
+            theta2_ref=1.8f;
         }
     }
     else if(currentState==Homing)
@@ -376,22 +500,22 @@
     error_integral1 = error_integral1 + err * T;
     u_i = Ki * error_integral1;
     
-/*    
     //D action
-    if(q){
-        error_prev = err;
-        q=false;
-        }
+    static float error_prevx=0;
+    static float error_derivativex;
+    static BiQuad LowPassFilterx(0.1311f,0.2622f,0.1311f,-0.7478,0.2722f);
+    
         
-    float error_derivative = (err - error_prev)/T;
-    float filtered_error_derivative = LowPassFilter.step(error_derivative);
-    u_d = Kd * filtered_error_derivative;
-    error_prev = err;
+    error_derivativex = (err - error_prevx)/T;
+    filtered_error_derivativex = LowPassFilterx.step(error_derivativex);
+    u_d = Kd * filtered_error_derivativex;
+    error_prevx = err;
     
-*/
-    
-    u = u_p + u_i;
-    
+    u = u_p + u_i+u_d;
+    if(filtered_error_derivativex>0.3)
+    {
+        u=0;
+    }
     return u;
     }
     
@@ -403,48 +527,125 @@
     error_integral2 = error_integral2 + err * T;
     u_i = Ki * error_integral2;
     
-/*    
+
     //D action
-    if(q){
-        error_prev = err;
-        q=false;
-        }
+    static float error_prevy=0;
+    static float error_derivativey;
+    static BiQuad LowPassFiltery(0.1311f,0.2622f,0.1311f,-0.7478,0.2722f);
+    
         
-    float error_derivative = (err - error_prev)/T;
-    float filtered_error_derivative = LowPassFilter.step(error_derivative);
-    u_d = Kd * filtered_error_derivative;
-    error_prev = err;
+    error_derivativey = (err - error_prevy)/T;
+    filtered_error_derivativey = LowPassFiltery.step(error_derivativey);
+    u_d = Kd * filtered_error_derivativey;
+    error_prevy = err;
     
-*/
     
-    u = u_p + u_i;
-    
+    u = u_p + u_i+u_d;
     return u;
     }
 void set_refxy(void)
 {
     float t=state_time.read();
     int tfloor=floor(t)/1;
-    if(tfloor%8==0||tfloor%8==1)
+    /* Rectangle
+    if(tfloor%12==0||tfloor%12==1||tfloor%12==2)
     {
-        x_ref=0.075+0.05*(t-tfloor+tfloor%8);
-        y_ref=0.2-0.05*(t-tfloor+tfloor%8);
+        x_ref=0.025+0.1*(t-tfloor+tfloor%12);
+        y_ref=0.2;
     }
-    else if(tfloor%8==2||tfloor%8==3)
+    else if(tfloor%12==3||tfloor%12==4||tfloor%12==5)
     {
-        x_ref=0.175+0.05*(t-tfloor+tfloor%8-2);
-        y_ref=0.1+0.05*(t-tfloor+tfloor%8-2);
+        x_ref=0.325;
+        y_ref=0.2+0.05*(t-tfloor+tfloor%12-3);
     }
-    else if(tfloor%8==4||tfloor%8==5)
+    else if(tfloor%12==6||tfloor%12==7||tfloor%12==8)
     {
-        x_ref=0.275-0.05*(t-tfloor+tfloor%8-4);
-        y_ref=0.2+0.05*(t-tfloor+tfloor%8-4);
+        x_ref=0.325-0.1*(t-tfloor+tfloor%12-6);
+        y_ref=0.35;
     }
     else
     {
-        x_ref=0.175-0.05*(t-tfloor+tfloor%8-6);
-        y_ref=0.3-0.05*(t-tfloor+tfloor%8-6);
+        x_ref=0.025;
+        y_ref=0.35-0.05*(t-tfloor+tfloor%12-9);
+    }
+    */
+    /* Circle
+    x_ref=0.175f+0.15*cos(t/2.5f);
+    y_ref=0.275f+0.15*sin(t/2.5f);
+    */
+    if(tfloor<3)
+    {
+        x_ref=0.025;
+        y_ref=0.1;
+    }
+    else if(tfloor>=3&&tfloor<=8)
+    {
+        x_ref=0.025;
+        y_ref=0.1+0.05*(t-3);
+    }
+    else if(tfloor==9)
+    {
+        x_ref=0.025+0.05*(t-9);
+        y_ref=0.4;
+    }
+    else if(tfloor>=10&&tfloor<=15)
+    {
+        x_ref=0.075;
+        y_ref=0.4-0.05*(t-10);
+    }
+    else if(tfloor==16)
+    {
+        x_ref=0.075+0.05*(t-16);
+        y_ref=0.1;
+    }
+    else if(tfloor>=17&&tfloor<=22)
+    {
+        x_ref=0.125;
+        y_ref=0.1+0.05*(t-17);
     }
+    else if(tfloor==23)
+    {
+        x_ref=0.125+0.05*(t-23);
+        y_ref=0.4;
+    }
+    else if(tfloor>=24&&tfloor<=29)
+    {
+        x_ref=0.175;
+        y_ref=0.4-0.05*(t-24);
+    }
+    else if(tfloor==30)
+    {
+        x_ref=0.175+0.05*(t-25);
+        y_ref=0.1;
+    }
+    else if(tfloor>=31&&tfloor<=36)
+    {
+        x_ref=0.225;
+        y_ref=0.1+0.05*(t-31);
+    }
+    else if(tfloor==37)
+    {
+        x_ref=0.225+0.05*(t-37);
+        y_ref=0.4;
+    }
+    else if(tfloor>=38&&tfloor<=43)
+    {
+        x_ref=0.275;
+        y_ref=0.4-0.05*(t-38);
+    }
+    else if(tfloor==44)
+    {
+        x_ref=0.275+0.05*(t-44);
+        y_ref=0.1;
+    }
+    else if(tfloor>=45&&tfloor<=50)
+    {
+        x_ref=0.325;
+        y_ref=0.1+0.05*(t-45);
+    }
+    //float asdfgh=x_ref;
+    //x_ref=y_ref-0.075;
+    //y_ref=asdfgh+0.125;
 }
  
 void setPWM_controller(void){
@@ -457,24 +658,26 @@
     }else{
         dir1 = 1;
         }
-    motor1_pwm.write(min(fabs(u_1),1.0f));
+    motor1_pwm.write(min(0.5f+0.5f*fabs(u_1),1.0f));
     
     if(u_2 < 0.0f){
         dir2 = 0;
     }else{
         dir2 = 1;
         }
-    motor2_pwm.write(min(fabs(u_2),1.0f));
+    motor2_pwm.write(min(0.5f+0.5f*fabs(u_2),1.0f));
     
     }
 
 void sample()
 {
     get_angles();
-    scope.set(0,P0);
-    scope.set(1,P1);
+    scope.set(0,theta1);
+    scope.set(1,theta2);
     scope.set(2,v_refx);
     scope.set(3,v_refy);
+    scope.set(4,theta1_ref);
+    scope.set(5,theta2_ref);
     scope.send(); 
     switch(currentState)
     {
@@ -504,7 +707,8 @@
             break;
         case Operating:
             read_emg();
-            set_v_refxy();
+            //set_v_refxy();
+            set_ref_fake_emg();
             setPWM_controller();
             ledred=1;
             ledgreen=0;
@@ -569,10 +773,16 @@
             break;
         case Operating:
             currentState=Demo;
+            Kp=24.6;
+            Ki=28.3;
+            Kd=20;
             wait(1);
             break;
         case Demo:
             currentState=Operating;
+            Kp=15;
+            Ki=38.3;
+            Kd=20;
             wait(1);
             break;
     }
@@ -586,6 +796,10 @@
     sample_timer.attach(&sample, 0.002);
     err.fall(error_occur);
     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;
     motor1_pwm.period(1.0/frequency_pwm);
     motor2_pwm.period(1.0/frequency_pwm);