Diff: main.cpp

Revision:

0:7b9ca8e5811b

Child:

1:aa0d93df1af1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 29 09:43:31 2015 +0000
@@ -0,0 +1,145 @@
+#include "mbed.h"
+
+Serial pc(USBTX, USBRX);
+
+
+// Define pin for motor control
+DigitalOut directionPin(D4);
+PwmOut PWM(D5);
+DigitalOut directionPin_key(D7);
+PwmOut PWM_key(D6);
+
+DigitalIn buttonccw(PTA4);
+DigitalIn buttoncw(PTC6);
+
+AnalogIn left_pot(A0);
+AnalogIn right_pot(A1);
+double emg_left;
+double emg_right;
+
+const int Button_move_pressed = 0;
+const int Button_back_pressed = 0;
+
+const int cw = 1;
+const int ccw = 0;
+
+//right button
+DigitalIn button_left(PTC4);
+DigitalIn button_right(PTC6);
+
+int main()
+{
+    pc.printf("at the begin");
+    //directionPin_key.write(cw);
+    //PWM_key.write(0);
+
+    //aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
+    //HIDScope_timer.attach(&Go_flag_HIDScope, 0.02);//0.02 had worked
+    //Filteren_timer.attach(&Go_flag_filteren,0.04);// 0.04 had worked
+    while(1) {
+
+        /*
+        if(Flag_filteren) {
+            Flag_filteren = false;
+            // filter left and set bool
+            filter_signal_hid_left = Filteren_left(analog_emg_left.read());
+
+            if (filter_signal_hid_left > high_threshold) {
+                left_movement = true;
+            } else if (filter_signal_hid_left < low_threshold) {
+                left_movement = false;
+            }
+            // make filter right and set bool
+            filter_signal_hid_right = Filteren_right(analog_emg_right.read());
+
+            if (filter_signal_hid_right > high_threshold) {
+                right_movement = true;
+            } else if (filter_signal_hid_right < low_threshold) {
+                right_movement = false;
+            }
+
+
+        }
+
+
+
+        if(Flag_HIDScope) {
+            Flag_HIDScope = false;
+            HIDScope_kijken();
+
+        }
+        
+
+        
+        if(flag_motor) {
+            flag_motor = false;
+            motor1_Controller();
+
+        }
+        
+       if( left_movement and right_movement == false) {
+            setpoint = making_setpoint(cw);
+
+
+        }
+       else if(right_movement and left_movement == false ) {
+            setpoint =  making_setpoint(ccw);
+        }
+        
+       else if(right_movement and left_movement) {
+            
+            PWM_key.write(1);
+            //pc.printf("I am working");
+        } 
+        else {
+            PWM_key.write(0);
+            // pc.printf("resting /n");
+        }
+        */
+        
+        emg_left = left_pot.read();
+        emg_right = right_pot.read();
+        
+        while (emg_left > 0.5 and emg_right < 0.5)
+        {
+            directionPin.write(cw);
+            PWM.write(1);
+            pc.printf("left \n");
+            emg_left = left_pot.read();
+        }
+        
+        while (emg_right > 0.5 and emg_left < 0.5)
+        {
+            directionPin.write(ccw);
+            PWM.write(1);
+            pc.printf("right \n");
+            emg_right = right_pot.read();
+        }
+        if (button_right.read() == Button_move_pressed)
+        {
+            pc.printf("button move \n");
+            //move to key
+            directionPin_key.write(cw);
+            PWM_key.write(0.3);
+            wait(0.8);
+            PWM.write(0);
+            wait(0.3);
+            directionPin_key.write(ccw);
+            PWM_key.write(0.3);
+            wait(0.8);
+            
+        }
+        /*
+        while (button_left.read() == Button_back_pressed)
+        {
+            //move back to start
+            pc.printf("button move backwards \n");
+            directionPin_key.write(ccw);
+            PWM_key.write(0.3);
+        }
+        */
+        PWM_key.write(0);
+        PWM.write(0);
+    }
+}
+