Diff: main.cpp

Revision:

17:18e9df406502

Parent:

16:3c9a3ff09765

--- a/main.cpp	Thu Nov 03 16:50:49 2016 +0000
+++ b/main.cpp	Mon Nov 07 09:38:21 2016 +0000
@@ -265,11 +265,78 @@
     return y;
 }
 
+int t = 0;
+double value_right = 0;
+double value_left = 0;
+
+double calibrate_right(){
+    for(t=0; t<200; t++){
+      if(value_right <= rlf_y){
+          value_right = rlf_y; 
+        }else{
+        value_right = value_right +0.0 ; 
+        }
+        }
+        return value_right;
+}
+
+double calibrate_left(){
+    for(t=0; t<200; t++){
+      if(value_left <= llf_y){
+          value_left = llf_y; 
+        }else{
+        value_left = value_left +0.0 ; 
+        }
+        }
+        return value_left;
+}
+
+
 /* function that calculates the filtered EMG signal from the raw EMG signal. 
 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
 
+//==========PID================
+//setpoints
+const double Setpoint_Translation = -200;
+const double Setpoint_Back = 0;
+const double Setpoint_Rotation = 0;
+double M3_ControlSpeed = 0;
+double M2_ControlSpeed = 0;
+double SetpointError_Translation = 0;
+double SetpointError_Rotation = 0;
+
+//booleans for control
+bool booltranslate = false;
+bool boolrotate = false;
+
+
+    double setpoint;
+    double Arm_ControlSpeed = 0;
+    double Cart_ControlSpeed = 0;
+
+//Cart PID
+const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
+const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
+double Translation_error = 0;
+double Translation_e_prev = 0;
+
+//Arm PID
+const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429;
+double Rotation_error = 0;
+double Rotation_e_prev = 0;
+
+double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
+{
+    double e_der = (error - e_prev) / Ts;
+    e_prev = error;
+    e_int = e_int + (Ts * error);
+
+    return kp*error + ki + e_int + kd + e_der;
+}
+
+
 
 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
 //======== Functions and main ==============================================================
@@ -282,12 +349,14 @@
     lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
     lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
     lrect_y = fabs(lhf_y);
-    llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
+    llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/value_left;
+    
     rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
     rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);    
     rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
     rrect_y = fabs(rhf_y);
-    rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
+    rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/value_right;
+    
     scope.set(1, llf_y);
     scope.set(0, rlf_y);
     scope.send();
@@ -375,7 +444,23 @@
                     }
              }
              
-             
+            SetpointError_Translation =  setpoint - position_cart;
+
+    //set direction
+    if (SetpointError_Translation < 0) {
+        cart_speed = 0;
+    } else {}
+    
+    
+    Cart_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
+    if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
+        cart_speed = 0;
+        
+    }
+      else{
+    cart_speed = Cart_ControlSpeed;
+    } 
+            
               
             // controle LED    
             led_g = not LedOn;
@@ -400,7 +485,7 @@
 
             
                 if (rlf_y > threshold_value_claw) {
-                    if(position_cart > -170 && position_arm >= 95){             //If the cart is not at the end, the arm can't move any further than 45 degrees
+                    if(position_cart > -170 && position_arm >= 90){             //If the cart is not at the end, the arm can't move any further than 45 degrees
                     Arm.stop(1)==1;
                     
                     }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){
@@ -409,9 +494,6 @@
                     }else if(position_cart<= -170 && position_arm>=160){         //If the cart is at the right end, the arm can't move any further than 70 degrees
                     Arm.stop(1)==1;
                     
-                    }else if(position_cart <= -170 && position_arm >= 95){
-                    Arm.speed(0.075)==0.075;
-    
                     }else{
                     Arm.speed(arm_speed)==arm_speed;
                     } 
@@ -421,7 +503,7 @@
                         }
          
                 }else if (llf_y > threshold_value_claw) {
-                    if(position_cart < 170 && position_arm <= -95){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
+                    if(position_cart < 170 && position_arm <= -90){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
                     Arm.stop(1)==1;
                     
                     }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){
@@ -453,6 +535,25 @@
                     }    
                     }
             } 
+            
+            {
+
+    SetpointError_Rotation =  setpoint - position_arm;
+
+    //set direction
+    if (SetpointError_Rotation > 0) {
+        arm_speed = 0;
+    } else {}
+    
+    Arm_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
+    if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) {
+        Arm_ControlSpeed = 0;
+    }else{
+        arm_speed = Arm_ControlSpeed;
+    }
+}
+            
+            
             // controle LED   
             led_r = not LedOn;
             led_b = not LedOn;