File content as of revision 16:696e9cbcc823:

#include "mbed.h"
#include "MODSERIAL.h"
#include "FastPWM.h"
#include "QEI.h"
#define PI 3.14159265

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
AnalogIn EMG1(A2);
AnalogIn EMG2(A3);

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 max;  //params of the emg, tbd during calibration
} EMG_values;

int enc1_zero;  //the zero position of the encoders, to be determined from the
int enc2_zero;  //encoder calibration
int EMG1_filtered;
int EMG2_filtered;
int enc1_value;
int enc2_value;

//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;
volatile bool failure_occurred = false;
float pot_1;                //used to keep track of the potentiometer values
float pot_2;
bool enc_has_been_calibrated;
bool EMG_has_been_calibrated;

void do_nothing()

/*
    Idle state. Used in the beginning, before the calibration states.
*/
{
    if (button1_pressed) {
        state_changed = true;
        state = cali_enc;
        button1_pressed = false;
    }
}

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()
/*
    Calibration 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;
    }
    if (button1_pressed) {
        enc1_zero = enc1_value;
        enc2_zero = enc2_value;
        enc_has_been_calibrated = true;
        button1_pressed = false;
        state = moving_magnet_off;
        state_changed = true;

    }
}

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

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

void measure_signals()
{
    enc1_value = enc1.getPulses();
    enc2_value = enc2.getPulses();
    if (enc_has_been_calibrated) {
        enc1_value -= enc1_zero;
        enc2_value -= enc2_zero;
    }
    EMG1_raw = EMG1.read();
    EMG2_raw = EMG2.read();
}

void output()
{
    motor1_direction = actuator.dir1;
    motor2_direction = acuator.dir2;
    motor1_pwm.write(actuator.pwm1);
    motor2_pwm.write(actuator.pwm2);
}

void state_machine()
{
    check_failure(); //check for an error in the last loop before 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 check_failure()
{
    state = failure;
    state_changed = true;
}

void but1_interrupt()
{
    if(button2.read()) {//both buttons are pressed
        failure_occurred = true;
    }
    but1_pressed = true;
    pc.printf("Button 1 pressed \n\r");
}

void but2_interrupt()
{
    if(button1.read()) {//both buttons are pressed
        failure_occurred = true;
    }
    but2_pressed = true;
    pc.printf("Button 2 pressed \n\r");
}
int schmitt_trigger(float i) 
{
    speed = -1; //default value, this means the state should not change
    float levels = 5.0;
    if (i >  0/14 && i <  2/14) {speed = 0;}
    if (i >  3/14 && i <  5/14) {speed = 1;}
    if (i >  6/14 && i <  8/14} (speed = 2;}
    if (i >  9/14 && i < 11/14) {speed = 3;}
    if (i > 12/14 && i < 14/14) {speed = 4;}    
    return speed;
}

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) {
        wait(0.1f);
    }
}]