File content as of revision 8:6f6a4dc12036:

#include "mbed.h"
#include "MODSERIAL.h"
#include "FastPWM.h"
#include "QEI.h"

Serial pc(USBTX, USBRX);         //verbinden met pc
DigitalOut motor2_direction(D4);    //verbinden met motor 2 op board (altijd d4)
FastPWM motor2_pwm(D5);             //verbinden met motor 2 pwm (altijd d5)
DigitalOut motor1_direction(D6);    //verbinden met motor 2 op board (altijd d4)
FastPWM motor1_pwm(D7);             //verbinden met motor 2 pwm (altijd d5)
Ticker loop_ticker;                 //used in main()
AnalogIn Pot1(A1);                  //pot 1 op biorobotics shield
AnalogIn Pot2(A0);                  //pot 2 op biorobotics shield
InterruptIn but1(D10);            //knop 1 op birorobotics shield
InterruptIn but2(D9);            //knop 1 op birorobotics shield
QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken

//variables
enum States {idle, cw, ccw, end, failure};
States current_state;               //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure

class motor_state {                 // Een emmer met variabelen die je hierna motor gaat noemen.
    public:                         //Wat is public?
    float pwm1; //pwm of 1st motor
    float pwm2; //pwm of 2nd motor
    int dir1; //direction of 1st motor
    int dir2; //direction of 2nd motor
    float speed1;   // speed motor 1
    double speed2;   // speed motor 2
};
motor_state motor;              //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1

bool state_changed = false;
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
float pot_1 = 0;
float pot_2 = 0;

// the funtions needed to run the program
void measure_signals() {
    pot_1 = Pot1.read();
    pc.printf("pot_1 = %f",pot_1);  // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
    motor.speed1 = pot_1*0.75;      //pod_read * 0.75 (dus max 75%)
    
    pot_2 = Pot2.read();
    pc.printf("pot_2 = %f",pot_2);  // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
    motor.speed2 = pot_2*0.75;      //pod_read * 0.75 (dus max 75%)
    return;
    }

void do_nothing() {
    motor.speed1 = 0;
    motor.speed2 = 0;
    
    if (but1_pressed == true) { //this moves the program from the idle to cw state
        current_state = cw;
        state_changed = true; //to show next state it can initialize
        pc.printf("Changed state from idle to cw\r\n");
        but1_pressed = false; //reset button 1
    }
    
}


void rotate_cw() {
    if (state_changed) {
        pc.printf("State changed to CW\r\n");
        state_changed = false; //reset this so it wont print next loop
    }
    motor.dir1 = 1;             //todo: check if this is actually clockwise
    motor.dir2 = 1;             //todo: check if this is actually clockwise
    
    if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
        current_state = ccw;
        state_changed = true;
        but1_pressed = false; //reset this
    }
    
}
 
void rotate_ccw() {
    //similar to rotate_cw()
    if (state_changed) {
        pc.printf("State changed to CCW\r\n");
        state_changed = false;
    }
    motor.dir1 = 0;
    motor.dir2 = 0;

    if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
        current_state = cw;
        state_changed = true;
        but1_pressed = false;
    }
}
        
void motor_controller() {
    motor1_pwm.period(0.0000625‬f);              // 1/frequency van waarop hij draait
    motor1_direction = motor.dir1;              // Zet de richting goed
    motor1_pwm.write(motor.speed1);             // Zet het op de snelheid van motor.speed1
    
    motor2_pwm.period(0.0000625f);              // 1/frequency van waarop hij draait
    motor2_direction = motor.dir2;
    motor2_pwm.write(motor.speed2);             
    return;
    }
    
void output() {return;}

void but1_interrupt () {
    but1_pressed = true;
    pc.printf("Button 1 pressed \n\r");
}

void but2_interrupt () {
    but2_pressed = true;
    pc.printf("Button 2 pressed \n\r");
}

void state_machine() {
    
    if (but2_pressed){              // Is dit de goede locatie hiervoor?
        current_state = idle;
        but2_pressed = false;
        pc.printf("Do_nothing happened due to pressing button 2 \n\r");   
    }
    
    //run current state
    switch (current_state) {
        case idle:                  // hoezo de :?
            do_nothing();
            break;
        case cw:
            rotate_cw();
            break;
        case ccw:
            rotate_ccw();
            break;
        case end:
            break;
        case failure:
            break;
    }        
}

    
void main_loop() {
    measure_signals();
    state_machine();
    motor_controller();
    output();
}

int main() {
    pc.baud(115200);
    pc.printf("Executing main()... \r\n");
    current_state = idle;
    
    motor.dir1 = 0;
    motor.dir2 = 0;
    
    but1.fall(&but1_interrupt);
    but2.fall(&but2_interrupt);
    loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz
    pc.printf("Main_loop is running\n \r");
    while (true) {}
}