Diff: main.cpp

Revision:

26:cd1db85359aa

Parent:

25:ae908de29943

Child:

27:3392f03bfada

--- a/main.cpp	Sat Oct 03 18:04:16 2015 +0000
+++ b/main.cpp	Mon Oct 05 16:58:58 2015 +0000
@@ -32,6 +32,8 @@
     Ticker ScopeTime;
     Ticker myControllerTicker2;
     Ticker myControllerTicker1;
+    Ticker myEMG1;
+    Ticker myEMG2;
 
 // Constants
     double reference2, reference1;
@@ -40,18 +42,19 @@
     int count = 0;
     double Grens2 = 90, Grens1 = 90;
     double Stapgrootte = 5;
+    double Threshold;
 
 //Sample time (motor-step)
     const double m2_Ts = 0.01, m1_Ts = 0.01;
 
 //Controller gain Motor 2 & 1
-    const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
-    const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
+    const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
+    const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
     double m2_err_int = 0, m2_prev_err = 0;
     double m1_err_int = 0, m1_prev_err = 0;
 
 //Derivative filter coeffs Motor 2 & 1
-    const double BiGain2 = 0.016955, BiGain1 = 0.016955;
+    const double BiGain2 = 0.012, BiGain1 = 0.016955;
     const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
     const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
 
@@ -97,6 +100,13 @@
     // PID
         return Kp * e + Ki*e_int + Kd*e_der;
     }
+    
+//--------------------------------------------------------------------------------------------------------------------------//
+//EMG functions
+//--------------------------------------------------------------------------------------------------------------------------//
+
+// Hier komen functies die de EMG signalen uitlezen en filteren
+
 //--------------------------------------------------------------------------------------------------------------------------//
 // Motor control functions
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -164,6 +174,7 @@
         ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
         myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
         myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
+        // 2 Tickers die de waarden van de EMG-signalen uitlezen
     
 //--------------------------------------------------------------------------------------------------------------------------//
 // Control Program
@@ -172,7 +183,7 @@
     {
         char c = pc.getc();
     // 1 Program UP
-        if(c == 'e') 
+        if(c == 'e') // If ((EMG1 => Threshold) && (EMG2 => Threshold))
         {
             count = count + 1;
             if(count > 2)
@@ -182,7 +193,7 @@
 
         }
      // 1 Program DOWN
-        if(c == 'd')
+        if(c == 'd') // Hoe gaat dit aangestuurd worden?
         {
             count = count - 1;
             if(count < 0)
@@ -196,7 +207,7 @@
                 
                 LedR = LedB = 1;
                 LedG = 0;
-                if(c == 'r')
+                if(c == 'r') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
                 {
                     m2_ref = m2_ref + Stapgrootte;
                     m1_ref = m1_ref - Stapgrootte;
@@ -206,7 +217,7 @@
                         m1_ref = -1*Grens1;
                     }
                 }
-                if(c == 'f')
+                if(c == 'f') // if ((EMG2 => Threshold) && (EMG1 =< Threshold))
                 {
                     m2_ref = m2_ref - Stapgrootte;
                     m1_ref = m1_ref + Stapgrootte;
@@ -222,7 +233,7 @@
         {
                 LedG = LedB = 1;
                 LedR = 0;
-                if(c == 't')
+                if(c == 't') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
                 {
                     m1_ref = m1_ref + Stapgrootte;
                     if (m1_ref > Grens1)
@@ -230,7 +241,7 @@
                         m1_ref = Grens1;
                     }
                 }
-                if(c == 'g')
+                if(c == 'g') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
                 {
                     m1_ref = m1_ref - Stapgrootte;
                     if (m1_ref < -1*Grens1)
@@ -248,7 +259,8 @@
                 //VUUUUR!! (To Do)
                 wait(1);
                 m2_ref = 0;
-                m1_ref = 0;   
+                m1_ref = 0;
+                count = 0;   
         }
     }