File content as of revision 12:88cbc65f2563:

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

Serial pc(USBTX, USBRX);            //connect to pc
DigitalOut motor1_direction(D4);    //rotation motor 1 on shield (always D6)
FastPWM motor1_pwm(D5);             //pwm 1 on shield (always D7)
DigitalOut motor2_direction(D7);    //rotation motor 2 on shield (always D4)
FastPWM motor2_pwm(D6);             //pwm 2 on shield (always D5)
Ticker loop_ticker;                 //used in main()
AnalogIn Pot1(A1);                  //pot 1 on biorobotics shield
AnalogIn Pot2(A0);                  //pot 2 on biorobotics shield
InterruptIn but1(D10);              //debounced button on biorobotics shield
InterruptIn but2(D9);               //debounced button on biorobotics 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, cali_EMG, cali_enc, moving_magnet_off, moving_magnet_on, homing, failure};
States state;                       //using the States enum
struct actuator_state
{
    float duty_cycle1;              //pwm of 1st motor
    float duty_cycle2;              //pwm of 2nd motor
    int dir1;                       //direction of 1st motor
    int dir2;                       //direction of 2nd motor
    bool magnet;                    //state of the magnet
} actuators;

struct EMG_params
{
    float idk;  //params of the emg, tbd during calibration
} emg_values;

//initializing enums and classes


int enc_zero;   //the zero position of the encoders, to be determined from the 
                //encoder calibration

//variables used throughout the program
bool state_changed = false; //used to see if the state is "starting"
volatile bool but1_pressed = false;
volatile bool but2_pressed = false;
float pot_1;                //used to keep track of the potentiometer values
float pot_2;

void do_nothing()

/*
    Idle state. Used in the beginning, before the calibration states.
*/
{}

void failure()
/*
    Failure mode. This should execute when button 2 is pressed during operation.
*/
{
    if (state_changed) {
        pc.printf("Something went wrong!\r\n");
        state_changed = false;
    }
}

void cali_EMG()
/*
    Calibratioin of the EMG. Values determined during calibration should be
    added to the EMG_params instance.
*/
{
    if (state_changed) {
        pc.printf("Started EMG calibration\r\n");
        state_changed = false;
    }
}
void cali_enc()
/*
    Calibration of the encoder. The encoder should be moved to the lowest 
    position for the linear stage and the most upright postition for the 
    rotating stage.
*/
{
    if (state_changed) {
        pc.printf("Started encoder calibration\r\n");
        state_changed = false;
    }
}
void moving_magnet_off()
/*
    Moving with the magnet disabled. This is the part from the home position 
    towards the storage of chips.
*/
{
    if (state_changed) {
        pc.printf("Moving without magnet\r\n");
        state_changed = false;
    }
    return;
}
void moving_magnet_on()
/*
    Moving with the magnet enabled. This is the part of the movement from the 
    chip holder to the top of the playing board.
*/
{
    if (state_changed) {
        pc.printf("Moving with magnet\r\n");
        state_changed = false;
    }
    return;
}
void homing()
/*
    Dropping the chip and moving towards the rest position. 
*/
{
    if (state_changed) {
        pc.printf("Started homing");
        state_changed = false;
    }
    return;
}

// the funtions needed to run the program
void measure_signals()
{
    return;
}

void do_nothing()
{
    actuator.duty_cycle1 = 0;
    actuator.duty_cycle2 = 0;


    //state guard
    if (but1_pressed) { //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 motor_controller()
{

    motor1_direction = actuator.dir1;            // Zet de richting goed
    motor1_pwm.write(actuator.duty_cycle1);             // Zet het op de snelheid van actuator.speed1


    motor2_direction = actuator.dir2;
    motor2_pwm.write(actuator.duty_cycle2);
    return;
}

void output()
{
    return;
}

void state_machine()
{
    //run current state
    switch (current_state) {
        case idle:
            do_nothing();
            break;
        case failure:
            failure();
            break;
        case cali_EMG:
            cali_EMG();
            break;
        case cali_ENC:
            cali_encoder();
            break;
        case moving_magnet_on:
            moving_magnet_on();
            break;
        case moving_magnet_off:
            moving_magnet_off();
            break;
        case homing:
            homing();
            break;
    }
}

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

//Helper functions, not directly called by the main_loop functions or 
//state machines
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");
}

int main()
{
    pc.baud(115200);
    pc.printf("Executing main()... \r\n");
    current_state = idle;
    
    motor2_pwm.period(1/160000f);   // 1/frequency van waarop hij draait
    motor1_pwm.period(1/160000f);   // 1/frequency van waarop hij draait

    actuator.dir1 = 0;
    actuator.dir2 = 0;
    
    actuator.magnet = false;
    
    but1.fall(&but1_interrupt);
    but2.fall(&but2_interrupt);
    loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
    pc.printf("Main_loop is running\n\r");
    while (true) {}
}