Diff: main.cpp

Revision:

2:3f67b4833256

Parent:

1:afb820c6fc0d

Child:

3:ed4676f76a5c

diff -r afb820c6fc0d -r 3f67b4833256 main.cpp
--- a/main.cpp	Tue Oct 30 14:26:15 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:58:07 2018 +0000
@@ -2,6 +2,10 @@
 #include "QEI.h"
 #include "HIDScope.h"
 #include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "math.h"
+
+#define IGNORECOUNT 100
 
 PwmOut pwmpin1(D6);
 PwmOut pwmpin2(D5);
@@ -14,34 +18,121 @@
 QEI motor1(D13,D12,NC, 32);
 QEI motor2(D11,D10,NC, 32);
 
+//Define objects
+AnalogIn    emg0( A0 );     // EMG at A0
+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
+
+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
+BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
+BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
+BiQuadChain emg1bqc; // merged chain of three filters
+
+
+AnalogIn    emg2( A2 );     // EMG at A2
+BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
+BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
+BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
+BiQuadChain emg2bqc; // merged chain of three filters
+
 DigitalIn kill_switch(SW2); //position has to be changed
 DigitalIn next_switch(SW3); //name and position should be replaced
 
+enum states{PositionCalibration, EmgCalibration, Movement, KILL};
+states CurrentState;
+Ticker sample_timer;
+Ticker MotorsTicker;
+Timer timer;
+
 //for testing purposes
 DigitalOut ledred(LED_RED);
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
+MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(2);
 
-enum states{PositionCalibration, EmgCalibration, Movement, KILL};
-states CurrentState;
-MODSERIAL pc(USBTX, USBRX);
-//HIDScope scope(2);
+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;
+bool    emg1Bool    = 0;
+int     emg1Ignore  = 0;
+bool    emg2Bool    = 0;
+int     emg2Ignore  = 0;
 
-Ticker MotorsTicker;
+float threshold0;
+float threshold1;
+float threshold2;
 
 volatile float pwm_value1 = 0.0;
 volatile float pwm_value2 = 0.0;
 
+/** Sample functions
+ * these functions sample the emg and send it to HIDScope
+ **/
+bool emg0Filter(void){  
+    double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute, 
+    /* this is the threshhold */ 
+    if (emg0filteredAbs > threshold0) {  // when above threshold  set bool to 1, here can the parameters be changed using global variables
+        emg0Bool = true;
+        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;
+        }
+    else {
+        emg0Ignore--;      // else decrease counter by one each time has passed without threshold being met
+        }
+    return emg0Bool;
+}
+
+bool emg1Filter(void){  
+    double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
+    /* this is the threshhold */ 
+    if (emg1filteredAbs > threshold1) {  // when above threshold  set bool to 1 here can the parameters be changed using global variables
+        emg1Bool = true;
+        emg1Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
+        }
+    else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
+         emg1Bool = false;
+        }
+    else {
+        emg1Ignore--;      // else decrease counter by one each time has passed without threshold being met
+        }
+    return emg1Bool;
+}
+
+bool emg2Filter(void){  
+    double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
+    /* this is the threshhold */ 
+    if (emg2filteredAbs > threshold2) {  // when above threshold  set bool to 1 here can the parameters be changed using global variables
+        emg2Bool = true;
+        emg2Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
+        }
+    else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
+         emg2Bool = false;
+        }
+    else {
+        emg2Ignore--;      // else decrease counter by one each time has passed without threshold being met
+        }
+    return emg2Bool;
+}
+void sample() {  
+    bool Bool1 = emg0Filter(); // whatever name casper uses for the bool
+    bool Bool2 = emg1Filter();
+    bool Bool3 = emg2Filter();
+}
+
 void positionCalibration() {
     while(!button1){
-        directionpin2 = true;
-        pwm_value2 = 1.0f;
+        directionpin1 = true;
+        pwm_value1 = 0.7f;
     } 
-    pwm_value2 = 0.0f;
+    pwm_value1 = 0.0f;
     while(!button2){
         directionpin2 = true;
         pwm_value2 = 0.7f;
-        //wait(0.01f);
     }
     pwm_value2 = 0.0f;
         
@@ -54,16 +145,41 @@
     }
 }
 
-void emgCalibration() {
-    ledblue = !ledblue;
-    wait(0.5f);
-    
-    if (!next_switch) {
+void emg0Calibration() {
+    int C = 500;           // half a second at 1000Hz
+    double A0=0, A1=0, A2=0, A3=0, A4=0;
+    double emg0FiAbs;
+    while (C > 0){
+        emg0FiAbs = fabs( emg1bqc.step(emg0.read()));
+        if (C==500){                        //first instance make all values the first in case this is the highest
+            A0=A1=A2=A3=A4=emg0FiAbs;
+        }
+        else if(emg0FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
+            A4=A3;
+            A3=A2;
+            A2=A1;
+            A1=A0;
+            A0=emg0FiAbs;
+        }
+        C--;
+        wait(0.001f);
+        threshold0 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
+     }
+     
+     if (!next_switch) {
         CurrentState = Movement;
         pc.printf("current state = Movement\n\r");
     }
 }
 
+void emg1Calibration() {
+    
+}
+
+void emg2Calibration() {
+    
+}
+
 void movement() {
     
 }
@@ -81,22 +197,23 @@
     pwmpin2.period_us(60);
     directionpin1 = true;
     directionpin2 = true;
-    ledred = true;
-    ledgreen = true;
-    ledblue = true;
     
-    MotorsTicker.attach(&move_motors, 0.02f);
+    // 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 );
+    
+    MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
+    sample_timer.attach(&sample, 0.001);    //ticker at 1000Hz
+
     CurrentState = PositionCalibration;
     pc.printf("current state = PositionCalibration\n\r");
     
     while (true) {
         switch(CurrentState) {
             case PositionCalibration:
-                ledred = true;
-                ledgreen = false;
-                ledblue = true;
                 positionCalibration();
-                
                 if (!kill_switch) {
                     CurrentState = KILL;   
                     pc.printf("current state = KILL\n\r"); 
@@ -104,10 +221,9 @@
             break;
                 
             case EmgCalibration:
-                ledred = true;
-                ledgreen = true;
-                // ledblue = false;
-                emgCalibration();
+                emg0Calibration();
+                emg1Calibration();
+                emg2Calibration();
                 
                 if (!kill_switch) {
                     CurrentState = KILL;   
@@ -116,9 +232,6 @@
             break;
             
             case Movement:
-                ledred = true;
-                ledgreen = false;
-                ledblue = false;
                 movement();
                 
                 if (!kill_switch) {
@@ -132,16 +245,23 @@
                 ledgreen = true;
                 ledblue = true;
                 ledred = false;
-                //turnoffmotors(); //placeholder
-                //flashsos() //placeholder 
-                wait(1.0f);
-                while(u) {
-                    if (!kill_switch) {
-                        u = false;
-                        ledred = true;
-                        wait(1.0f);
-                        CurrentState = PositionCalibration;
-                        pc.printf("current state = PositionCalibration\n\r");
+                
+                pwm_value1 = 0;
+                pwm_value2 = 0;
+                
+                timer.start();
+                if (timer.read_ms()> 2000) {
+                    timer.stop();
+                    timer.reset();
+                    while(u) {
+                        if (!kill_switch) {
+                            timer.start();
+                            u = false;
+                            ledred = true;
+                            CurrentState = PositionCalibration;
+                            pc.printf("current state = PositionCalibration\n\r");
+                            wait(1.0f); 
+                        }
                     }
                 }
             break;