Diff: emgmeting.cpp

Revision:

3:9b8d3180fe48

Parent:

2:8ed9953fc4ab

Child:

4:043af1e01b76

Child:

5:a53081a119c0

--- a/emgmeting.cpp	Thu Oct 31 21:10:09 2019 +0000
+++ b/emgmeting.cpp	Thu Oct 31 21:20:42 2019 +0000
@@ -256,39 +256,38 @@
 
 
 // if threshold_emgx is reached, brushingmodes activated
+}
 
+  
+
+void brushingmode()
+{
     
 //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
+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
+motor2_pwm.write(maxPWM2);
 
-else if (tijd > 18000){
-if (emgRBfiltered > threshold_emgRB){
+}else if (emgRBfiltered > threshold_emgRB){
     motor2_pwm.write(maxPWM2);
-    motor2_direction.write(direction1 = !direction1);}
+    motor2_direction.write(direction1 = !direction1); //is counterclockwise
 //tandenborstel naar rechts
 
-}else if (tijd > 18000){
-if (emgRTfiltered > threshold_emgRT){
-   motor1_direction.write(direction1); 
-motor1_pwm.write(maxPWM1);}
-// SetMotor1 = maxPWM1;
+}else 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_direction.write(direction1 = !direction1);
-motor1_pwm.write(maxPWM1);}
+}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 --------------------------------
@@ -313,7 +312,7 @@
 
     calibreren.attach(calibration, 0.001f);
     filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
-    //actie.attach(brushingmode, 0.001f); 
+    actie.attach(brushingmode, 0.001f); 
     
 }