Diff: emgmeting.cpp

Revision:

4:043af1e01b76

Parent:

3:9b8d3180fe48

--- a/emgmeting.cpp	Thu Oct 31 21:20:42 2019 +0000
+++ b/emgmeting.cpp	Thu Oct 31 21:36:30 2019 +0000
@@ -44,10 +44,10 @@
 //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 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 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;
@@ -82,7 +82,7 @@
 double threshold_emgLB;
 double threshold_emgRB;
 double threshold_emgRT;
-double threshold = 0.25;
+double threshold = 0.15;
 double emgLB_max = 0.000;
 double emgRB_max = 0.000;
 double emgRT_max = 0.000;
@@ -119,7 +119,7 @@
 
 double maxPWM1 = 0.2;
 double maxPWM2 = 0.2;
-    
+
 DigitalOut led5(LED_RED);
 
 DigitalOut motor1_direction(D4);                                                //richting van motor1
@@ -154,7 +154,7 @@
     scope.set(2, emgRBraw);
     scope.set(3, emgRBfiltered);
     scope.set(4, emgRTraw);
-    scope.set(5, emgRTfiltered); 
+    scope.set(5, emgRTfiltered);
 
 
     //rechterbicep
@@ -170,7 +170,7 @@
     emgRTraw= emgRT.read();
     emgRTHP= RightLegHP.step(emgRTraw);
     emgRTN= RightLegN.step(emgRTHP);
-   // emgmeansubRT = emgRTHP - RestmeanRT;
+    // emgmeansubRT = emgRTHP - RestmeanRT;
     emgRTA= fabs(emgRTN);
     emgRTfiltered=RightLegLP.step(emgRTA);
     //RTvalue = emgRTfiltered/emgRT_max;
@@ -180,25 +180,25 @@
 
 void calibration()
 {
-    
-        emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
-        emgLBHP=LeftBicepHP.step(emgLBraw);
-        emgLBN=LeftBicepN.step(emgLBHP);
-        emgLBA= fabs(emgLBN);
-        emgLBfiltered=LeftBicepLP.step(emgLBA);
+
+    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);
-    
+    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);
+
+    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();
@@ -242,52 +242,69 @@
         }
         //pc.printf("emgRT_max = %f \r\n", emgRT_max);
         led5=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
+            threshold_emgLB = threshold*emgLB_max;   //bepaal threshold, nu op 0.15*maximale waarde.
+    threshold_emgRB = threshold*emgRB_max;
+    threshold_emgRT = threshold*emgRT_max;
     } else if (tijd > 18000) {
-        
-        //pc.printf("Calibration finished!");
+        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);
+        }
+
+    } else if (tijd > 18000) {
+        if (emgRBfiltered > threshold_emgRB) {
+            motor2_pwm.write(maxPWM2);
+            motor2_direction.write(direction1 = !direction1);
+        } //is counterclockwise
+//tandenborstel naar rechts
+
+    } else if (tijd > 18000) {
+        if (emgRTfiltered > threshold_emgRT) {
+            motor1_direction.write(direction1 = !direction1);
+            motor1_pwm.write(maxPWM1);
+        }
+//tandenborstel naar achter
+
+    } else if (tijd > 18000) {
+        if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB) {
+            motor1_direction.write(direction1); //motor 1 gaat cw
+            motor1_pwm.write(maxPWM1);
+        }
+//tandenborstel naar voren
     } 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 (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);
+
+    }else if (emgRBfiltered > threshold_emgRB){
+        motor2_pwm.write(maxPWM2);
+        motor2_direction.write(direction1 = !direction1); //is counterclockwise
+    //tandenborstel naar rechts
+
+    }else if (emgRTfiltered > threshold_emgRT){
+    motor1_direction.write(direction1 = !direction1);
+    motor1_pwm.write(maxPWM1);
+    //tandenborstel naar achter
+
+    }else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
+    motor1_direction.write(direction1); //motor 1 gaat cw
+    motor1_pwm.write(maxPWM1);
+    //tandenborstel naar voren
+    */
 }
 
-  
-
-void brushingmode()
-{
-    
-//boven draait arm 1 aan, dus motor 1 
-//onder draait arm 2 aan, dus motor 2 
-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);
-
-}else if (emgRBfiltered > threshold_emgRB){
-    motor2_pwm.write(maxPWM2);
-    motor2_direction.write(direction1 = !direction1); //is counterclockwise
-//tandenborstel naar rechts
-
-}else if (emgRTfiltered > threshold_emgRT){
-motor1_direction.write(direction1 = !direction1);
-motor1_pwm.write(maxPWM1);
-//tandenborstel naar achter
-
-}else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
-motor1_direction.write(direction1); //motor 1 gaat cw
-motor1_pwm.write(maxPWM1);
-//tandenborstel naar voren
-
-}
-}
 int main()
 {
     // -------------------- Serial Comms --------------------------------
@@ -312,8 +329,8 @@
 
     calibreren.attach(calibration, 0.001f);
     filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
-    actie.attach(brushingmode, 0.001f); 
-    
+   // actie.attach(brushingmode, 0.001f);
+
 }