Diff: main.cpp

Revision:

17:d1acb6888b82

Parent:

16:2d115aa2773e

Child:

18:8002c75b8e20

Child:

20:0f6a88f29a71

--- a/main.cpp	Fri Oct 18 10:14:17 2019 +0000
+++ b/main.cpp	Fri Oct 18 13:05:44 2019 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include "HIDScope.h"
-//#include "QEI.h"
+#include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
+#include "BiQuad.h"
 #include "FastPWM.h"
 #include <iostream>
 MODSERIAL pc(USBTX, USBRX);
@@ -19,8 +19,9 @@
 Ticker ticktick;
 Timer state_time;
 Timeout EMG_peak;
+Timeout turn;
 Ticker      sample_timer;
-HIDScope    scope( 3);
+HIDScope    scope( 2);
 DigitalOut  ledred(LED_RED);
 DigitalOut  ledblue(LED_BLUE);
 DigitalOut  ledgreen(LED_GREEN);
@@ -48,6 +49,8 @@
     count0+=1;
     static float RMS0[150];
     static float HighPass0[30];
+    static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
+    static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
     float I0;
     float If0;
     //signal 1
@@ -60,24 +63,25 @@
     float I1;
     float If1;
     I0=emg0.read(); //read signal
-    HighPass_value0+=(I0-HighPass0[count0%30])/30.0;
-    HighPass0[count0%30]=I0;
+    double notched0=Notch0.step(I0);
+    HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
+    HighPass0[count0%30]=notched0;
     If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
     RMS_value0+=(If0-RMS0[count0%150])/150.0;
     RMS0[count0%150]=If0;
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     P0=sqrt(RMS_value0);
     I1=emg1.read(); //read signal
-    HighPass_value1+=(I1-HighPass1[count1%30])/30.0;
-    HighPass1[count1%30]=I1;
+    double notched1=Notch1.step(I1);
+    HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
+    HighPass1[count1%30]=notched1;
     If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
     RMS_value1+=(If1-RMS1[count1%150])/150.0;
     RMS1[count1%150]=If1;
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     P1=sqrt(RMS_value1);
     scope.set(0, P0 ); // send root mean squared
-    scope.set(1, EMG_min0);
-    scope.set(2,EMG_max0);
+    scope.set(1, notched0);
     /* 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 */
@@ -126,29 +130,31 @@
    static bool motor_on=false;
    float Q0;
    Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
-   if (Q0>0.3 && !ignore_peaks)
+   if (Q0>-0.2f && !ignore_peaks)
    {
-        if (!motor_on)
+        if (motor_on)
         {
-            motor1_pwm.write(1.0f);
-            EMG_peak.attach(unignore_peaks,1);
+            motor1_pwm.write(0.0f);
+            EMG_peak.attach(unignore_peaks,0.8);
+            turn.attach(start_ignore_turn,1);
+            ignore_turn=false;
             ignore_peaks=true;
-            motor_on=true;
+            motor_on=false;
         }
         else if(ignore_turn)
         {
-            motor1_pwm.write(0.0f);
-            EMG_peak.attach(unignore_peaks,1);
-            EMG_peak.attach(start_ignore_turn,1.5);
+            motor1_pwm.write(1.0f);
+            EMG_peak.attach(unignore_peaks,0.8);
+            turn.attach(start_ignore_turn,1);
             ignore_turn=false;
             ignore_peaks=true;
-            motor_on=false;
+            motor_on=true;
         }
         else
         {
             motor1_pwm.write(1.0f);
             dir1=!dir1;
-            EMG_peak.attach(unignore_peaks,1);
+            EMG_peak.attach(unignore_peaks,1.5);
             ignore_peaks=true;
             motor_on=true;
         }