Diff: main.cpp

Revision:

32:c2c80a2ca83d

Parent:

31:85d3b4db5e2b

Child:

33:4e3870ab4e17

--- a/main.cpp	Tue Oct 20 13:25:36 2015 +0000
+++ b/main.cpp	Wed Oct 21 13:53:23 2015 +0000
@@ -14,9 +14,10 @@
 QEI             Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
 PwmOut          PowerServo(D3);
 
-// Buttons & EMG (PotMeter)
+// Buttons & EMG
 DigitalIn       Button(PTC6);
 DigitalIn       Button2(PTA4);
+DigitalIn       Button3(D9);
 AnalogIn        EMG(A0);
 AnalogIn        EMG2(A1);
 
@@ -31,6 +32,7 @@
 // Debugging
 Serial          pc(USBTX, USBRX);
 HIDScope        scope(6);
+DigitalOut      led(LED_RED);
 
 
 
@@ -40,10 +42,10 @@
 int             Fired = 0;
 
 // EMG
-double          emg_value;
-double          emg_value2;
-const double    T1 = 0.33333; // Treshold 1
-const double    T2 = 0.66666; // Treshold 2
+double          T1 = 0;
+double          T2 = 0;
+double          T3 = 0;
+double          T4 = 0;
 
 // Motor 1 (Translatie)
 double          n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
@@ -57,7 +59,7 @@
 const double    Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
 const double    Kd = 10;
 const double    Ki = 0.2;
-double          v = 0; //snelheid van de motor (0-0.1)?
+double          v = 0;
 double          upperlimit; //max aantal rotaties omhoog
 const double    downlimit = 0.4;
 const double    margin = 0.4;
@@ -85,7 +87,7 @@
 bool            Home = false;
 
 // Filter
-double Fs2 = 500; // in Hz
+double Fs2 = 1000; // in Hz
 const double TijdCal = 5;
 double Max = 0;
 double Max2 = 0;
@@ -165,34 +167,34 @@
     // 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);
+    y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
+    y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
+    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);
+    y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
+    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);
+    y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
+    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);
+    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);
+    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);
+    y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
+    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);
+    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);
 
     if (Cali == true) {
         if (y8 >= Max) {
@@ -250,9 +252,43 @@
     Rotatie = (Pulses*twopi)/4200;
     Pulses2 = Encoder2.getPulses();
     Rotatie2 = (Pulses2*twopi)/4200;
+    scope.set(0,Goal);
+    scope.set(1,Rotatie);
+    scope.set(2,y8);
+    scope.set(3,Goal2);
+    scope.set(4,Rotatie2);
+    scope.set(5,y18);
     scope.send();
 }
 
+void Calibration ()
+{
+    if (OutRange == false && OutRange2 == false) {
+        if (Button == 0) {
+            Cali = true;
+            TijdEMGCal.start();
+            Excecute = false;
+            led = 0;
+            v = 0;
+            v2 = 0;
+        }
+    }
+    if (TijdEMGCal.read() >= TijdCal) {
+        Cali = false;
+        led = 1;
+        TijdEMGCal.stop();
+        TijdEMGCal.reset();
+        T1 = 0.1*Max;
+        T2 = 0.25*Max;
+        T3 = 0.1*Max2;
+        T4 = 0.25*Max2;
+        pc.printf("Max = %f\nT1 = %f\nT2 = %f", Max, T1, T2);
+        wait (3);
+        Excecute = true;
+
+    }
+}
+
 int main()
 {
     upperlimit = n1*twopi;
@@ -264,30 +300,9 @@
     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);
+    led = 1;
     while (true) {
-        Encoder.reset();
-        Encoder2.reset();
-        if (Button == 0) {
-            Cali = true;
-            TijdEMGCal.start();
-        }
-        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;
-        }
+        Calibration();
         while (Excecute) {
             // Eerst wordt motor 1 aangestuurd
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
@@ -302,15 +317,15 @@
                 OutRange = false;
             }
             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
-                if (emg_value >= T2 ) {
+                if (y8 >= T2 ) {
                     Direction = 1;
                     v = 0.1;
                 }
-                if (emg_value > T1 && emg_value < T2) {
+                if (y8 > T1 && y8 < T2) {
                     Direction = 0;
                     v = 0.1;
                 }
-                if (emg_value <= T1) {
+                if (y8 <= T1) {
                     Direction = 0;
                     v = 0;
                 }
@@ -329,15 +344,15 @@
                 OutRange2 = false;
             }
             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
-                if (emg_value2 >= T2 ) {
+                if (y18 >= T4 ) {
                     Direction2 = 1;
                     v2 = 0.1;
                 }
-                if (emg_value2 > T1 && emg_value2 < T2) {
+                if (y18 > T3 && y18 < T4) {
                     Direction2 = 0;
                     v2 = 0.1;
                 }
-                if (emg_value2 <= T1) {
+                if (y18 <= T3) {
                     Direction2 = 0;
                     v2 = 0;
                 }
@@ -354,6 +369,11 @@
                     Excecute = false;
                 }
             }
+            if (Button3 == 0) {
+                Excecute = false;
+                Home = true;
+            }
+            Calibration();
         }
 
         while (Home) { //Terugkeren naar vaste positie
@@ -376,6 +396,10 @@
                 Errord2 = 0;
                 Errorp2 = 0;
                 Fired = 0;
+                wait (3);
+                Encoder.reset();
+                Encoder2.reset();
+                Excecute = true;
             }
         }
     }