Diff: main.cpp

Revision:

23:b0222fa7c131

Parent:

22:335a30b0825d

Child:

24:3c9ac44890f0

--- a/main.cpp	Mon Oct 21 14:13:40 2019 +0000
+++ b/main.cpp	Fri Oct 25 09:52:56 2019 +0000
@@ -20,8 +20,10 @@
 
 Ticker ticktick;
 Timer state_time;
-Timeout EMG_peak;
-Timeout turn;
+Timeout EMG_peakx;
+Timeout turnx;
+Timeout EMG_peaky;
+Timeout turny;
 Ticker      sample_timer;
 HIDScope    scope( 4);
 DigitalOut  ledred(LED_RED);
@@ -36,12 +38,16 @@
 volatile float EMG_max0=0;
 volatile float EMG_min1=1;
 volatile float EMG_max1=0;
-volatile bool ignore_peaks=false;
-volatile bool ignore_turn=true;
+volatile bool ignore_peaksx=false;
+volatile bool ignore_turnx=true;
+volatile bool ignore_peaksy=false;
+volatile bool ignore_turny=true;
 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
 states currentState=Waiting;
 volatile bool motor1_calibrated=false;
 
+volatile float x_ref=0.175;
+volatile float y_ref=0.175;
 static float v_refx; //reference speeds 
 static float v_refy;
 static float T = 0.002; //measuring period
@@ -56,8 +62,8 @@
 //PID controller variables
 bool q = true;
 float error_prev;
-static float Kp = 8;
-static float Ki = 1.02;
+static float Kp = 24.6;
+static float Ki = 28.3;
 static float Kd = 0.1;
 float u;
 float u_p;
@@ -160,7 +166,7 @@
         motor2_pwm.write(0.75f);
         dir2=1;
     }
-    else if(t>0.5f)
+    else if(t>1.0f)
     {
         motor1_pwm.write(0.0f);
         dir2=1; //???
@@ -204,55 +210,96 @@
     }
 }
 
-void unignore_peaks(void)
+void unignore_peaksx(void)
 {
-    ignore_peaks=false;
+    ignore_peaksx=false;
+}
+void start_ignore_turnx(void)
+{
+    ignore_turnx=true;
 }
-void start_ignore_turn(void)
+void unignore_peaksy(void)
 {
-    ignore_turn=true;
+    ignore_peaksy=false;
+}
+void start_ignore_turny(void)
+{
+    ignore_turny=true;
 }
 
-void set_PWM(void)
+void set_v_refxy(void)
 {
-   static bool motor_on=false;
+   static bool motorx_on=false;
+   static float signx=0.15;
    float Q0;
    Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
-   if (Q0>-0.2f && !ignore_peaks)
+   static bool motory_on=false;
+   static float signy=0.15;
+   float Q1;
+   Q1=2.0f*(P1-(EMG_min1+EMG_max1)/2.0f)/(EMG_max1-EMG_min1);
+   if (Q0>-0.2f && !ignore_peaksx)
    {
-        if (motor_on)
+        if (motorx_on)
         {
-            motor1_pwm.write(0.0f);
-            EMG_peak.attach(unignore_peaks,0.8);
-            turn.attach(start_ignore_turn,1);
-            ignore_turn=false;
-            ignore_peaks=true;
-            motor_on=false;
+            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_turn)
+        else if(ignore_turnx)
         {
-            motor1_pwm.write(1.0f);
-            EMG_peak.attach(unignore_peaks,0.8);
-            turn.attach(start_ignore_turn,1);
-            ignore_turn=false;
-            ignore_peaks=true;
-            motor_on=true;
+            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
         {
-            motor1_pwm.write(1.0f);
-            dir1=!dir1;
-            EMG_peak.attach(unignore_peaks,1.5);
-            ignore_peaks=true;
-            motor_on=true;
+            signx=-1.0*signx;
+            v_refx=1.0*signx;
+            EMG_peakx.attach(unignore_peaksx,1.5);
+            ignore_peaksx=true;
+            motorx_on=true;
         } 
     }
-   motor2_pwm.write(ain1.read());
+   if (Q1>-0.2f && !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,1.5);
+            ignore_peaksy=true;
+            motory_on=true;
+        } 
+    }
 }
 
 void error(void){
     
-    float dvd=L * (tan(theta2) * pow(tan(theta1),2) - tan(theta1) * pow(tan(theta2),2));
+    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; 
@@ -274,6 +321,11 @@
         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;
     
@@ -335,25 +387,50 @@
     
     return u;
     }
-    
+void set_refxy(void)
+{
+    float t=state_time.read();
+    int tfloor=floor(t)/1;
+    if(tfloor%8==0||tfloor%8==1)
+    {
+        x_ref=0.075+0.05*(t-tfloor+tfloor%8);
+        y_ref=0.2-0.05*(t-tfloor+tfloor%8);
+    }
+    else if(tfloor%8==2||tfloor%8==3)
+    {
+        x_ref=0.175+0.05*(t-tfloor+tfloor%8-2);
+        y_ref=0.1+0.05*(t-tfloor+tfloor%8-2);
+    }
+    else if(tfloor%8==4||tfloor%8==5)
+    {
+        x_ref=0.275-0.05*(t-tfloor+tfloor%8-4);
+        y_ref=0.2+0.05*(t-tfloor+tfloor%8-4);
+    }
+    else
+    {
+        x_ref=0.175-0.05*(t-tfloor+tfloor%8-6);
+        y_ref=0.3-0.05*(t-tfloor+tfloor%8-6);
+    }
+}
+ 
 void setPWM_controller(void){
     error();
     u_1 = PID1(error_1);
     u_2 = PID2(error_2);
     
     if(u_1 < 0.0f){
-        dir1 = 1;
+        dir1 = 0;
     }else{
-        dir1 = 0;
+        dir1 = 1;
         }
-    motor1_pwm.write(fabs(u_1));
+    motor1_pwm.write(min(fabs(u_1),1.0f));
     
     if(u_2 < 0.0f){
-        dir2 = 1;
+        dir2 = 0;
     }else{
-        dir2 = 0;
+        dir2 = 1;
         }
-    motor2_pwm.write(fabs(u_1));
+    motor2_pwm.write(min(fabs(u_2),1.0f));
     
     }
 
@@ -393,12 +470,15 @@
             break;
         case Operating:
             read_emg();
-            set_PWM();
+            set_v_refxy();
+            setPWM_controller();
             ledred=1;
             ledgreen=0;
             ledblue=1;
             break;
         case Demo:
+            set_refxy();
+            setPWM_controller();
             ledred=0;
             ledgreen=0;
             ledblue=0;
@@ -443,11 +523,13 @@
                 motor1_calibrated=true;
                 state_time.reset();
                 dir1=!dir1;
-                wait(1);
+                motor1_pwm.write(0.0f);
             }
             else
             {
                 currentState=EMG_calibration;
+                motor2_pwm.write(0.0f);
+                motor1_pwm.write(0.0f);
             }   
             wait(1);
             break;