Diff: main.cpp

Revision:

37:c0f6e3b44d7b

Parent:

36:7d7c16c688df

--- a/main.cpp	Mon Oct 30 11:59:02 2017 +0000
+++ b/main.cpp	Mon Oct 30 12:28:49 2017 +0000
@@ -6,7 +6,7 @@
 //Define objects
 AnalogIn    emg( A0 ); //EMG
 AnalogIn    emg1( A1 ); //EMG
-HIDScope    scope( 6 ); // aantal scopes dat gemaakt kan worden
+HIDScope    scope( 2 ); // aantal scopes dat gemaakt kan worden
 DigitalOut  ledB(LED_BLUE); 
 DigitalOut  ledG(LED_GREEN);
 DigitalIn TestButton(PTA4); // button naast het ledje
@@ -20,13 +20,16 @@
 
  int P= 200; // aantal test punten voor de moving average
  double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P
+ double B[200];
  int k = 0; // counter voor de configuratie
  double Vvector[200]; // vector voor de Vwaarde configuratie
  double Vwaarde[2]; // vector voor waardes van V
  int x = 0; // x waarde voor de Vwaarde
- double movmeanR; //moving average for the right arm (first emg-shield)
+ double movmeanR;
+ double movmeanL; //moving average for the right arm (first emg-shield)
  int MotorLock = 0;
  double EMGInputR;
+ double EMGInputL;
  float ErrorZero = 3; 
  double MotorSpeed = 0.5;
  
@@ -36,6 +39,10 @@
 BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
 BiQuad bq2R( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
 BiQuad bq3R(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?
+BiQuadChain bqcL;
+BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
+BiQuad bq2L( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
+BiQuad bq3L(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?
 
 
 
@@ -43,9 +50,11 @@
 void emgSample() {
     
     double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal
+    double emgFilteredL = bqcL.step( emg1.read());
     double emgabsR = abs(emgFilteredR); // absolute waarde van het gefilterde signaal
-    scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek
-    scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek
+    double emgabsL = abs(emgFilteredL);
+    //scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek
+    //scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek
     
     
     // deze for-loop maakt de vector voor de moving average
@@ -65,6 +74,23 @@
     
     movmeanR = sumR/P; //dit is de moving average waarde
     
+    // deze for-loop maakt de vector voor de moving average
+    for(int i = P-1; i >= 0; i--){
+        if (i == 0) {
+            B[i] = emgabsL;
+            }
+         else {
+             B[i] = B[i-1];
+             }   
+    }
+    double sumL = 0;
+    // deze for-loop sommeert de array
+    for (int n = 0; n < P-1; n++) {
+    sumL = sumL + B[n];
+    }
+    
+    movmeanL = sumL/P; //dit is de moving average waarde
+    
     // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
     if (TestButton==0 & k<200) {
         Vvector[k] = movmeanR;
@@ -77,9 +103,9 @@
             sumZ = sumZ + Vvector[n];
             } // neemt de som van de zerovector array
         Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value
-        scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
+        //scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje
         if (x == 1) {
-            scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
+          //  scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje
         }
         k++;
         ledB = 1;
@@ -95,7 +121,8 @@
             x=0;
         }
     }
-    scope.set(2, movmeanR); // stuurt de moving average naar de plot
+    scope.set(0, movmeanR); // stuurt de moving average naar de plot
+    scope.set(1, movmeanL);
     
     if (movmeanR > Vwaarde[1]) {
         movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
@@ -108,7 +135,7 @@
     else {
         EMGInputR = EMGInputRawR;
     } 
-    scope.set(5,EMGInputR);  
+    //scope.set(5,EMGInputR);  
     
     scope.send();
     
@@ -155,6 +182,7 @@
 ledB = 1;
 ledG = 1;
 bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // hier wordt het filter gemaakt
+bqcL.add( &bq1L ).add( &bq2L ).add( &bq3L ); // hier wordt het filter gemaakt
 emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
 Vwaarde[0] = 0.01;
 motor2DirectionPin = 1;