File content as of revision 0:7b9ca8e5811b:

#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);
    }
}