Diff: main.cpp

Revision:

15:ad065ab92d11

Parent:

14:54343b9fd708

Child:

16:733a71a177e8

Child:

19:1fd39a2afc30

--- a/main.cpp	Tue Oct 15 12:43:25 2019 +0000
+++ b/main.cpp	Tue Oct 15 13:17:49 2019 +0000
@@ -12,43 +12,18 @@
 
 PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet 
 PwmOut motor2(D5);  // samen kunnen gaan met de servo motor
+
+DigitalOut motor1_dir(D7);
+DigitalOut motor2_dir(D4);
   
 DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
 DigitalIn Emergency_button_pressed(D2);
 
+AnalogIn EMG_biceps_right_raw (A0);
+AnalogIn EMG_biceps_left_raw (A1);
+Analogin EMG_calf_raw (A2);
+
 Ticker loop_ticker;
-    
-// Motoren aan-/uitzetten
-void motors_on_off()
-    {
-          if (motor1 == 0)
-          {
-            if (motor2 == 0)
-            {
-                motor2 = 0.1;
-                motor1 = 0.9;
-            }
-            else
-            {
-                motor2 = 0;
-                motor1 = 0.9;
-            }
-          }
-          else 
-          {
-            if (motor2 == 0)
-            {
-                motor2 = 0.1;
-                motor1 = 0;
-            }
-            else
-            {
-                motor2 = 0;
-                motor1 = 0;
-            }
-          }
-          pc.printf("Motoren aan/uit functie\r\n");
-    }    
 
 // Emergency    
 void emergency()
@@ -71,6 +46,14 @@
     }  
     
 // Motoren aanzetten
+void motors_on()
+    {
+        motor1.write(0.9);
+        motor1_dir.write(1);
+        motor2.write(0.1);
+        motor1_dir.write(1);
+        pc.printf("Motoren aan functie\r\n");
+    }
 
 // Finite state machine programming (calibration servo motor?)
 enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
@@ -92,7 +75,7 @@
             }          
             if  (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
             {      
-                motors_on_off();
+                motors_on();
                 currentState = Calib_motor;
                 stateChanged = true;
                 pc.printf("Moving to Calib_motor state\r\n");
@@ -118,7 +101,7 @@
             } 
             break;
             
-         case Calib_EMG: // motoren uit!
+         case Calib_EMG:
             
             motors_off();
             if (stateChanged)
@@ -136,7 +119,7 @@
             
          case Homing:
             
-            motors_on_off();
+            motors_on();
             if (stateChanged)
             {   
                 // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
@@ -162,7 +145,7 @@
                 
                 if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
                 {
-                    motors_on_off();
+                    motors_off();
                     currentState = Motors_off;
                     stateChanged = true;
                     pc.printf("Terug naar de state Motors_off\r\n");