Diff: main.cpp

Revision:

10:2325e545ce11

Parent:

9:40c9a18c3430

Child:

11:4ee7f6d482f4

--- a/main.cpp	Thu Nov 01 17:09:18 2018 +0000
+++ b/main.cpp	Thu Nov 01 17:29:01 2018 +0000
@@ -48,10 +48,8 @@
 
 
 // PID  CONTROLLER     --        PIN DEFENITIONS 
-//AnalogIn button2(A4);
-//AnalogIn button1(A3);
-//DigitalIn button3(SW2);
-//DigitalIn button4(SW3);
+AnalogIn button3(A4);
+AnalogIn button4(A5);
 
 DigitalOut directionpin1(D7);   // motor 1
 DigitalOut directionpin2(D4);   // motor 2
@@ -70,9 +68,8 @@
 Ticker show_counts;
 Ticker Scope_Data;
 
-//----------------GLOBALS--------------------------
+//------------------------GLOBALS-----------------------------
 // GLOBALS EMG
-
 //Filtered EMG signals from the end of the chains
 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
 int i = 0;
@@ -156,7 +153,6 @@
 double motor_position;
 bool AlwaysTrue;
 
-
 //----------------FUNCTIONS--------------------------
 
 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
@@ -671,8 +667,6 @@
                     led2 = 1;
                     led3 = 1;
                 }
-                currentState = MOVEMENT  ;
-                stateChanged = false;
             }
 
             break;
@@ -726,10 +720,43 @@
     led3 = 1;
     
     pwmpin1.period_us(60); // setup motor
-        ref_rot.attach(Motor_mover, 0.001);// HAS TO GO TO STATE MACHINE
+    ref_rot.attach(Motor_mover, 0.001);// HAS TO GO TO STATE MACHINE
 
     while (true) {
-        ProcessStateMachine();
+        //ProcessStateMachine();
+        
+        if (button2 == false) {
+            wait(0.01f);
+
+            // berekenen positie
+            float px = positionx(1,0);  // EMG: +x, -x
+            float py = positiony(0,0);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+        }
+
+        if (button1 == false) {
+            wait(0.01f);
+            // berekenen positie
+            float px = positionx(0,1);  // EMG: +x, -x
+            float py = positiony(0,0);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+        }
+
+        if (button3 == false) {
+            wait(0.01f);
+            // berekenen positie
+            float px = positionx(0,0);  // EMG: +x, -x
+            float py = positiony(1,0);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+        }
+
+        if (button4 == false) {
+            wait(0.01f);
+            // berekenen positie
+            float px = positionx(0,0);  // EMG: +x, -x
+            float py = positiony(0,1);  // EMG: +y, -y
+            //printf("positie (%f,%f)\n\r",px,py);
+        }
 
     }