Diff: main.cpp

Revision:

36:7d7c16c688df

Parent:

35:5a8a7bd8ae59

Child:

37:c0f6e3b44d7b

--- a/main.cpp	Thu Oct 26 10:05:07 2017 +0000
+++ b/main.cpp	Mon Oct 30 11:59:02 2017 +0000
@@ -24,50 +24,50 @@
  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 movmean; 
+ double movmeanR; //moving average for the right arm (first emg-shield)
  int MotorLock = 0;
- double EMGInput;
+ double EMGInputR;
  float ErrorZero = 3; 
  double MotorSpeed = 0.5;
  
  
  // Filters 
-BiQuadChain bqc;
-BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp?
-BiQuad bq2( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306,   -1.2243497755555959, 0.5133795508233266); //hp?
-BiQuad bq3(  0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   -1.2243497755555959, 0.5133795508233266); // notch?
+BiQuadChain bqcR;
+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?
 
 
 
 // sample function voor plotten van de emg signalen en moving average 
 void emgSample() {
     
-    double emgFiltered = bqc.step( emg.read() ); // gefilterde waarde van het emg signaal
-    double emgabs = abs(emgFiltered); // absolute waarde van het gefilterde signaal
-    scope.set(0, emgFiltered ); // stuurt de waarden naar de grafiek
-    scope.set(1, emgabs ); // stuurt de waarden naar de grafiek
+    double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal
+    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
     
     
     // deze for-loop maakt de vector voor de moving average
     for(int i = P-1; i >= 0; i--){
         if (i == 0) {
-            A[i] = emgabs;
+            A[i] = emgabsR;
             }
          else {
              A[i] = A[i-1];
              }   
     }
-    double sum = 0;
+    double sumR = 0;
     // deze for-loop sommeert de array
     for (int n = 0; n < P-1; n++) {
-    sum = sum + A[n];
+    sumR = sumR + A[n];
     }
     
-    movmean = sum/P; //dit is de moving average waarde
+    movmeanR = sumR/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] = movmean;
+        Vvector[k] = movmeanR;
         ledB = !ledB;
         k++;
     }    
@@ -95,20 +95,20 @@
             x=0;
         }
     }
-    scope.set(2, movmean); // stuurt de moving average naar de plot
+    scope.set(2, movmeanR); // stuurt de moving average naar de plot
     
-    if (movmean > Vwaarde[1]) {
-        movmean = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
+    if (movmeanR > Vwaarde[1]) {
+        movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn
     }
     
-    double EMGInputRaw =  (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); 
-    if (EMGInputRaw < 0) {
-        EMGInput = 0;
+    double EMGInputRawR =  (movmeanR - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); 
+    if (EMGInputRawR < 0) {
+        EMGInputR = 0;
     }
     else {
-        EMGInput = EMGInputRaw;
+        EMGInputR = EMGInputRawR;
     } 
-    scope.set(5,EMGInput);  
+    scope.set(5,EMGInputR);  
     
     scope.send();
     
@@ -121,7 +121,7 @@
     // Positive value means clockwise rotation.
     const float maxVelocity=8.4; // in rad/s of course!
     float referenceVelocity;  // in rad/s
-    referenceVelocity = (EMGInput * maxVelocity) * MotorLock; 
+    referenceVelocity = (EMGInputR * maxVelocity) * MotorLock; 
     return referenceVelocity;
 }
 
@@ -154,7 +154,7 @@
 {  
 ledB = 1;
 ledG = 1;
-bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt
+bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // 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;
@@ -165,7 +165,7 @@
         }
         else MotorLock = 1;
        // MotorOn.rise(&MotorLocker);
-        motor2MagnitudePin = EMGInput*MotorLock;
+        motor2MagnitudePin = EMGInputR*MotorLock;
         wait(0.01);
     }
 }
\ No newline at end of file