File content as of revision 15:3f3d87513a6b:

// 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(3);               


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

InterruptIn start_button(SW2);
InterruptIn initializing_stopper(SW3);

DigitalOut ledred(LED_RED, 1);
DigitalOut ledgreen(LED_GREEN, 1);


//Definieren van de tickers ------------------------------------------
Ticker test_ticker;
Ticker hidscope_ticker;



// HET VASTSTELLEN VAN ALLE TE GEBRUIKEN VARIABELEN EN CONSTANTEN ======================================================

enum states_enum{off, init_m1, init_m2, finish_init, run};                   // De states waar de main loop doorheen loopt, off is uit, init is intialiseren en run is het aanturen vd motoren met emg.
states_enum states = off;

bool starting = false;                               // conditie om programma te starten


double error_prev_1_in = 0.0, error_int_1_in = 0.0; // De error waardes voor het initialiseren
double PID_output_1_in = 0.0;                         // De PID output voor het initialiseren
bool initializing = true;                              // conditie voor het initialiseren


volatile bool safe = true;                      // Conditie voor de while-loop in main

double position_motor1;

int counts1;                                // Pulses van motoren
int counts2;

const double pi = 3.14159265358979323846264338327950288419716939937510;     // pi
const double rad_per_count = 2.0*pi/8400.0;       // Aantal rad per puls uit encoder

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

double error1_int = 0.00000;                 // Initiele error integral waardes
double error2_int = 0.00000;

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

volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers

const double Kp_1 = 0.1;                    // De PID variablen voor motor 1
const double Kd_1 = 0.01;
const double Ki_1 = 0.01;

const double Kp_2 = 1.0000000;                    // De PID variablen voor motor 2
const double Kd_2 = 0.1;
const double Ki_2 = 0.3;

const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
const double vrijheid_m2_meter = 0.25;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen

const double starting_pos_m1 = 0.5*pi;                // Startin posities van de motoren
const double starting_pos_m2 = 0.25;

double reference_position_motor1 = 0;     // Dit zijn de eerste posities waar de motoren heen willen bij de initialisatie
double reference_position_motor2 = 0;           

double PID_output_1 = 0.0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
double PID_output_2 = 0.0;

double error_prev_1 = 0.0;                      // De initiele previous error
double error_int_1 = 0.0;                       // De initiele intergral error

double error_prev_2 = 0.0;
double error_int_2 = 0.0;

//bool which_motor = false;
enum which_motor{motor1, motor2};
which_motor motor_that_runs = motor1;






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

// De program starter ------------------------------------------------
void start_program(){
    states = init_m1;
    pc.printf("Starting\n\r");
}


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


// De initialiseer beeindiger knop ------------------------------------------------
void stop_initializing(){
    states = run;
    ledred.write(1);
    encoder_motor1.reset();
    pc.printf("Initializing ended\r\n");
}

/*
// De statechanger -----------------------------------------------------------
void state_changer(){
    if(states == off){
        states = init_m1;
    }
    ()
*/

// Functie voor het switchen tussen de motors ------------------------------
void motor_switch(){
    if(motor_that_runs == motor1){
        motor_that_runs = motor2;
        }
            else{
                motor_that_runs = motor1;
            }

}


// 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 --------------------
double get_reference_position_m1(const double aantal_rad){
    double 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 de uiterste positie vd arm in radialen                                                                    
}


// Functie voor het vinden van de reference position van motor 2 --------------------
double get_reference_position_m2(const double aantal_meter){
    double 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_meter;            // Aantal meter is hoeveelheid radialen van middelpunt tot minima.                                                                    
}
   

// HIDScope functie ----------------------------------------------------------------------
void set_scope(double input1, double input2, double input3){
    scope.set(0, input1); 
    scope.set(1, input2);
    scope.set(2, input3);
    scope.send();
}


// De PID-controller voor de motors -------------------------------------------------
double PID_controller(double ref_pos, double mot_pos, double &e_prev, double &e_int, const double Kp, const double Kd, const double Ki){
    double error = ref_pos - mot_pos;       // error uitrekenen
    double error_dif = (error-e_prev)/T_getpos;      // De error differentieren
    //error_dif = .....       // Filter biquad poep
    e_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
    e_int = e_int + T_getpos*error;             // De error integreren
    return Kp*error + Ki*e_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 go-flag functies -----------------------------------------------------------------
void flag1_activate(){flag1=true;}           // Activeert de flag
void flag2_activate(){flag2=true;}   


// De initialiseren functie ------------------------------------------------------------
void initialise_m1(){ 
    if (flag1){
        flag1 = false;
        ledred = !ledred;
        PID_output_1_in = PID_controller(reference_position_motor1, get_position_m1(rad_per_count)+starting_pos_m1, error_prev_1_in, error_int_1_in, Kp_1, Kd_1, Ki_1);
        set_motor1(PID_output_1_in);
    }
}


// De functie die de motoren aanstuurt -------------------------------------------------
void running_motors(){
    if (flag1){
            flag1 = false;
            ledgreen = !ledgreen;
            switch (motor_that_runs){
                case motor1 :      // In deze case draait alleen motor 1
                    PID_output_1 = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
                    set_motor1(PID_output_1);
                    break;
                case motor2 :
                    PID_output_2 = PID_controller(get_reference_position_m2(vrijheid_m2_meter), get_position_m2(meter_per_count), error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
                    set_motor2(PID_output_2);
                    break;
            }
    }
}


// De HIDscope debug functie ----------------------------------------------------------------
void call_HIDscope(double input_1, double input_2, double input_3){
        if (flag2){
            flag2 = false;
            set_scope(input_1, input_2, input_3);
        }
}




// DE MAIN =================================================================================================================
int main()
{
    pc.baud(SERIALBAUD);
    
    kill_switch.fall(&emergency_stop);      // Activeert kill switch
    test_button.fall(&motor_switch);        // Switcht motoren
    start_button.fall(&start_program);
    initializing_stopper.fall(&stop_initializing);
    
    
    test_ticker.attach(&flag1_activate,0.1);                  // Activeert de go-flag van motor positie 
    hidscope_ticker.attach(&flag2_activate,0.01);   
    
    while(safe){
        switch(states){
            case off :              // Dan staat het programma stil en gebeurt er niks
                break;
            case init_m1 :          // Hier voert hij alleen het initialiseren van motor 1 uit
                initialise_m1();      
                break;
            case run :
                running_motors();
                break;
            
        }
        call_HIDscope(PID_output_1_in, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(meter_per_count));
    }  
    motor1_speed_pin = 0;   //Dit zet de motoren uit nadat de kill switch is gebruikt
    motor2_speed_pin = 0;
}