Revision 4:dfe39188f2b2, committed 2018-11-01

Comitter:

CasperK

Date:

Thu Nov 01 14:11:05 2018 +0000

Parent:

3:ed4676f76a5c

Commit message:

Implemented emg and emgcalibration

Changed in this revision

--- a/MODSERIAL.lib	Wed Oct 31 10:35:27 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/Sissors/code/MODSERIAL/#da0788f0bd77

--- a/main.cpp	Wed Oct 31 10:35:27 2018 +0000
+++ b/main.cpp	Thu Nov 01 14:11:05 2018 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "HIDScope.h"
-#include "MODSERIAL.h"
+//#include "MODSERIAL.h"
 #include "BiQuad.h"
 #include "math.h"
 
@@ -23,9 +23,7 @@
 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 emg0bqc1; // merged chain of three filters
-BiQuadChain emg0bqc2;
-BiQuadChain emg0bqc3;
+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
@@ -33,7 +31,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
@@ -43,8 +40,11 @@
 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;
+enum position_states{PositionCalibration, EmgCalibration, Movement, KILL};
+enum emg_states{EMG0, EMG1, EMG2};
+position_states CurrentState;
+emg_states CalibrationState;
+
 Ticker sample_timer;
 Ticker MotorsTicker;
 Timer timer;
@@ -53,8 +53,8 @@
 DigitalOut ledred(LED_RED);
 DigitalOut ledgreen(LED_GREEN);
 DigitalOut ledblue(LED_BLUE);
-MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(5);
+//MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(6);
 
 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;
@@ -62,6 +62,9 @@
 int     emg1Ignore  = 0;
 bool    emg2Bool    = 0;
 int     emg2Ignore  = 0;
+float   Calibration0;
+float   Calibration1;
+float   Calibration2;
 
 double  input = 0;              // raw input
 double  filtHigh = 0;           // filtered after highpass
@@ -76,42 +79,115 @@
 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){  
+void positionCalibration() {
+    while(!button1) {
+        directionpin1 = true;
+        pwm_value1 = 0.7f;
+    }
+    pwm_value1 = 0.0f;
+    while(!button2) {
+        directionpin2 = true;
+        pwm_value2 = 0.6f;
+    }
+    pwm_value2 = 0.0f;
+    
+    if(!next_switch) {
+        CurrentState = EmgCalibration;
+    }
+}
+
+void CalibrateEMG0(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
+    ledred = !ledred;
+    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(emg1.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);
+    }
+    Calibration0 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
+    ledred = !ledred;
+}  
 
-    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);
-    
+void CalibrateEMG1(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
+    ledgreen = !ledgreen;
+    int C = 500;           // half a second at 1000Hz
+    double A0=0, A1=0, A2=0, A3=0, A4=0;
+    double emg1FiAbs;
+    while (C > 0){
+        emg1FiAbs = fabs( emg1bqc.step(emg1.read()));
+        if (C==500){                        //first instance make all values the first in case this is the highest
+            A0=A1=A2=A3=A4=emg1FiAbs;
+        }
+        else if(emg1FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
+            A4=A3;
+            A3=A2;
+            A2=A1;
+            A1=A0;
+            A0=emg1FiAbs;
+        }
+        C--;
+        wait(0.001f);
+    }
+    Calibration1 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
+    ledgreen = !ledgreen;
+}  
+
+void CalibrateEMG2(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
+    ledblue = !ledblue;
+    int C = 500;           // half a second at 1000Hz
+    double A0=0, A1=0, A2=0, A3=0, A4=0;
+    double emg2FiAbs;
+    while (C > 0){
+        emg2FiAbs = fabs( emg2bqc.step(emg2.read()));
+        if (C==500){                        //first instance make all values the first in case this is the highest
+            A0=A1=A2=A3=A4=emg2FiAbs;
+        }
+        else if(emg2FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
+            A4=A3;
+            A3=A2;
+            A2=A1;
+            A1=A0;
+            A0=emg2FiAbs;
+        }
+        C--;
+        wait(0.001f);
+    }
+    Calibration2 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
+    ledblue = !ledblue;
+}    
+
+bool emg0Filter(void){  
+    double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute, 
     /* 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
+    if (emg0filteredAbs > Calibration0) {  // when above threshold  set bool to 1, here can the parameters be changed using global variables
         emg0Bool = 1;
-        emg0Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
+        emg0Ignore = IGNORECOUNT;   // here is the counter reset to catch sudden jolts
         }
     else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
          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;
 }
 
 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
+    if (emg1filteredAbs > Calibration1) {  // 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)
         }
@@ -127,79 +203,71 @@
 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
+    if (emg2filteredAbs > Calibration2) {  // 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;
+        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){
-        directionpin1 = true;
-        pwm_value1 = 0.7f;
-    } 
-    pwm_value1 = 0.0f;
-    while(!button2){
-        directionpin2 = true;
-        pwm_value2 = 0.7f;
-    }
-    pwm_value2 = 0.0f;
-        
-    // pwm_value1 = potmeter1;
-    // pwm_value2 = potmeter2;
+void sample() {  
+    emg0Filter();
+    emg1Filter();
+    emg2Filter();
     
-    if (!next_switch) {
-        CurrentState = EmgCalibration;
-        pc.printf("current state = EmgCalibration\n\r");
-    }
+    scope.set(0, Calibration0);
+    scope.set(1, Calibration1);
+    scope.set(2, Calibration2);
+    scope.set(3, emg0Bool);
+    scope.set(4, emg1Bool);
+    scope.set(5, emg2Bool);
+    scope.send();
 }
 
-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;
+void emgCalibration() {
+    emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 );       // combining biquad chains is done in main, before the ticker, so it happens only once.
+    emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
+    emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 );
+    
+    bool u = true;
+    CalibrationState = EMG0;
+    while (u){                      // !!! this is a place holder for the calibration!!! 
+        switch(CalibrationState) {
+            case EMG0:
+            if(!next_switch) {   
+                CalibrateEMG0(); // calibration function
+                CalibrationState = EMG1;
+            }
+            break;
+            case EMG1:
+            if(!next_switch) {   
+                CalibrateEMG1(); // calibration function
+                CalibrationState = EMG2;
+            }
+            break;
+            case EMG2:
+            if(!next_switch) {      
+                CalibrateEMG2(); // calibration function
+                CurrentState = Movement;
+                sample_timer.attach(&sample, 0.001);    //ticker at 1000Hz
+                ledred = false;     // to indicate filter is working
+                ledgreen = false;
+                ledblue = false;
+                u = false;
+            }
+            break;
         }
-        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() {
     
@@ -212,8 +280,8 @@
 
 int main()
 {
-    pc.baud(115200);
-    pc.printf(" ** program reset **\n\r");
+//    pc.baud(115200);
+//    pc.printf(" ** program reset **\n\r");
     pwmpin1.period_us(60);
     pwmpin2.period_us(60);
     directionpin1 = true;
@@ -222,73 +290,72 @@
     ledred = true;
     ledgreen = true;
     ledblue = true;
-    
-    // emg filters
-    // combining biquad chains is done in main, before the ticker, so only once.
-    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
 
     CurrentState = PositionCalibration;
-    pc.printf("current state = PositionCalibration\n\r");
+//    pc.printf("current state = PositionCalibration\n\r");
     
     while (true) {
         switch(CurrentState) {
             case PositionCalibration:
-                positionCalibration();
-                if (!kill_switch) {
-                    CurrentState = KILL;   
-                    pc.printf("current state = KILL\n\r"); 
-                }
+            ledgreen = false;
+            positionCalibration();
+            if (!kill_switch) {
+                CurrentState = KILL;   
+//                pc.printf("current state = KILL\n\r"); 
+            }
             break;
                 
             case EmgCalibration:
-                emg0Calibration();
-                //emg1Calibration();
-                //emg2Calibration();
-                
-                if (!kill_switch) {
-                    CurrentState = KILL;   
-                    pc.printf("current state = KILL\n\r");  
-                }
+            ledgreen = true;
+            ledblue = false;
+            emgCalibration();
+            //emg1Calibration();
+            //emg2Calibration();
+            
+            if (!kill_switch) {
+                CurrentState = KILL;   
+//                    pc.printf("current state = KILL\n\r");  
+            }
             break;
             
             case Movement:
-                movement();
-                
-                if (!kill_switch) {
-                    CurrentState = KILL; 
-                    pc.printf("current state = KILL\n\r");     
-                }
+            ledred = true;
+            ledgreen = false;
+            ledblue = false;
+            movement();
+            
+            if (!kill_switch) {
+                CurrentState = KILL; 
+//                pc.printf("current state = KILL\n\r");     
+            }
             break;
             
             case KILL:
-                bool u = true;
-                ledgreen = true;
-                ledblue = true;
-                ledred = false;
-                
-                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); 
-                        }
+            bool u = true;
+            ledgreen = true;
+            ledblue = true;
+            ledred = false;
+            
+            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;  
         }
         wait(0.2f);         

--- a/mbed.bld	Wed Oct 31 10:35:27 2018 +0000
+++ b/mbed.bld	Thu Nov 01 14:11:05 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/675da3299148
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file