Diff: main.cpp

Revision:

36:5d1225d72bca

Parent:

35:b71140a46b9c

Child:

37:0dbf536a95c8

--- a/main.cpp	Tue Oct 27 16:38:47 2015 +0000
+++ b/main.cpp	Wed Oct 28 14:11:09 2015 +0000
@@ -18,10 +18,8 @@
 DigitalIn       Button(PTC6);
 DigitalIn       Button2(PTA4);
 DigitalIn       Button3(D9);
-//AnalogIn        EMG(A0);
-//AnalogIn        EMG2(A1);
-AnalogIn        EMG(A2);
-AnalogIn        EMG2(A3);
+AnalogIn        EMG(A0);
+AnalogIn        EMG2(A1);
 
 // Tickers & timers
 
@@ -50,7 +48,7 @@
 double          T4 = 0;
 
 // Motor 1 (Translatie)
-const double          n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
+const double    n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
 int             Pulses;
 double          Rotatie = 0; //aantal graden dat de motor is gedraaid
 double          Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
@@ -68,7 +66,7 @@
 bool            OutRange = false;
 
 // Motor 2 (Rotatie)
-const double          n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
+const double    n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
 int             Pulses2;
 double          Rotatie2 = 0;
 double          Goal2;
@@ -104,9 +102,11 @@
 double y6;
 double y7;
 double y8;
+double y9;
+double y10;
 
 double u10;
-double y10;
+double y11;
 double y12;
 double y13;
 double y14;
@@ -140,10 +140,10 @@
 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 gainHP1=0.96860379641660377;
+const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.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;
+const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000;
 
 // Low pass
 const double gainLP6=0.000048;
@@ -186,8 +186,8 @@
     // EMG 2
     u10 = EMG2.read();
     // Notch
-    y10 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
-    y12 = biquad(y10,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
+    y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
+    y12 = biquad(y11,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
     y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
 
     // HP
@@ -198,7 +198,6 @@
     y16 = fabs(y15);
     y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
     y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
-    y18 = EMG.read();
 
     if (Cali == true) {
         if (y8 >= Max) {
@@ -286,11 +285,11 @@
         led = 1;
         TijdEMGCal.stop();
         TijdEMGCal.reset();
-        T1 = 0.2*Max;
-        T2 = 0.4*Max;
-        T3 = 0.2*Max2;
-        T4 = 0.4*Max2;
-        pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4);
+        T1 = 0.35*Max;
+        T2 = 0.6*Max;
+        T3 = 0.35*Max2;
+        T4 = 0.6*Max2;
+        //pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4);
         wait (3);
         Excecute = true;
     }
@@ -327,11 +326,11 @@
             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
                 if (y8 >= T2 ) {
                     Direction = 1;
-                    v = 0.15;
+                    v = 0.12;
                 }
                 if (y8 > T1 && y8 < T2) {
                     Direction = 0;
-                    v = 0.15;
+                    v = 0.08;
                 }
                 if (y8 <= T1) {
                     Direction = 0;
@@ -371,7 +370,7 @@
                 PowerServo.write(0.04);
                 Fired=Fired+1;
                 pc.printf("Fire = %i", Fired);
-                if (Fired == 3) {
+                if (Fired == 4) {
                     wait (1);
                     Home = true;
                     Excecute = false;
@@ -387,9 +386,10 @@
         while (Home) { //Terugkeren naar vaste positie
             pc.printf("Home\n");
             OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
+            OutRange2 = true;
             Goal = margin;
             Goal2 = 0;
-            if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) {
+            if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) {
                 timer.start();
             } else {
                 timer.stop();