Diff: main.cpp

Revision:

0:2c9afa8a21ee

diff -r 000000000000 -r 2c9afa8a21ee main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 02 17:42:27 2017 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#include "TextLCD.h"
+
+
+HIDScope scope(5);
+Ticker HidTicker;
+
+AnalogIn bicepsEMG(A0);
+AnalogIn tricepsEMG(A1);
+AnalogIn carpiFlexEMG(A2);
+AnalogIn palmarisEMG(A3);
+
+double tickerF =0.002;
+double bicepsMOVAG [20];
+double tricepsMOVAG [20];
+double carpiFlexMOVAG [20];
+double palmarisMOVAG [20];
+int MOVAGLength=20;
+
+//Notch Filter 50Hz Q of 0.7
+
+double bicepsNotcha0 = 0.7063988100714527;
+double bicepsNotcha1 = -1.1429772843080923;
+double bicepsNotcha2 = 0.7063988100714527;
+double bicepsNotchb1 = -1.1429772843080923;
+double bicepsNotchb2 = 0.41279762014290533;
+
+double tricepsNotcha0 = 0.7063988100714527;
+double tricepsNotcha1 = -1.1429772843080923;
+double tricepsNotcha2 = 0.7063988100714527;
+double tricepsNotchb1 = -1.1429772843080923;
+double tricepsNotchb2 = 0.41279762014290533;
+
+double carpiFlexNotcha0 = 0.7063988100714527;
+double carpiFlexNotcha1 = -1.1429772843080923;
+double carpiFlexNotcha2 = 0.7063988100714527;
+double carpiFlexNotchb1 = -1.1429772843080923;
+double carpiFlexNotchb2 = 0.41279762014290533;
+
+double palmarisNotcha0 = 0.7063988100714527;
+double palmarisNotcha1 = -1.1429772843080923;
+double palmarisNotcha2 = 0.7063988100714527;
+double palmarisNotchb1 = -1.1429772843080923;
+double palmarisNotchb2 = 0.41279762014290533;
+
+//Highpass filters 20Hz cutoff Q of 0.7
+double bicepsHigha0 = 0.8370879899975344;
+double bicepsHigha1 = -1.6741759799950688;
+double bicepsHigha2 = 0.8370879899975344;
+double bicepsHighb1 = -1.6474576182593796;
+double bicepsHighb2 = 0.7008943417307579;
+
+double tricepsHigha0 = 0.8370879899975344;
+double tricepsHigha1 = -1.6741759799950688;
+double tricepsHigha2 = 0.8370879899975344;
+double tricepsHighb1 = -1.6474576182593796;
+double tricepsHighb2 = 0.7008943417307579;
+
+double carpiFlexHigha0 = 0.8370879899975344;
+double carpiFlexHigha1 = -1.6741759799950688;
+double carpiFlexHigha2 = 0.8370879899975344;
+double carpiFlexHighb1 = -1.6474576182593796;
+double carpiFlexHighb2 = 0.7008943417307579;
+
+double palmarisHigha0 = 0.8370879899975344;
+double palmarisHigha1 = -1.6741759799950688;
+double palmarisHigha2 = 0.8370879899975344;
+double palmarisHighb1 = -1.6474576182593796;
+double palmarisHighb2 = 0.7008943417307579;
+
+
+
+BiQuad bicepsNotch (bicepsNotcha0,bicepsNotcha1,bicepsNotcha2,bicepsNotchb1,bicepsNotchb2);
+BiQuad tricepsNotch (tricepsNotcha0,tricepsNotcha1,tricepsNotcha2,tricepsNotchb1,tricepsNotchb2);
+BiQuad carpiFlexNotch (carpiFlexNotcha0,carpiFlexNotcha1,carpiFlexNotcha2,carpiFlexNotchb1,carpiFlexNotchb2);
+BiQuad palmarisNotch (palmarisNotcha0,palmarisNotcha1,palmarisNotcha2, palmarisNotchb1,palmarisNotchb2);
+
+BiQuad bicepsHigh (bicepsHigha0,bicepsHigha1,bicepsHigha2,bicepsHighb1,bicepsHighb2);
+BiQuad tricepsHigh (tricepsHigha0,tricepsHigha1,tricepsHigha2,tricepsHighb1,tricepsHighb2);
+BiQuad carpiFlexHigh (carpiFlexHigha0,carpiFlexHigha1,carpiFlexHigha2,carpiFlexHighb1,carpiFlexHighb2);
+BiQuad palmarisHigh (palmarisHigha0,palmarisHigha1,palmarisHigha2, palmarisHighb1,palmarisHighb2);
+
+//set your pins here: motor1_pwm and motor1_dir, etc.
+//reserve variables for the motor positions (in rad) etc.
+
+
+Ticker main_loop; //ticker that will handle the main behavior
+enum States {calib1, calib2, homing, emgcontrol, demo}; //calib1=0, calib2=1, ..., demo=4
+TextLCD lcd(D8, D9, D4, D5, D6, D7); // rs, e, d4-d7
+int state, counter;
+bool position_controller_on;
+float looptime = 0.001f;
+Timer flex;
+Timer prep;
+
+void filter(){
+  
+        
+    double bicepsSignal = bicepsEMG.read();
+    double bicepsFiltered = bicepsSignal;    
+    double tricepsSignal = tricepsEMG.read();
+    double tricepsFiltered = tricepsSignal;
+    double carpiFlexSignal = carpiFlexEMG.read();
+    double carpiFlexFiltered = carpiFlexSignal;
+    double palmarisSignal = palmarisEMG.read();
+    double palmarisFiltered = palmarisSignal;
+
+    //Filter out 50Hz from all signals
+    bicepsFiltered = bicepsNotch.step(bicepsFiltered);
+    tricepsFiltered = tricepsNotch.step(tricepsFiltered);
+    carpiFlexFiltered = carpiFlexNotch.step(carpiFlexFiltered);
+    palmarisFiltered = palmarisNotch.step(palmarisFiltered);
+    
+    //Highpass filtering
+    bicepsFiltered = bicepsHigh.step(bicepsFiltered);
+    tricepsFiltered = tricepsHigh.step(tricepsFiltered);
+    carpiFlexFiltered = carpiFlexHigh.step(carpiFlexFiltered);
+    palmarisFiltered = palmarisHigh.step(palmarisFiltered);
+    
+    //Rectification 
+    bicepsFiltered = fabs(bicepsFiltered);
+    tricepsFiltered = fabs(tricepsFiltered);
+    carpiFlexFiltered = fabs(carpiFlexFiltered);
+    palmarisFiltered = fabs(palmarisFiltered);
+    
+    //caculate moving average
+    for(int i=0;i<MOVAGLength-1;i++){
+        bicepsMOVAG[i]=bicepsMOVAG[i+1];
+        tricepsMOVAG[i]=tricepsMOVAG[i+1];  
+        carpiFlexMOVAG[i]=carpiFlexMOVAG[i+1];  
+        palmarisMOVAG[i]=palmarisMOVAG[i+1];        
+    }
+                
+    bicepsMOVAG[MOVAGLength-1]=bicepsFiltered;
+    tricepsMOVAG[MOVAGLength-1]=tricepsFiltered;
+    carpiFlexMOVAG[MOVAGLength-1]=carpiFlexFiltered;
+    palmarisMOVAG[MOVAGLength-1]=palmarisFiltered;
+    
+    double bicepsSum;
+    double tricepsSum;
+    double carpiFlexSum;
+    double palmarisSum;    
+    
+    for(int i=0;i<MOVAGLength;i++){
+        bicepsSum+= bicepsMOVAG[i];
+        tricepsSum+= tricepsMOVAG[i];
+        carpiFlexSum+= carpiFlexMOVAG[i];
+        palmarisSum+= palmarisMOVAG[i];       
+    }
+        
+    double bicepsAvg= bicepsSum/MOVAGLength;
+    double tricepsAvg= tricepsSum/MOVAGLength;
+    double carpiFlexAvg= carpiFlexSum/MOVAGLength;
+    double palmarisAvg= palmarisSum/MOVAGLength;
+        
+   
+     scope.set(0,bicepsAvg);
+     scope.set(1,tricepsAvg);
+     scope.set(2,carpiFlexAvg);
+     scope.set(3,palmarisAvg);
+     scope.send();
+    }
+    
+int main()
+{
+    for(int i=0;i<MOVAGLength;i++){
+        bicepsMOVAG[i]=0;     
+        tricepsMOVAG[i]=0;
+        carpiFlexMOVAG[i]=0;
+        palmarisMOVAG[i]=0;
+    }
+    HidTicker.attach(filter,tickerF);
+    while (true) {
+                
+            if(Sensor.read_u16()>12000 && Sensor.read_u16()< 15000){
+               lcd.printf("Prepare /n to flex biceps");
+               prep.start();
+               while(prep.read() < 3){}
+               lcd.cls(); 
+               lcd.printf("Prepare to flex in %f seconds", 3);
+               wait(1);
+               lcd.printf("Prepare to flex in %f seconds", 2);
+               wait(1);
+               lcd.printf("Prepare to flex in %f seconds", 1);
+               wait(1);
+               lcd.printf("Flex!");
+               flex.start();
+               double bicepsMax=0;
+               while(flex.read()<1){
+                   if()
+                   
+                   }
+               lcd.cls(); 
+               lcd.printf(" * Stretch * ");
+               wait(4);
+               lcd.cls();                
+                } else {
+                lcd.printf("Press to start", Sensor.read_u16());
+                wait(1);        
+                lcd.cls();
+            }
+    }
+    
+}
\ No newline at end of file