File content as of revision 30:457e42514c47:

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

// Includen van alle libraries ---------------------------------------
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#include "HIDScope.h"
#include "BiQuad.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::X4_ENCODING);
QEI encoder_motor2(D12,D13,NC,64, QEI::X4_ENCODING);


// 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 motor_switch_button(D3);
InterruptIn state_switch_button(D2);
DigitalIn EMG_sim1(SW2);
DigitalIn EMG_sim2(SW3);

//AnalogIn pot1(A1);                              // Dit is de gesimuleerde emg1
//AnalogIn pot2(A2);                              // Dit is de gesimuleerde emg2


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

AnalogIn    emg0( A0 );         //  right biceps
AnalogIn    emg1( A1 );         //  left biceps

AnalogIn init_pot(A3);          // POT meter 


//Definieren van de tickers ------------------------------------------
Ticker motor_ticker;                        // Deze ticker activeert het motor_runs programma, dit leest motor pos en ref pos uit, rekent de PID uit en stuurt met dit de motoren
Ticker hidscope_ticker;


// Het definieren van de filters ----------------------------------------
BiQuadChain bqc1;       //  Combined filter: high-pass (10Hz) and notch (50Hz)
BiQuad bq1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
BiQuad bq3( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );

BiQuadChain bqc2;
BiQuad bq4 = bq1;
BiQuad bq5 = bq2;
BiQuad bq6 = bq3;

BiQuadChain bqc3;       //  Low-pass filter (5Hz)
BiQuad bq7( 5.84514e-08, 1.16903e-07, 5.84514e-08, -1.94264e+00, 9.43597e-01 );
BiQuad bq8( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.97527e+00, 9.76245e-01 );

BiQuadChain bqc4;
BiQuad bq9 = bq7;
BiQuad bq10 = bq9;

BiQuadChain bqc5;       //  Low-pass filter (10Hz) voor PID
BiQuad bq11( 1.32937e-05, 2.65875e-05, 1.32937e-05, -1.77831e+00, 7.92447e-01 );
BiQuad bq12( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.89342e+00, 9.08464e-01 );






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

enum states_enum{off, init, init_fish, 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;

double ref_pos_prev_m1 = 0.0;                          // De initiele ref_pos_pref
double ref_pos_prev_m2 = 0.0;

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 = 0.008;
const double meter_per_count = rad_per_count * radius_tandwiel;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!

const double v_max_karretje = 8.4*radius_tandwiel;                  // Maximale snelheid karretje, gelimiteerd door de v_max vd motor

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

const double T_motor_function = 0.01;           // Periode van de frequentie van het aanroepen van onder andere positiechecker (get_position) en de rest vd motor functie

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

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

const double Kp_2 = 1.0;                    // De PID variablen voor motor 2
const double Kd_2 = 0.4;
const double Ki_2 = 0.1;

const double vrijheid_m1_rad = 0.4*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 initial_pos_m1 = vrijheid_m1_rad;                // Startin posities van de motoren
const double initial_pos_m2 = vrijheid_m2_meter; 

double position_motor1;

double position_motor2; 
double position_motor2_prev = initial_pos_m2;    

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;

double reference_pos_m2;                      // De reference positie waar de motor heen wil afhankelijk van het EMG signaal
double reference_pos_m1;

enum which_motor{motor1, motor2};               // enum van de motoren
which_motor motor_that_runs = motor1;

int  digital0;               //  Define digital signals fot EMG
int  digital1;

double  threshold0 = 0.3;       //  Define thresholds (signal is normalized, so 0.3 = 30% of maximum value) fot EMG
double  threshold1 = 0.3;

double max_out0 = 0;            //  Set initial maximum values of the signals to 0, the code will check if there's a new maximum value every iteration
double max_out1 = 0;






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


// De statechanger -----------------------------------------------------------
void state_changer(){
    if(states == off){
        states = init;
    }
    else if(states == init){
        states = init_fish;
    }
    else if(states == init_fish){
        states = run;
    }
    else{
        states = off;
    }
}


// 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 + initial_pos_m1;      // 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, double mot_pos_prev){     // returned de positie van het karretje in meter
    double mot_pos;
    counts2 = encoder_motor2.getPulses();               // leest aantal pulses vd encoder af
    mot_pos = meterpercount * counts2 + mot_pos_prev;
    encoder_motor2.reset();
    mot_pos_prev = mot_pos;
    return mot_pos_prev;                     // rekent de positie van het karretje uit en geeft dit als output vd functie
}


// Functie voor het uitlezen van het EMG signaal ------------------------------------------
int get_EMG()
{
    double in0 = emg0.read();                   //  EMG signal 0 (right biceps)
    double in1 = emg1.read();                   //  EMG signal 1 (left biceps)
    
    double filtered0 = bqc1.step(in0);          //  Filter signals   (high pass and notch)
    double filtered1 = bqc2.step(in1);
    double abs_filtered0 = abs(filtered0);      //  Absolute value of filtered signals
    double abs_filtered1 = abs(filtered1);
    double out0 = bqc3.step(abs_filtered0);     //  Filter the signals again to remove trend  (low pass)
    double out1 = bqc4.step(abs_filtered1);
        
    if (out0 > max_out0)                        //  Check if the signals have a new maximum value
        {   max_out0 = out0;    }
    if (out1 > max_out1)
        {   max_out1 = out1;    }
    
    double normalized_out0 = out0 / max_out0;   //  Normalize the signals (divide by maximum value to scale from 0 to 1)
    double normalized_out1 = out1 / max_out1;
    
    if (normalized_out0 > threshold0)           //  If the signal value is above the threshold, give an output of 1
        {   digital0 = 1;   }
    else                                        //  If the signal value is below the threshold, give an output of 0
        {   digital0 = 0;   }
            
    if (normalized_out1 > threshold1)           //  Do the same for the second EMG signal
    {   digital1 = 1;   }
    else
    {   digital1 = 0;   }
    
    /*
    scope.set(0,in0);           //  EMG signal 0    (right biceps)
    scope.set(1,in1);           //  EMG signal 1    (left biceps)
    scope.set(2,digital0);      //  Signal 0 after filtering, detrending, rectification, digitalization
    scope.set(3,digital1);      //  Signal 1 after filtering, detrending, rectification, digitalization
    scope.set(4,digital0 - digital1);         //  Final output signal
    
    scope.send();               //  Send all channels to the PC at once
    */

    return digital0 - digital1;         //  Subtract the digital signals (right arm gives positive values, left arm give negative values)
}



// Functie voor het vinden van de reference position van motor 1 --------------------
double get_reference_position_m1(double &ref_pos_prev, const double vrijheid_rad){
    double ref_pos;
    // int final_signal = get_EMG();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
    int final_signal = EMG_sim1.read() - EMG_sim2.read();
    switch(final_signal){
        case 0 :
            ref_pos = ref_pos_prev;
            break;
        case 1 :
            ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad*0.3;         // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
            break;
        case -1 :
            ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad*0.3;
            break;
    }
    if(ref_pos >= vrijheid_rad){
        ref_pos = vrijheid_rad;
    }
    if(ref_pos <= -vrijheid_rad){
        ref_pos = -vrijheid_rad;
    }
    ref_pos_prev = ref_pos;
    return ref_pos_prev;            // vrijheid_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(double &ref_pos_prev, const double vrijheid_meter){
    double ref_pos;
    int final_signal = EMG_sim1.read() - EMG_sim2.read();     // Het uiteindelijke signaal uit de emg als output zit is -1, 0 of 1.
    switch(final_signal){
        case 0 :
            ref_pos = ref_pos_prev;
            break;
        case 1 :
            ref_pos = ref_pos_prev + T_motor_function*v_max_karretje;           // Hierdoor veranderd de ref_pos met de maximale snelheid vd motor (karretje)
            break;
        case -1 :
            ref_pos = ref_pos_prev - T_motor_function*v_max_karretje;
            break;
    }
    if(ref_pos >= vrijheid_meter){
        ref_pos = vrijheid_meter;
    }
    if(ref_pos <= 0){
        ref_pos = 0;
    }
    ref_pos_prev = ref_pos;
    return ref_pos_prev;                                                                                
}


// 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_motor_function;      // De error differentieren
    //scope.set(error_dif,0); 
    //error_dif = bqc5.step(error_dif);       // Filter biquad
    //scope.set(error_dif,1); scope.send();
    e_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
    e_int = e_int + T_motor_function*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;}   


// Dit doet de robot als hij niets doet ------------------------------------------------------
void robot_is_off(){
    ledgreen.write(1);              
    ledred.write(0);               // Indicator ik ben uit
    motor1_speed_pin = 0;           // Uitzetten vd motoren
    motor2_speed_pin = 0;
}


// De initialisatie functie --------------------------------------------------------------------
void initialise(){
    ledred.write(1);
    ledblue.write(0);
    if (flag1){
        double init_m2_speed = init_pot;
            set_motor2(init_m2_speed);
        }
}


// De initialiseren beeindigen functie ------------------------------------------------------------
void initialise_finish(){       // Deze functie laat de motor van een vaste uiterste positie (fysisch limiet) naar het middelpunt draaien.
    ledblue.write(1);
    ref_pos_prev_m1 = initial_pos_m1;
    ref_pos_prev_m2 = initial_pos_m2;
    position_motor1 = initial_pos_m1;
    position_motor2 = initial_pos_m2;
    position_motor2_prev = initial_pos_m2;
    PID_output_1 = 0.0;                      
    PID_output_2 = 0.0;
    ledgreen.write(0);
    ledred.write(0);
    encoder_motor1.reset();
    encoder_motor2.reset();             
    motor1_speed_pin = 0;
}  


// De functie die de motoren aanstuurt -------------------------------------------------
void running_motors(){              // Deze functie kijkt welke motor moet draaien en rekent dan de PID uit en geeft dit door aan de motor input functie
    if (flag1){
            flag1 = false;
            ledred.write(1);
            switch (motor_that_runs){
                case motor1 :      // In deze case draait alleen motor 1
                    reference_pos_m1 = get_reference_position_m1(ref_pos_prev_m1, vrijheid_m1_rad);
                    position_motor1 = get_position_m1(rad_per_count);
                    PID_output_1 = PID_controller(reference_pos_m1, position_motor1, error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);
                    //PID_output_2 = PID_controller(-reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1); // Zorgt er voor dat motor2 meedraait met motor1 
                    set_motor1(PID_output_1);
                    //set_motor2(-PID_output_1);
                    encoder_motor2.reset();
                    break;
                case motor2 :
                    reference_pos_m2 = get_reference_position_m2(ref_pos_prev_m2, vrijheid_m2_meter);
                    position_motor2 = get_position_m2(meter_per_count, position_motor2_prev);
                    PID_output_2 = PID_controller(reference_pos_m2, position_motor2, error_prev_2, error_int_2, Kp_2, Kd_2, Ki_2);
                    PID_output_1 = PID_controller(reference_pos_m1, get_position_m1(rad_per_count), error_prev_1, error_int_1, Kp_1, Kd_1, Ki_1);   // Zorgt ervoor dat motor1 op de dezelfde positie blijft als motor 2 draait.
                    set_motor2(PID_output_2);
                    set_motor1(PID_output_1);
                    position_motor2_prev = position_motor2;
                    break;
            }
    }
}


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




// DE MAIN =================================================================================================================
int main()
{
    pc.baud(SERIALBAUD);
    
    bqc1.add( &bq1 ).add( &bq2 ).add( &bq3 );
    bqc2.add( &bq4 ).add( &bq5 ).add( &bq6 );
    bqc3.add( &bq7 ).add( &bq8 );
    bqc4.add( &bq9 ).add( &bq10 );
    bqc5.add( &bq11 ).add( &bq12 );
    
    
    
    
    state_switch_button.fall(&state_changer);        // Switcht states
    motor_switch_button.fall(&motor_switch);        // Switcht motors
    

    
    motor_ticker.attach(&flag1_activate, T_motor_function);                  // Activeert de go-flag van motor positie 
    hidscope_ticker.attach(&flag2_activate,0.01);   // Leest ref pos en mot pos uit, rekent PID uit en stuurt motoren aan.
    
    while(1){
        switch(states){
            case off :                    // Dan staat het programma stil en gebeurt er niks
                robot_is_off();
                break;
            case init :          // Hier voert hij alleen het initialiseren van motor 1 uit
                initialise();      
                break;
            case init_fish :
                initialise_finish();
                break;
            case run :
                running_motors();       // Hier voert hij het programma voor het aansturen vd motors uit
                break;
            
        }
        call_HIDscope(PID_output_1, reference_pos_m1, position_motor1);
    }  
}