File content as of revision 14:4cf17b10e504:

#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

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;

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
    }

}

<<<<<<< working copy
void kinematics1()                                                  //
{
    double length_1;
    volatile double theta;
    volatile double length_2;

    class H_matrix                                                  //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
    {
    public:
        int h1_1_1 = 1;
        int h1_1_2 = 0;
        float h1_1_3 = sin(theta*PI/180)*(length_1+length_2);          //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
        int h1_2_1 = 0;
        int h1_2_2 = 1;
        float h1_2_3 = cos(theta*PI/180)*(length_1+length_2);
        int h1_3_1 = 0;
        int h1_3_2 = 0;
        int h1_3_3 = 1;
    }
    H_matrix H;

    class Position_vector1                                          //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm)
    {
    public:
        float p1_1_1
        float p1_2_1
        float p1_3_1
    }
    class Position_vector2
    {
    public:
        float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1
                       float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1
                                      float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_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

    >>>>>>> merge rev

    <<<<<<< working copy

    void kinematics2() {                                                //
        double length_1;
        volatile double theta;
        volatile double length_2;

        class H_matrix2                                                     //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
        {
        public:
            int h2_1_1 = 1;
            int h2_1_2 = 0;
            float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2);         //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
            int h2_2_1 = 0;
            int h2_2_2 = 1;
            float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2);
            int h2_3_1 = 0;
            int h2_3_2 = 0;
            int h2_3_3 = 1;
        }
        H_matrix H;

        class Position_vector3                                          //positie vector gezien vanuit het onderste draaipunt
        {
        public:
            float p3_1_1
            float p3_2_1
            float p3_3_1
        }
        class Position_vector4
        {
        public:
            float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1
                           float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1
                                          float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1
        }
        H_matrix2 H2
    }

    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

        motor.pwm1 = x_input;       //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
        motor.pwm2 = y_input;       // ook nog niet echt de y dus
        //state guard
        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
        }

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

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