Controls the rovot with pot meters and button, fall back for if the filters fail
Dependencies: mbed
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); + } +} +