File content as of revision 18:a584af8b4cfb:

#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
*/ 

AnalogIn vrx(A0);                   //Joystick_x
AnalogIn vry(A1);                   //Joystick_y

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

float Joystick_x = 0.0;
float Joystick_y = 0.0;

//variables
enum States {idle, calib_EMG, calib_enc, moving_magnet_offf, moving_magnet_onn, home, fail};
States current_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;

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 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()
{
    Joystick_x = (vrx.read()*100-50)/50;
    Joystick_y = (vry.read()*100-50)/50;
    pc.printf("X-coordinate = %.2f | Y-coordinate = %.2f \n\r", Joystick_x, Joystick_y);

    return;
}

void do_nothing()
{
    actuators.duty_cycle1 = 0;
    actuators.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 output()
{
    
    if (Joystick_x < 0){
        motor2_pwm.write(fabs(Joystick_x));   // 1/frequency van waarop hij draait
        actuators.dir2 = 0;
        motor2_dir = motor.dir2;
    }
    else {
        motor2_pwm.write(Joystick_x);   // 1/frequency van waarop hij draait
        motor.dir2 = 1;
        motor2_dir = motor.dir2;
    }
    if (Joystick_y < 0){
        motor1_pwm.write(fabs(Joystick_x));   // 1/frequency van waarop hij draait
        motor.dir1 = 0;
        motor1_dir = motor.dir1;
    }
    else {
        motor1_pwm.write(Joystick_x);   // 1/frequency van waarop hij draait
        motor.dir1 = 1;
        motor1_dir = motor.dir1;
    }
    
    
    return;
}

void state_machine()
{
    //run current state
    switch (current_state) {
        case idle:
            do_nothing();
            break;
        case fail:
            failure();
            break;
        case calib_EMG:
            cali_EMG();
            break;
        case calib_ENC:
            cali_encoder();
            break;
        case moving_magnet_onn:
            moving_magnet_on();
            break;
        case moving_magnet_offf:
            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

    actuators.dir1 = 0;
    actuators.dir2 = 0;

    actuators.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) {}
}