File content as of revision 3:581c5918b590:

// HET DEFINIEREN VAN ALLES ==========================================================================================

// Includen van alle libraries ---------------------------------------
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#include "HIDScope.h"

// Definieren van de HIDscope ----------------------------------------
HIDScope scope(1)               


// Definieren van de Serial en Encoder -------------------------------
MODSERIAL pc(USBTX, USBRX);
#define SERIALBAUD 115200

QEI encoder_motor1(D10,D11,NC,64);
QEI encoder_motor2(D12,D13,NC,64);


// Definieren van de Motorpins ---------------------------------------
DigitalOut motor1_direction_pin(D7);
PwmOut motor1_speed_pin(D6);

DigitalOut motor2_direction_pin(D4);
PwmOut motor2_speed_pin(D5);


//Definieren van bord-elementen --------------------------------------
InterruptIn kill_switch(D8);
InterruptIn test_button(D9);
AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2



// HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================
volatile bool safe = true;                      // Conditie voor de while-loop in main

int position_motor1;
int counts_motor1;

const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
const double rad_per_count = pi/64.0;       // Aantal rad per puls uit encoder

const double meter_per_count = 1;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!

double error_prev = 0.00000;            // Initiele error derivative waardes
double error_prev = 0.00000;

double error1_int = 0.d;                 // Initiele error integral waardes
double error2_int = 0.d;

const double T_getpos = 0.01;           // Periode van de frequentie van het aanroepen van de positiechecker (get_position)





// ALLE FUNCTIES BUITEN DE MAIN-LOOP ====================================================================================

// De emergency break ------------------------------------------------
void emergency_stop(){
    safe = false;
    pc.printf("Emergency stop!!! Please reset your K64F board\r\n");
}


// Functie voor het vinden van de positie van motor 1 ---------------------
double get_position_m1(const double radpercount){          //returned de positie van de motor in radialen (positie vd arm in radialen)
    counts1 = encoder_motor1.getPulses();           // Leest aantal pulses uit encoder af
    return radpercount * counts1;      // rekent positie motor1 uit en geeft deze als output vd functie
}


// Functie voor vinden van de positie van motor 2 -----------------------------
double get_position_m2(const double meterpercount){     // returned de positie van het karretje in meter
    counts2 = encoder_motor2.getPulses();               // leest aantal pulses vd encoder af
    return meterpercount * counts2;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
}


// Functie voor het vinden van de reference position van motor 1 en 2 --------------------
double get_reference_position_m1(double aantal_rad, double aantal_meter){
    ref_pos = pot1.read - pot2.read;     // De reference position als output (zit tussen -1 en 1, -1 is maximaal links en 1 is maximaal rechts)
    return ref_pos * aantal_rad;            // Aantal rad is hoeveelheid radialen van middelpunt tot minima
    return ref_pos * aantal_meter;           // Aantal meter is de helft van de lengte die het karretje kan bewegen
}
   

// HIDScope functie ----------------------------------------------------------------------
void set_scope(double input){
    scope.set(0, input); 
    scope.send();
}


// De PID-controller voor de motors -------------------------------------------------
double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int){
    double error = ref_pos - mot_pos;       // error uitrekenen
    double error_dif = (error-error_prev)/T_getpos      // De error differentieren
    //error_dif = .....       // Filter biquad poep
    error_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
    error_int = error_int + T_getpos*error;             // De error integreren
    return Kp*error + Ki*error_int + Kd*error_dif       // De uiteindelijke PID output
}
       

// Motor 1 besturen ---------------------------------------------------------------------
void set_motor1(double motor_input){        // functie die de motor aanstuurt mt als input de PID-output
    if (motor_input >= 0){                  // Dit checkt welke kant de motor op moet draaien
        motor1_direction_pin = 1;           // Clockwise
    }
        else {
            motor1_direction_pin = 0;       // CounterClockwise
        }
    if (fabs(motor_input)>1){               // Dit fixed de PwmOutput op maximaal 1
        motor_input = 1;
    }
    motor1_speed_pin = fabs(motor_input);   // Dit geeft de uiteindelijke input door aan de motor
}
 
 
// Motor 2 besturen ---------------------------------------------------------------------
void set_motor2(double motor_input){        // functie die de motor aanstuurt mt als input de PID-output
    if (motor_input >= 0){                  // Dit checkt welke kant de motor op moet draaien
        motor2_direction_pin = 1;           // Clockwise
    }
        else {
            motor2_direction_pin = 0;       // CounterClockwise
        }
    if (fabs(motor_input)>1){               // Dit fixed de PwmOutput op maximaal 1
        motor_input = 1;
    }
    motor2_speed_pin = fabs(motor_input);   // Dit geeft de uiteindelijke input door aan de motor
}



// DE MAIN =================================================================================================================
int main()
{
    pc.baud(SERIALBAUD);
    pc.printf("Starting");
    
    test_button.fall(&PID_controller);      // Activeert test button
    kill_switch.fall(&emergency_stop);      // Activeert kill switch
    
    while(safe){            // Draait loop zolang alles veilig is.
    }
    
    motor1_speed_pin = 0;   //Dit zet de motoren uit nadat de kill switch is gebruikt
    motor2_speed_pin = 0;
}