File content as of revision 5:aa8b5d5e632f:

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

MODSERIAL 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)
Ticker loop_ticker;                 //used in main()
InterruptIn button(D10);            //knop op birorobotics shield
QEI encoder (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder gebruiken

//variables
enum States {idle, cw, ccw, end, failure};
States current_state;
class motor_state {
    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
};
motor_state motor;

bool state_changed = false;
volatile bool button_pressed = false;

void measure_signals() {return;}

void do_nothing() {
    if (button_pressed) {
        current_state = cw;
        state_changed = true;
        pc.printf("Changed state from idle to cw\r\n");
        button_pressed = false;
    }
}
void rotate_cw() {
    if (state_changed) {
        pc.printf("State changed to CW\r\n");
        state_changed = false;
    }
    motor.dir2 = 1;
    motor.pwm2 = 0.5f;
    if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
        current_state = ccw;
        state_changed = true;
        button_pressed = false;
    }
}

void rotate_ccw() {
    if (state_changed) {
        pc.printf("State changed to CCW\r\n");
        state_changed = false;
    }
    motor.dir2 = 0;
    motor.pwm2 = 0.5f;
    if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
        current_state = cw;
        state_changed = true;
        button_pressed = false;
    }
}
        
void motor_controller() {return;}
void output() {return;}

void button_interrupt () {
    button_pressed = true;
}

void state_machine() {
    
    //run current state
    switch (current_state) {
        case idle:
            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.pwm1 = 0;
    motor.pwm2 = 0;
    motor.dir1 = 0;
    motor.dir2 = 0;
    button.fall(&button_interrupt);
    loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
    
    while (true) {}
    //while (true) {
    //    enc_count = encoder.getPulses(); //kijkt op welke stand hij nu staat
    //    pc.printf("%f\r\n", float(enc_count-prev_count)*360/(8400*0.025)); //verschil met vorige stap in graden per stap gedeeld door tijd = snelheid
    //    prev_count = enc_count;
    //    wait_ms(25);
    //}
}