Diff: main.cpp

Revision:

1:3f49c8818619

Parent:

0:5816557b2064

Child:

2:7873c44b0568

--- a/main.cpp	Mon Oct 19 10:06:36 2015 +0000
+++ b/main.cpp	Thu Oct 22 10:46:10 2015 +0000
@@ -15,7 +15,7 @@
     MODSERIAL pc(USBTX, USBRX);             // To/From PC
     QEI Encoder2(D3, D2, NC, 32);           // Encoder Motor 2
     QEI Encoder1(D13,D12,NC, 32);           // Encoder Motor 1
-    HIDScope scope(5);                      // Scope, 4 channels
+    HIDScope scope(6);                      // Scope, 4 channels
 
 // LEDs
     DigitalOut LedR(LED_RED);
@@ -50,10 +50,6 @@
     int count = 0;
     double Grens2 = 90, Grens1 = 90;
     double Stapgrootte = 5;
-    
-    DigitalOut led(LED_RED);
-    DigitalOut ledG(LED_GREEN);
-    DigitalOut ledB(LED_BLUE);
    
 // Declaring variables
     double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
@@ -85,18 +81,13 @@
     const double m2_Ts = 0.01, m1_Ts = 0.01;
 
 //Controller gain Motor 2 & 1
-    const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
-    const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
+    const double m2_Kp = 2.1/57,m2_Ki = 3.9/57, m2_Kd = 0.1/57;
+    const double m1_Kp = 2.1/57,m1_Ki = 3.9/57, m1_Kd = 0.1/57;
     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.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;
     
 // coëfficiënten
-const double BiGainEMG_H1 = 0.796821; 
+    const double BiGainEMG_H1 = 0.796821; 
     const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
     
     const double BiGainEMG_L1= 0.001041;
@@ -104,10 +95,6 @@
    
     const double BiGainEMG_N1 = 1.0;
     const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
-   
-// Filter variables
-    double m2_f_v1 = 0, m2_f_v2 = 0;
-    double m1_f_v1 = 0, m1_f_v2 = 0;
     
 // Creating the filters
     biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
@@ -126,12 +113,11 @@
 //HIDScope
     void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
     {
-        scope.set(0, reference2 - position2);
-        scope.set(1, position2);
-        scope.set(2, reference1 - position1);    
-        scope.set(3, position1);
-        scope.set(4, EMG_left_MAF);
-        scope.set(5, EMG_right_MAF);
+        scope.set(0, position2);  
+        scope.set(1, position1);
+        scope.set(2, EMG_left_MAF);
+        scope.set(3, EMG_right_MAF);
+        scope.set(4, count);
         scope.send();
     
     }
@@ -198,7 +184,7 @@
     {
         // Setpoint motor 2
             reference2 = m2_ref;                           // Reference in degrees
-            position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
+            position2 = 360.0*Encoder2.getPulses()/(32.0*131.0); // Position in degrees
         // Speed control
             double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
             double m2_P2 = m2_P1;
@@ -219,7 +205,7 @@
     {
         // Setpoint Motor 1
             reference1 = m1_ref;                           // Reference in degrees
-            position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
+            position1 = 360.0*Encoder1.getPulses()/(32.0*131.0); // Position in degrees
         // Speed control
             double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err);
             double m1_P2 = m1_P1;
@@ -258,46 +244,51 @@
         SampleEMGRight.attach(&EMGfilterRight, 0.01f);
         MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.01f);
         MovingAverageRight.attach(&MovingAverageFilterRight, 0.01f);
-        
-    // Defining threshold 
-        ledG.write(1), led.write(1), ledB.write(1);
+//--------------------------------------------------------------------------------------------------------------------------//
+// Determing Threshold 
+//--------------------------------------------------------------------------------------------------------------------------//      
         wait(20);
     
-    ledG.write(1);
-    wait(0.2);
-    ledG.write(0);
-    wait(0.2);
-    ledG.write(1);
-    wait(0.2);
-    ledG.write(0);
-    wait(0.2);
-    ledG.write(1);
-    wait(0.2);    
-    ledG.write(0);
-    wait(2);
-    Threshold1 = 0.5*EMG_left_MAF;
-    Threshold2 = 0.2*EMG_left_MAF;
-    ledG.write(1);
+        LedG.write(1);
+        wait(0.2);
+        LedG.write(0);
+        wait(0.2);
+        LedG.write(1);
+        wait(0.2);
+        LedG.write(0);
+        wait(0.2);
+        LedG.write(1);
+        wait(0.2);    
+        LedG.write(0);
+        wait(2);
+        Threshold1 = 0.8*EMG_left_MAF;
+        Threshold2 = 0.2*EMG_left_MAF;
+        LedG.write(1);
+        LedR.write(0);
+        wait(2);
+        LedR.write(1);
+
+        wait(2);
+        LedB.write(1);
+        wait(0.2);
+        LedB.write(0);
+        wait(0.2);
+        LedB.write(1);
+        wait(0.2);
+        LedB.write(0);
+        wait(0.2);
+        LedB.write(1);
+        wait(0.2);    
+        LedB.write(0);
+        wait(2);
+        Threshold3 = 0.8*EMG_right_MAF;
+        Threshold4 = 0.2*EMG_right_MAF;
+        LedB.write(1);
+        pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4); 
+        LedR.write(0);
+        wait(2);
+        LedR.write(1);
     
-    wait(2);
-    ledB.write(1);
-    wait(0.2);
-    ledB.write(0);
-    wait(0.2);
-    ledB.write(1);
-    wait(0.2);
-    ledB.write(0);
-    wait(0.2);
-    ledB.write(1);
-    wait(0.2);    
-    ledB.write(0);
-    wait(2);
-    Threshold3 = 0.5*EMG_right_MAF;
-    Threshold4 = 0.2*EMG_right_MAF;
-    ledB.write(1);
-    
-    pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4); 
-    ledG.write(1);
     
 //--------------------------------------------------------------------------------------------------------------------------//
 // Control Program
@@ -306,14 +297,14 @@
     {
                                             //char c = pc.getc();
     // 1 Program UP
-       if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
+       if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
         {
             count = count + 1;
             if(count > 2)
                 {
                     count = 2;
                 }
-
+            wait(2);
         }
      // 1 Program DOWN
      //   if(c == 'd') // Hoe gaat dit aangestuurd worden?
@@ -339,6 +330,7 @@
                         m2_ref = Grens2;
                         m1_ref = -1*Grens1;
                     }
+                    wait(0.1);
                 }
                 if((EMG_right_MAF < Threshold1) && (EMG_left_MAF > Threshold1)) //if (c == 'f') //   
                 {
@@ -349,28 +341,33 @@
                         m2_ref = -1*Grens2;
                         m1_ref = Grens1;
                     }
+                    wait(0.1);
                 }
-        
+        }
     // PROGRAM 1: Motor 1 control, Red LED
         if(count == 1) 
         {
                 LedG = LedB = 1;
                 LedR = 0;
-                if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF <= Threshold1)) // if(c == 't') //
+                if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF <= Threshold1)) // if(c == 't') //
                 {
                     m1_ref = m1_ref + Stapgrootte;
+                    
                     if (m1_ref > Grens1)
                     {
                         m1_ref = Grens1;
                     }
+                    wait(0.1);
                 }
-                if ((EMG_left_MAF > Threshold1) && (EMG_right_MAF < Threshold1)) //if(c == 'g') //
+                if ((EMG_left_MAF > Threshold1) && (EMG_right_MAF < Threshold3)) //if(c == 'g') //
                 {
                     m1_ref = m1_ref - Stapgrootte;
+                  
                     if (m1_ref < -1*Grens1)
                     {
                         m1_ref = -1*Grens1;
                     }
+                    wait(0.1);
                 }
         }
     // PROGRAM 2: Firing mechanism & Reset, Blue LED
@@ -385,6 +382,6 @@
                 m1_ref = 0;
                 count = 0;   
         }
-}
+
 }
 }
\ No newline at end of file