Diff: main.cpp

Revision:

3:ed4676f76a5c

Parent:

2:3f67b4833256

Child:

4:dfe39188f2b2

diff -r 3f67b4833256 -r ed4676f76a5c main.cpp
--- a/main.cpp	Tue Oct 30 15:58:07 2018 +0000
+++ b/main.cpp	Wed Oct 31 10:35:27 2018 +0000
@@ -23,7 +23,9 @@
 BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
 BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
 BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
-BiQuadChain emg0bqc; // merged chain of three filters
+BiQuadChain emg0bqc1; // merged chain of three filters
+BiQuadChain emg0bqc2;
+BiQuadChain emg0bqc3;
 
 AnalogIn    emg1( A1 );     // EMG at A1
 BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
@@ -52,7 +54,7 @@
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
 MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(2);
+HIDScope scope(5);
 
 bool    emg0Bool    = 0;        // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
 int     emg0Ignore  = 0;
@@ -61,6 +63,12 @@
 bool    emg2Bool    = 0;
 int     emg2Ignore  = 0;
 
+double  input = 0;              // raw input
+double  filtHigh = 0;           // filtered after highpass
+double  filtlow = 0;            // filtered after lowpass
+double  filtNotch = 0;          // filtered after notch
+double  emg0filteredAbs;
+
 float threshold0;
 float threshold1;
 float threshold2;
@@ -72,18 +80,31 @@
  * these functions sample the emg and send it to HIDScope
  **/
 bool emg0Filter(void){  
-    double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute, 
+
+    input = emg0.read();
+    scope.set( 0, input);
+    filtHigh = emg0bqc1.step(emg0.read());
+    scope.set( 1, filtHigh);
+    filtlow = emg0bqc2.step(emg0.read());
+    scope.set( 2, filtlow);
+    filtNotch = emg0bqc3.step(emg0.read());
+    scope.set( 3, filtNotch);
+    
     /* this is the threshhold */ 
+    emg0filteredAbs = fabs(filtNotch);
     if (emg0filteredAbs > threshold0) {  // when above threshold  set bool to 1, here can the parameters be changed using global variables
-        emg0Bool = true;
+        emg0Bool = 1;
         emg0Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
         }
     else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
-         emg0Bool = false;
+         emg0Bool = 0;
         }
     else {
         emg0Ignore--;      // else decrease counter by one each time has passed without threshold being met
         }
+
+    scope.set( 4, emg0Bool);
+    scope.send();
     return emg0Bool;
 }
 
@@ -120,8 +141,8 @@
 }
 void sample() {  
     bool Bool1 = emg0Filter(); // whatever name casper uses for the bool
-    bool Bool2 = emg1Filter();
-    bool Bool3 = emg2Filter();
+    // bool Bool2 = emg1Filter();
+    // bool Bool3 = emg2Filter();
 }
 
 void positionCalibration() {
@@ -198,11 +219,15 @@
     directionpin1 = true;
     directionpin2 = true;
     
+    ledred = true;
+    ledgreen = true;
+    ledblue = true;
+    
     // emg filters
     // combining biquad chains is done in main, before the ticker, so only once.
-    emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 );       
-    emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
-    emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
+    emg0bqc1.add( &emg0bq1 );  
+    emg0bqc2.add( &emg0bq1 ).add( &emg0bq2 );
+    emg0bqc3.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 );    // combining biquad chains is done in main, before the ticker, so only once.
     
     MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
     sample_timer.attach(&sample, 0.001);    //ticker at 1000Hz
@@ -222,8 +247,8 @@
                 
             case EmgCalibration:
                 emg0Calibration();
-                emg1Calibration();
-                emg2Calibration();
+                //emg1Calibration();
+                //emg2Calibration();
                 
                 if (!kill_switch) {
                     CurrentState = KILL;