Diff: main.cpp

Revision:

30:4c6321941515

Parent:

29:a48b63e60a40

Child:

31:620950f7e514

Child:

32:3832f732f17a

--- a/main.cpp	Wed Oct 25 12:21:50 2017 +0000
+++ b/main.cpp	Wed Oct 25 12:38:09 2017 +0000
@@ -26,7 +26,9 @@
  double Vwaarde[2]; // vector voor waardes van V
  int x = 0; // x waarde voor de Vwaarde
  double movmean; 
- int MotorLock = 0; 
+ int MotorLock = 0;
+ double EMGInput;
+ float ErrorZero = 1.5; 
  
  // Filters 
 BiQuadChain bqc;
@@ -91,7 +93,13 @@
             x=0;
         }
     }
-    double EMGInput =  (movmean - Vwaarde[0]*3)/(Vwaarde[1] - Vwaarde[0]*3); 
+    double EMGInputRaw =  (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); 
+    if (EMGInputRaw < 0) {
+        EMGInput = 0;
+    }
+    else {
+        EMGInput = EMGInputRaw;
+    } 
     scope.set(5,EMGInput);  
     scope.set(2, movmean); // stuurt de moving average naar de plot
     
@@ -104,7 +112,7 @@
     // Positive value means clockwise rotation.
     const float maxVelocity=8.4; // in rad/s of course!
     float referenceVelocity;  // in rad/s
-    referenceVelocity = ((movmean - Vwaarde[0]*3)/(Vwaarde[1] - Vwaarde[0]*3) * maxVelocity) * MotorLock; 
+    referenceVelocity = (EMGInput * maxVelocity) * MotorLock; 
     return referenceVelocity;
 }