Diff: main.cpp

Revision:

3:be5ac89a0b53

Parent:

2:36ad60c0aa01

Child:

4:5ceb8f058874

--- a/main.cpp	Thu Oct 18 14:16:08 2018 +0000
+++ b/main.cpp	Fri Oct 19 08:05:58 2018 +0000
@@ -1,11 +1,11 @@
 #include "mbed.h"
 #include "math.h"
 #include "BiQuad.h"
-//#include "Servo.h"
-#include "HIDScope.h"
+#include "Servo.h"
+//#include "HIDScope.h"
 
 
-//Servo myservo(D7);
+
 //Define objects
 AnalogIn    emgL( A0 );                                                         // EMG Left Arm
 AnalogIn    emgR( A1 );                                                         // EMG Right Arm
@@ -13,12 +13,12 @@
 DigitalOut  ledR(LED_RED);
 DigitalOut  ledG(LED_GREEN);                                                    // Informative LED for gaining zero and max
 DigitalIn   TestButton(PTA4);                                                   // Button used for gaining zero and max
-DigitalIn   onoff(PTC6);                                                        // Button used for switching between zero and max
+DigitalIn   zeromax(PTC6);                                                        // Button used for switching between zero and max
 Ticker      emgSampleTicker;                                                    // Ticker for sample frequency
-DigitalOut  signalL(D4);                                                        // Output to motorshield, emg left biceps
-DigitalOut  signalR(D5); 
-HIDScope    scope( 2 );                                                       // Output to motorshield, emg right biceps
-
+//HIDScope    scope( 4 );                                                       
+Servo myservo(D13);
+DigitalOut motor1direction(D7);
+PwmOut motor1control(D6);
 
 
  int P= 200;                                                                    // Number of points for moving average first emg
@@ -54,9 +54,10 @@
 
 
 void emgSample() {                                                              // EMG function
-    
-    double emgFilteredR = bqcR.step( emgR.read() );  
-    double emgabsR = abs(emgFilteredR);                                         // Absolute value of EMG signal right biceps          
+                                                             
+    double emgNOFilteredR = bq3R.step(emgR.read());                          // Filtered HP value of EMG signal left biceps                
+    double emgHPFilteredR = bq2R.step(emgNOFilteredR);                            // Filtered HP value of EMG signal left biceps
+    double emgabsR = fabs(emgHPFilteredR);                                       // Absolute value of EMG signal left biceps          
     
     double emgNOFilteredL = bq3L.step(emgL.read());                          // Filtered HP value of EMG signal left biceps                
     double emgHPFilteredL = bq2L.step(emgNOFilteredL);                            // Filtered HP value of EMG signal left biceps
@@ -79,13 +80,6 @@
     }
     movmeanL = sumL/P;     
     
-     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
-    scope.set(0, emgL.read() );
-    scope.set(1, movmeanL);
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
-    *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
-    scope.send();                                                     // Moving average value left biceps
     
     for(int i = P-1; i >= 0; i--){                                              // For-loop used for moving average
         if (i == 0) {
@@ -101,11 +95,22 @@
     sumR = sumR + B[n];
     }
     movmeanR = sumR/P; //dit is de moving average waarde                        // Moving average value right biceps
+    
+    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+   // scope.set(0, emgL.read() );
+    //scope.set(1, movmeanL);
+    //scope.set(2, emgR.read());
+    //scope.set(3, movmeanR);
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    *  Ensure that enough channels are available (HIDScope scope( 2 ))
+    *  Finally, send all channels to the PC at once */
+   // scope.send();                                                     // Moving average value left biceps
 
 
     if (TestButton==0 & k<200) {                                                // Loop used for gaining max and zero value
         Lvector[k] = movmeanL;
         Rvector[k] = movmeanR;
+        
         if (x==0){
         ledB = !ledB;                                                           // SPIER NIET AANSPANNEN BIJ BLAUW
         }                                                                       // SPIER WEL AANSPANNEN BIJ ROOD
@@ -128,7 +133,7 @@
         ledR = 1;
         ledG = !ledG;
     }
-    else if (k == 201 && onoff ==0) {                                           // Loop used for switching between zero and max
+    else if (k == 201 && zeromax ==0) {                                           // Loop used for switching between zero and max
         ledG = !ledG;
         k = 0;
         if (x==0) {
@@ -143,24 +148,6 @@
         thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
         thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
     }
-    
-        if (movmeanL > thresholdL)                                              // Signal sent to the motors
-        {
-            signalL = 1;
-        }
-        else 
-        {
-            signalL = 0;
-        }
-        
-        if (movmeanR > thresholdR)
-        {
-            signalR = 1;
-        }
-        else 
-        {
-            signalR = 0;
-        }
 }
 
 int main()
@@ -168,19 +155,26 @@
 ledB = 1;
 ledG = 1;
 ledR = 1;
-bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R );                                    // Filter for emg signal
 emgSampleTicker.attach( &emgSample, 0.002 );                                    // Ticker for EMG function
 Lwaarde[0] = 0.01;                                                              // Startvalue for zerovalue, to - 
 Rwaarde[0] = 0.01;                                                              // - prevent dividing by 0
     while(1) {
-       // 
-//        if (movmeanL > (0.02))
-//        {   myservo = 0.5;
-//            wait(0.01);
-//        }
-//        else {
-//            myservo = 0.0;
-//            wait(0.01);
-//            }
+        
+        if (movmeanR > thresholdR)
+        {   myservo = 0.5;
+            wait(0.01);
+        }
+        else {
+            myservo = 0.0;
+            wait(0.01);
+            }
+            
+        if (movmeanL > thresholdL)
+        {   motor1control.write(0.8);
+            motor1direction.write(true);
+            }
+        else {
+            motor1control.write(0);
+            }
     }
 }
\ No newline at end of file