Diff: emgmeting.cpp

Revision:

2:8ed9953fc4ab

Child:

3:9b8d3180fe48

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/emgmeting.cpp	Thu Oct 31 21:10:09 2019 +0000
@@ -0,0 +1,321 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#include "math.h"
+#include <stdio.h>      /* printf */
+#include <stdlib.h>     /* abs */
+#include <complex>
+#include "FastPWM.h"
+
+MODSERIAL pc(USBTX, USBRX);
+
+//Filters EMG
+//Filters Linker Biceps
+//In de volgorde High pass, notch, low pass
+//BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
+//BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+BiQuad LBHP1(0.995566972017647,  -1.991133944035294,   0.995566972017647, 1.000000000000000,  -1.991114292201654,   0.991153595868935);
+//BiQuad LBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
+BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
+BiQuad LBLP1(0000.087655548754006,   0000.175311097508013,   0000.087655548754006,    1.000000000000000,  -1.973344249781299,   0.973694871976315);
+//BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain LeftBicepHP;
+BiQuadChain LeftBicepN;
+BiQuadChain LeftBicepLP;
+
+//Filters Rechter Biceps
+//In de volgorde High pass, notch, low pass
+//BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
+//BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+BiQuad RBHP1(0.995566972017647,  -1.991133944035294,   0.995566972017647, 1.000000000000000,  -1.991114292201654,   0.991153595868935);
+//BiQuad RBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
+BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
+BiQuad RBLP1(0000.087655548754006,   0000.175311097508013,   0000.087655548754006,  1.000000000000000,  -1.973344249781299,   0.973694871976315);
+//BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain RightBicepHP;
+BiQuadChain RightBicepN;
+BiQuadChain RightBicepLP;
+
+
+//Filters Rechter Quadriceps
+//In de volgorde High pass, notch, low pass
+//BiQuad RTHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
+//BiQuad RTHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+BiQuad RTHP1(0.995566972017647,  -1.991133944035294 ,  0.995566972017647 ,1.000000000000000,  -1.991114292201654 ,  0.991153595868935);
+//BiQuad RTHP2(1, 1.999999966458334, 0.999999966458334, -1.988418014198592, 0.988451549797368);
+BiQuad RTN1( 0.5, 0, 0.5, 0, 0);
+BiQuad RTLP1(0000.087655548754006  , 0.175311097508013 ,  0.087655548754006,  1.000000000000000 , -1.973344249781299  , 0.973694871976315);
+//BiQuad RTLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain RightLegHP;
+BiQuadChain RightLegN;
+BiQuadChain RightLegLP;
+
+double emgLBHP;
+double emgLBN;
+double emgLBA;
+//double emgLBLP;
+//double emgAbsLBNotch;
+
+double emgRBHP;
+double emgRBN;
+double emgRBA;
+double emgRBLP;
+double emgAbsRBNotch;
+
+double emgRTN;
+double emgRTHP;
+double emgRTA;
+//double emgRTLP;
+//double emgAbsRTNotch;
+
+double emgLBfiltered;
+double emgRBfiltered;
+double emgRTfiltered;
+double emgLBraw;
+double emgRBraw;
+double emgRTraw;
+
+
+double threshold_emgLB;
+double threshold_emgRB;
+double threshold_emgRT;
+double threshold = 0.25;
+double emgLB_max = 0.000;
+double emgRB_max = 0.000;
+double emgRT_max = 0.000;
+double emgLB_maxrust = 0.000;
+double emgRB_maxrust = 0.000;
+double emgRT_maxrust = 0.000;
+int tijd = 0;
+double timecalibration;
+
+double emgsumLB;
+double emgsumRB;
+double emgsumRT;
+double restmeanLB = 0.000;
+double restmeanRB = 0.000;
+double restmeanRT = 0.000;
+//double emgmeansubLB;
+//double emgmeansubRB;
+//double emgmeansubRT;
+double LBvalue;
+double RBvalue;
+double RTvalue;
+double emgLBrust;
+double emgRBrust;
+double emgRTrust;
+double RestmeanLB;
+double RestmeanRB;
+double RestmeanRT;
+
+
+
+const bool clockwise = true;
+volatile bool direction1 = clockwise;
+volatile bool direction2 = clockwise;
+
+double maxPWM1 = 0.2;
+double maxPWM2 = 0.2;
+    
+DigitalOut led5(LED_RED);
+
+DigitalOut motor1_direction(D4);                                                //richting van motor1
+FastPWM motor1_pwm(D5);                                                         //Motor 1 PWM controle van de snelheid
+DigitalOut motor2_direction(D7);                                                //richting van motor2
+FastPWM motor2_pwm(D6);                                                         //Motor 2 PWM controle van de snelheid
+
+AnalogIn emgLB(A0);     //read EMG left bicep
+AnalogIn emgRB(A1);     //read EMG right bicep
+AnalogIn emgRT(A2);     //read EMG right quadriceps
+
+HIDScope scope(6);      //aantal kanalen HIDScope (voor test 5, voor echt 6)
+
+Ticker filter;
+Ticker calibreren;
+Ticker actie;
+
+void Filteren()
+{
+    //linkerbicep
+    emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
+    emgLBHP=LeftBicepHP.step(emgLBraw);
+    emgLBN=LeftBicepN.step(emgLBHP);
+    //emgmeansubLB = emgLBN - RestmeanLB;
+    emgLBA= fabs(emgLBN);
+    emgLBfiltered=LeftBicepLP.step(emgLBA);
+    //LBvalue = emgLBfiltered/emgLB_max;
+
+    //to show if filter is working
+    scope.set(0, emgLBraw);
+    scope.set(1, emgLBfiltered);
+    scope.set(2, emgRBraw);
+    scope.set(3, emgRBfiltered);
+    scope.set(4, emgRTraw);
+    scope.set(5, emgRTfiltered); 
+
+
+    //rechterbicep
+    emgRBraw= emgRB.read();
+    emgRBHP= RightBicepHP.step(emgRBraw);
+    emgRBN=RightBicepN.step(emgRBHP);
+    //emgmeansubRB = emgRBN - RestmeanRB;
+    emgRBA= fabs(emgRBN);
+    emgRBfiltered=RightBicepLP.step(emgRBA);
+    //RBvalue = emgRBfiltered/emgRB_max;
+
+    //Right Quadriceps
+    emgRTraw= emgRT.read();
+    emgRTHP= RightLegHP.step(emgRTraw);
+    emgRTN= RightLegN.step(emgRTHP);
+   // emgmeansubRT = emgRTHP - RestmeanRT;
+    emgRTA= fabs(emgRTN);
+    emgRTfiltered=RightLegLP.step(emgRTA);
+    //RTvalue = emgRTfiltered/emgRT_max;
+    scope.send();
+
+}
+
+void calibration()
+{
+    
+        emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
+        emgLBHP=LeftBicepHP.step(emgLBraw);
+        emgLBN=LeftBicepN.step(emgLBHP);
+        emgLBA= fabs(emgLBN);
+        emgLBfiltered=LeftBicepLP.step(emgLBA);
+
+        emgRBraw= emgRB.read();
+        emgRBHP= RightBicepHP.step(emgRBraw);
+        emgRBN=RightBicepN.step(emgRBHP);
+        emgRBA= fabs(emgRBN);
+        emgRBfiltered=RightBicepLP.step(emgRBA);
+    
+
+        emgRTraw= emgRT.read();
+        emgRTHP= RightLegHP.step(emgRTraw);
+        emgRTN= RightLegN.step(emgRTHP);
+        emgRTA= fabs(emgRTN);
+        emgRTfiltered=RightLegLP.step(emgRTA);
+
+    if (tijd > 1000 && tijd < 6000) {
+        emgLBraw= emgLB.read();
+        emgLBHP=LeftBicepHP.step(emgLBraw);
+        emgLBN=LeftBicepN.step(emgLBHP);
+        //emgmeansubLB = emgLBN - restmeanLB;
+        emgLBA= fabs(emgLBN);
+        emgLBfiltered=LeftBicepLP.step(emgLBA);
+        if (emgLBfiltered > emgLB_max) {
+            emgLB_max = emgLBfiltered;
+        }
+        //pc.baud(115200);
+        //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
+        led5=0; //led gaat aan zodra je linkerbicep moet aanspannen
+    }
+
+    else if (tijd > 7000 && tijd < 12000) {
+        emgRBraw= emgRB.read();
+        emgRBHP= RightBicepHP.step(emgRBraw);
+        emgRBN=RightBicepN.step(emgRBHP);
+        //emgmeansubRB = emgRBN - RestmeanRB;
+        emgRBA= fabs(emgRBN);
+        emgRBfiltered=RightBicepLP.step(emgRBA);
+        if (emgRBfiltered > emgRB_max) {
+            emgRB_max = emgRBfiltered;
+        }
+        //pc.printf("emgRB_max = %f \r\n", emgRB_max);
+        led5=0; //led gaat uit zodra je rechterbicep moet aanspannen
+    }
+
+
+    else if (tijd > 13000 && tijd < 18000) {
+        emgRTraw= emgRT.read();
+        emgRTHP= RightLegHP.step(emgRTraw);
+        emgRTN= RightLegN.step(emgRTHP);
+        //emgmeansubRT = emgRTHP - RestmeanRT;
+        emgRTA= fabs(emgRTN);
+        emgRTfiltered=RightLegLP.step(emgRTA);
+        if (emgRTfiltered > emgRT_max) {
+            emgRT_max = emgRTfiltered;
+        }
+        //pc.printf("emgRT_max = %f \r\n", emgRT_max);
+        led5=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
+    } else if (tijd > 18000) {
+        
+        //pc.printf("Calibration finished!");
+    } else {
+        led5=1; //led gaat uit zodra kalibratie voltooid
+    }
+    tijd= tijd + 1;    //idealiter op een andere plek haha
+    
+threshold_emgLB = threshold*emgLB_max;   //bepaal threshold, nu op 0.15*maximale waarde. 
+threshold_emgRB = threshold*emgRB_max; 
+threshold_emgRT = threshold*emgRT_max;
+
+
+// if threshold_emgx is reached, brushingmodes activated
+
+    
+//boven draait arm 1 aan, dus motor 1 
+//
+//onder draait arm 2 aan, dus motor 2 
+if (tijd > 18000){
+} if (emgLBfiltered > threshold_emgLB){
+//tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
+motor2_direction.write(direction1);   //motor 2 gaat cw
+//motor2_direction.write(direction1 = !direction1); //is counterclockwise
+motor2_pwm.write(maxPWM2);}
+//SetMotor2=maxPWM2;        //1/2 welke motor
+
+else if (tijd > 18000){
+if (emgRBfiltered > threshold_emgRB){
+    motor2_pwm.write(maxPWM2);
+    motor2_direction.write(direction1 = !direction1);}
+//tandenborstel naar rechts
+
+}else if (tijd > 18000){
+if (emgRTfiltered > threshold_emgRT){
+   motor1_direction.write(direction1); 
+motor1_pwm.write(maxPWM1);}
+// SetMotor1 = maxPWM1;
+//tandenborstel naar achter
+
+}else if (tijd > 18000) {
+if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
+ //   motor1_direction.write(direction1); //motor 1 gaat cw
+ motor1_direction.write(direction1 = !direction1);
+motor1_pwm.write(maxPWM1);}
+//tandenborstel naar voren
+}}
+int main()
+{
+    // -------------------- Serial Comms --------------------------------
+    pc.baud(115200);
+    pc.printf("Hello");
+
+    // -------------------- BiQuad Chains -------------------------------
+    LeftBicepHP.add( &LBHP1 );  //BiQuadChain bqc;   //bqc.add( &bq1 ).add( &bq2 );
+    LeftBicepN.add( &LBN1 );
+    LeftBicepLP.add( &LBLP1 );
+
+
+    RightBicepHP.add( &RBHP1 );
+    RightBicepN.add( &RBN1 );
+    RightBicepLP.add( &RBLP1 );
+
+    RightLegHP.add( &RTHP1 );
+    RightLegN.add( &RTN1 );
+    RightLegLP.add( &RTLP1 );
+
+    // -------------------- Tickers -------------------------------------
+
+    calibreren.attach(calibration, 0.001f);
+    filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
+    //actie.attach(brushingmode, 0.001f); 
+    
+}
+
+
+