Diff: main.cpp

Revision:

31:85d3b4db5e2b

Parent:

30:37e778f27fce

--- a/main.cpp	Tue Oct 20 12:03:30 2015 +0000
+++ b/main.cpp	Tue Oct 20 13:25:36 2015 +0000
@@ -17,16 +17,16 @@
 // Buttons & EMG (PotMeter)
 DigitalIn       Button(PTC6);
 DigitalIn       Button2(PTA4);
-AnalogIn        PotMeter(A0);
-AnalogIn        PotMeter2(A2);
-//AnalogIn        EMG(A0);
-//AnalogIn        Emg(A1);
+AnalogIn        EMG(A0);
+AnalogIn        EMG2(A1);
 
 // Tickers & timers
+
+Ticker          biquadTicker;
 Ticker          MotorWrite;
 Ticker          Sender;
-Ticker          sampleEMG;
 Timer           timer;
+Timer           TijdEMGCal;
 
 // Debugging
 Serial          pc(USBTX, USBRX);
@@ -84,10 +84,126 @@
 bool            Excecute = false;
 bool            Home = false;
 
+// Filter
+double Fs2 = 500; // in Hz
+const double TijdCal = 5;
+double Max = 0;
+double Max2 = 0;
+bool Cali = false;
+
+double u1;
+double y1;
+double y2;
+double y3;
+double y4;
+double y5;
+double y6;
+double y7;
+double y8;
+
+double u10;
+double y10;
+double y12;
+double y13;
+double y14;
+double y15;
+double y16;
+double y17;
+double y18;
+
+double f1_v1=0,f1_v2=0;
+double f2_v1=0,f2_v2=0;
+double f3_v1=0,f3_v2=0;
+double f4_v1=0,f4_v2=0;
+double f5_v1=0,f5_v2=0;
+double f6_v1=0,f6_v2=0;
+double f7_v1=0,f7_v2=0;
+
+double f1_v3=0,f1_v4=0;
+double f2_v3=0,f2_v4=0;
+double f3_v3=0,f3_v4=0;
+double f4_v3=0,f4_v4=0;
+double f5_v3=0,f5_v4=0;
+double f6_v3=0,f6_v4=0;
+double f7_v3=0,f7_v4=0;
+
+// Notch
+const double gainNotch3=0.969922;
+const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000;
+const double gainNotch4=0.975183;
+const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000;
+const double gainNotch5=0.993678;
+const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000;
+
+// High pass
+const double gainHP1=0.939472;
+const double f1_a1=-0.87894202296,f1_a2=0.00000000,f1_b0=1.0000000,f1_b1=-1.00000000,f1_b2=0.000000000;
+const double gainHP2=0.935820;
+const double f2_a1=-1.86387364983,f2_a2=0.87941229211,f2_b0=1.0000000000,f2_b1=-2.0000000,f2_b2=1.0000000;
+
+// Low pass
+const double gainLP6=0.000048;
+const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000;
+const double gainLP7=0.000048;
+const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000;
+
 
 
 // Voids voor berekeningen in het hoofdprogramma
 
+double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u-a1*v1-a2*v2;
+    double y=b0*v+b1*v1+b2*v2;
+    v2=v1;
+    v1=v;
+    return y;
+}
+
+void myController()
+{
+    // EMG 1
+    u1 = EMG.read();
+    // Notch
+    double y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
+    double y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
+    double y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
+
+    // HP
+    double y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
+    double y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
+
+    // LP
+    y6 = fabs(y5);
+    double y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
+    double y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
+
+    // EMG 2
+    u10 = EMG2.read();
+    // Notch
+    double y10 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
+    double y12 = biquad(y10,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
+    double y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
+
+    // HP
+    double y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
+    double y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
+
+    // LP
+    y16 = fabs(y15);
+    double y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
+    double y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
+
+    if (Cali == true) {
+        if (y8 >= Max) {
+            Max = y8;
+        }
+        if (y18 >= Max2) {
+            Max2 = y18;
+        }
+    }
+}
+
 void MotorSet()
 {
     // Eerst motor 1 (translatie)
@@ -131,42 +247,11 @@
 void Send()
 {
     Pulses = Encoder.getPulses();
-    Rotatie = (Pulses*twopi)/4200; 
+    Rotatie = (Pulses*twopi)/4200;
     Pulses2 = Encoder2.getPulses();
     Rotatie2 = (Pulses2*twopi)/4200;
-    scope.set(0,Goal);
-    scope.set(1,Rotatie);
-    scope.set(2,emg_value);
-    scope.set(3,Goal2);
-    scope.set(4,Rotatie2);
-    scope.set(5,emg_value2);
     scope.send();
 }
-void EMGsample()
-{
-    // Lees het EMG signaal uit
-    //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
-    emg_value = PotMeter.read();
-    emg_value2 = PotMeter2.read();
-}
-
-void Fire ()
-{
-    PowerServo.write(0.27);
-    wait (1);
-    PowerServo.write(0.04);
-    Fired=Fired+1;
-    pc.printf("Fire = %i", Fired);
-    if (Fired == 3) {
-        wait (1);
-        Excecute = false;
-        Home = true;
-     }
-}
-
-
-// Calibratie moet nog worden uitgevoerd!!!!
-
 
 int main()
 {
@@ -175,17 +260,35 @@
     pc.baud(115200);
     PowerMotor.write(0);
     PowerMotor2.write(0);
-    Sender.attach(Send,0.5/Fs);
-    MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
-    sampleEMG.attach(EMGsample,0.5/Fs);
+    Sender.attach(Send,1/Fs);
+    MotorWrite.attach(MotorSet,1/Fs);
+    biquadTicker.attach(myController,1/Fs2);
     PowerServo.period_ms(20);
+
+    scope.set(0,Goal);
+    scope.set(1,Rotatie);
+    scope.set(2,emg_value);
+    scope.set(3,Goal2);
+    scope.set(4,Rotatie2);
+    scope.set(5,emg_value2);
     while (true) {
         Encoder.reset();
         Encoder2.reset();
         if (Button == 0) {
-            Excecute =! Excecute;
+            Cali = true;
+            TijdEMGCal.start();
         }
-        while (Excecute ) {
+        if (TijdEMGCal.read() >= TijdCal) {
+            Cali = false;
+            TijdEMGCal.stop();
+            TijdEMGCal.reset();
+            double T1 = 0.2*Max;
+            double T2 = 0.5*Max2;
+            pc.printf("Max = %f\nT1 = %f\nT2 = %f", Max, T1, T2);
+            wait (3);
+            Excecute = true;
+        }
+        while (Excecute) {
             // Eerst wordt motor 1 aangestuurd
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
                 Goal = upperlimit - margin;
@@ -240,7 +343,16 @@
                 }
             }
             if (Button2 == 0) { //Afvuren van de RBG
-                Fire();
+                PowerServo.write(0.27);
+                wait (1);
+                PowerServo.write(0.04);
+                Fired=Fired+1;
+                pc.printf("Fire = %i", Fired);
+                if (Fired == 3) {
+                    wait (1);
+                    Home = true;
+                    Excecute = false;
+                }
             }
         }