File content as of revision 29:55547603475e:

// Project BioRobotics - Opening a Door - Group 13 2019/2020
// Dion ten Berge       - s1864734
// Bas Rutteman         - s1854305
// Nick in het Veld     - s1915584
// Marleen van der Weij - s1800078
// Mevlid Yildirim      - s2005735 

/* To-Do
1. Kd, Ki, Kp waardes bepalen
2. Filter cutoff frequentie bepalen, zie https://github.com/tomlankhorst/biquad
3. Grenswaarde EMG signaal na het filteren
*/

//*****************************************************************************
// 1. Libraries ******************************************************************
//*****************************************************************************
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"

//*****************************************************************************
// 2. States ******************************************************************
//*****************************************************************************
enum States {StartWait, MotorCalibration, EMGCalibration, Homing, Demo, Operating, EmergencyMode, Idle}; //All robot states
States State;

//*****************************************************************************
// 3. (Global) Variables ***********************************************************
//*****************************************************************************
// 3.1 Tickers *****************************************************************
Ticker ticker_mainloop;      // The ticker which runs the mainloop
Ticker ticker_hidscope; // The ticker which sends data to the HIDScope server

// 3.2 General variables *******************************************************

MODSERIAL pc(USBTX, USBRX); // Serial communication with the board
QEI encoder_motor1(D12,D13,NC,64); // Defines encoder for motor 1
QEI encoder_motor2(D10,D11,NC,64); // Defines encoder for motor 1
double f=1/100;                     // Frequency, currently unused
const double Ts = 0.001;            // Sampletime
HIDScope scope(2);                  // Amount of HIDScope servers



    
    bool emg0_active;
    bool emg1_active;
    bool emg2_active;
    bool emg3_active;

// 3.3 EMG Variables **********************************************************
static BiQuad LowPassFilter_motor1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
static BiQuad LowPassFilter_motor2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 

static BiQuad highfilter0(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
static BiQuad LowPassFilter0( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
    
static BiQuad highfilter1(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
static BiQuad LowPassFilter1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
   
static BiQuad highfilter2(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
static BiQuad LowPassFilter2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 
   
static BiQuad highfilter3(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
static BiQuad LowPassFilter3( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 

double emg0_raw_signal ; double emg1_raw_signal ; double emg2_raw_signal ; double emg3_raw_signal ;
double high_emg0_signal ; double high_emg1_signal ; double high_emg2_signal ; double high_emg3_signal ;
double rec_emg0_signal ; double rec_emg1_signal ;  double rec_emg2_signal ;  double rec_emg3_signal ;   
double low_rec_high_emg0_signal ; double low_rec_high_emg1_signal ; double low_rec_high_emg2_signal ; double low_rec_high_emg3_signal ;
double emg0_signal ; double emg1_signal ; double emg2_signal ; double emg3_signal ;



// 3.4 Hardware ***************************************************************
    //3.4a Leds
    DigitalOut led_red(LED_RED); // Defines the red led on the K64 board (0=on, 1 = off)   
    DigitalOut led_green(LED_GREEN); // Defines the green led on the K64 board (0=on, 1 = off)
    DigitalOut led_blue(LED_BLUE); // Defines the blue led on the K64 board (0=on, 1 = off)  
    FastPWM led1(D9);         //CODE DOES NOT WORK WITH D8 PIN DEFINED      //Defines Led1 on the BioRobotics Shield
    FastPWM led2(A5);               //Defines Led2 on the BioRobotics Shield
    
    //3.4b Potmeters and buttons
  //  AnalogIn pot1_links(A5); //BUITEN GEBRUIK Defines potmeter1 on the BioRobotics Shield
    AnalogIn pot2_rechts(A4); //Defines potmeter2 on the BioRobotics Shield
    DigitalIn button1(D2); //Defines button1 on the BioRobotics Shield
    DigitalIn button2(D3); //Defines button2 on the BioRobotics Shield
    DigitalIn sw2(SW2); //Defines button SW2 on the K64 board
    DigitalIn sw3(SW3); //Defines button SW3 on the K64 board
    
    //3.4c Motors
    DigitalOut motor1DirectionPin(D7);   // motor 1 direction control (1=cw, 0=ccw)
    FastPWM motor1(D6);                  // motor 1 velocity control (between 0-1)
    FastPWM motor2(D5);                  // motor 2 velocity control (between 0-1)
    DigitalOut motor2DirectionPin(D4);   // motor 2 direction control (1=cw, 0=ccw)
    bool motor1_calibrated=false;
    bool motor2_calibrated=false;
 
    //3.4d EMGs
    AnalogIn    emg0(A0); //Rechterarm
    AnalogIn    emg1(A1); //Linkerarm
    AnalogIn    emg2(A2); //Rechterbeen
    AnalogIn    emg3(A3); //Linkerbeen
    
    double emg0_max=0.0; float emg0_threshhold=0.0;
    double emg1_max=0.0; double emg1_threshhold;
    double emg2_max=0.0; double emg2_threshhold;
    double emg3_max=0.0; double emg3_threshhold;
    
    double threshold_ratio=0.5;
    int emg_tijd=0; 
    bool calibration_done=false;
    int homing_tijd=0;
    int demo_tijd=0;
    
// 3.5 Motor 1 variables ***************************************************************   
    
    double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
    static double error_integral_motor1 = 0;
    double Yref_motor1;                                 
    double kp_motor1;
    double Ki_motor1;
    double Kd_motor1;
    double Up_motor1;
    double Ui_motor1;
    double Ud_motor1;
    
    double positie_motor1;                              //counts encoder
    double error1_motor1;
    double error1_prev_motor1;
    double error1_derivative_motor1;
    double error1_derivative_filtered_motor1;
    double P_motor1;
    
    double positie_verschil_motor1;
    double positie_prev_motor1;

// 3.5 Motor 2 variables ***************************************************************
    
    double counts_per_rad_motor2 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
    static double error_integral_motor2 = 0;
    double Yref_motor2;                                 
    double kp_motor2;
    double Ki_motor2;
    double Kd_motor2;
    double Up_motor2;
    double Ui_motor2;
    double Ud_motor2;

    double positie_motor2;                              //counts encoder
    double error1_motor2;
    double error1_prev_motor2;
    double error1_derivative_motor2;
    double error1_derivative_filtered_motor2;
    double P_motor2;
    
    double positie_verschil_motor2;
    double positie_prev_motor2;    
    
// 3.6 RKI Variables ***********************************************************    
    
    double theta_motor1;  //Hoek motor 1
    double theta_motor2;  //Hoek motor2
    double pi=3.14159265358979323846; 
    double length_link1=16.25; 
    double length_link2=15;
    double x = 0;   // X positie in het  begin, workspace max: 8.5 < x < 30.5
    double x_max= 100; //Maximale workspace waarde X, zou 30.5 moeten zijn
    double x_min= -100; //Minimale workspace waarde X, zou 8.5 moeten zijn;
    double x_home=3.0;  //Home positie, waar de x heen gaat bij homing
    double x_vergroting=0.02; //De stap waarmee de x groter wordt bij elke keer als de emg actief is. EMG wordt elke 1 miliseconde gerund. Dus als je 1 seconde lang een emg signaal geeft, krijg je een vergroting van 100.
    
    double y = 0; // Y positie in het  begin, workspace max:7.5 < y < 27
    double y_max=100; //Minimale workspace waarde y, zou 27 moeten zijn
    double y_min=-100; // Minimale workspace waarde y, zou 7.5 moeten zijn;
    double y_home=0; //Home positie, waar de y heen gaat bij homing
    double y_vergroting=0.02; //De stap waarmee de y groter wordt bij elke keer als de emg actief is. EMG wordt elke 1 miliseconde gerund. Dus als je 1 seconde lang een emg signaal geeft, krijg je een vergroting van 100.
    double a; //Constante voor de deel van de formules voor RKI
    double b; //Constante voor de deel van de formules voor RKI
    double c; //Constante voor de deel van de formules voor RKI
    double d; //Constante voor de deel van de formules voor RKI

//******************************************************************************
// 4. Functions ****************************************************************
//******************************************************************************

    // 4.1 Hidscope ****************************************************************
    void HIDScope() //voor HIDscope
    {
        scope.set(0, emg2_signal);
        scope.set(1, emg3_signal);
   //     scope.set(2, emg2_raw_signal);
   //     scope.set(3, emg3_raw_signal);
 

   //    scope.set(4, Ui_motor1);
   //    scope.set(5, Uk_motor1);
       
        scope.send();   
    }
    
    // 4.2a Encoder motor1 ****************************************************************
        double fencoder_motor1()                                    // bepaalt de positie van de motor
        {
            positie_motor1 = encoder_motor1.getPulses()/counts_per_rad_motor1;                // haalt encoder waardes op
            positie_verschil_motor1 =  (positie_motor1-positie_prev_motor1)/Ts; //voor de motorcalibratie
            positie_prev_motor1 = positie_motor1;
            
            return positie_motor1;                                 //geeft positie van motor
        }
    // 4.2b Encoder motor2 ****************************************************************        
        double fencoder_motor2()                                    // bepaalt de positie van de motor
        {
            positie_motor2 = encoder_motor2.getPulses()/counts_per_rad_motor2;                // haalt encoder waardes op
            positie_verschil_motor2 =  (positie_motor2-positie_prev_motor2)/Ts;   //voor de motorcalibratie
            positie_prev_motor2 = positie_motor2;
            
            return positie_motor2;                                 //geeft positie van motor
        }
        
    // 4.3 Calibration motors ****************************************************************
        
        // 4.3a Calibration motors ****************************************************************     
    void motor_calibration() 
    {
        // Calibration motor 1
        motor1DirectionPin=0; //direction of the motor
        motor1=1.0;
        wait(0.3);
        while (abs(positie_verschil_motor1)>5)
        {
            motor1=0.2  ; 
    //        pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
        }
        motor1=0.0;
        motor1_calibrated=true;  
     //   pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
    
    
    // 4.3b Calibration motors ********************************************
        // Calibration motor 2
        motor2DirectionPin=0; //direction of the motor
        motor2=1.0;
        wait(0.3);
        while (abs(positie_verschil_motor2)>5)
        {
            motor2=0.2  ; 
  //          pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
            led2=1;
        }
        motor2=0.0;
        led2=0;
        motor2_calibrated=true;  
    //    pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
        
            
    }
    
    // 4.4a PID-Controller motor 1**************************************************
    double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
    {
        //Proportional part
        kp_motor1 = 4.9801 ;      // moet nog getweaked worden
        Up_motor1 = kp_motor1 * error1_motor1;
    
        //Integral part
        Ki_motor1 = 22.6073;     // moet nog getweaked worden
        error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
        Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
    
        //Derivative part 
        Kd_motor1 = 0.012757 ;// moet nog getweaked worden  
        error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
        error1_derivative_filtered_motor1 = LowPassFilter_motor1.step(error1_derivative_motor1); //derivative wordt gefiltered
        Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
        error1_prev_motor1 = error1_motor1;

        P_motor1 = Up_motor1 ;//+ Ui_motor1 + Ud_motor1;                                           //sommatie van de u's
        
        return P_motor1; 
    }

    // 4.4b PID-Controller motor 2**************************************************
    double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
    {
        //Proportional part
        kp_motor2 = 4.9801 ;      // moet nog getweaked worden
        Up_motor2 = kp_motor2 * error1_motor2;
    
        //Integral part
        Ki_motor2 = 22.6073;     // moet nog getweaked worden
        error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2);       // integrale fout + (de sample tijd * fout)
        Ui_motor2 = Ki_motor2 * error_integral_motor2;                     //de fout keer de integrale fout
    
        //Derivative part 
        Kd_motor2 = 0.012757 ;// moet nog getweaked worden  
        error1_derivative_motor2 = (error1_motor2  -  error1_prev_motor2)/Ts;
        error1_derivative_filtered_motor2 = LowPassFilter_motor2.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
        Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
        error1_prev_motor2 = error1_motor2;

        P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2;                                           //sommatie van de u's

        return P_motor2; 
    }
   // 4.5a Motor 1 direction******************************************** 
    double motor1_pwm()
    {
        
        if (P_motor1 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
        {                
            motor1DirectionPin=1;   // Clockwise rotation
        } 
        else 
        {
             motor1DirectionPin=0;   // Counterclockwise rotation
        }
            
        if (fabs(P_motor1) > 0.99 )  // als de absolute waarde van de motorsnelheid groter is als 1, terug schalen naar 1, anders de absolute waarde van de snelheid. (Bij een waarde lager als 0 draait de motor niet)
        {         
            motor1 = 0.99 ;
        } 
        else 
        {
            motor1 = fabs(P_motor1);
        }
    }  
    
    // 4.5b Motor 1 direction ********************************************
   double motor2_pwm()
    {
        
        if (P_motor2 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
        {                
            motor2DirectionPin=1;   // Clockwise rotation
        } 
        else 
        {
             motor2DirectionPin=0;   // Counterclockwise rotation
        }
            
        if (fabs(P_motor2) > 0.99 )  // als de absolute waarde van de motorsnelheid groter is als 1, terug schalen naar 1, anders de absolute waarde van de snelheid. (Bij een waarde lager als 0 draait de motor niet)
        {         
            motor2 = 0.99 ;
        } 
        else 
        {
            motor2 = fabs(P_motor2);
        }            
    }
    
    // 4.6a Motor 1 controller ******************************************** 
    void motor1_controller(void)
    {
        error1_motor1 = (theta_motor1 - positie_motor1);
         
        if (motor1_calibrated==true&&motor2_calibrated==true)
            {
               motor1_pwm();
            } 
        
    }

    // 4.3a Motor 2 controller ********************************************
    void motor2_controller(void)
    {
        error1_motor2 = (theta_motor2 - positie_motor2);
        if (motor1_calibrated==true&&motor2_calibrated==true)
            {
                motor2_pwm();
            } 
    } 
    
void emg0_processing()
{
    emg0_raw_signal=emg0.read();

    high_emg0_signal  =     highfilter0.step(emg0_raw_signal);
    rec_emg0_signal   =     abs(high_emg0_signal);    
    low_rec_high_emg0_signal  =     LowPassFilter0.step(rec_emg0_signal);
    emg0_signal = low_rec_high_emg0_signal;  
    
    emg0_threshhold = 0.5*emg0_max; //
    
    if (calibration_done && (emg0_signal>emg0_threshhold))
    { emg0_active=true;}
    else {emg0_active=false;}
}     

void emg1_processing()
{
    emg1_raw_signal=emg1.read();

    high_emg1_signal  =     highfilter1.step(emg1_raw_signal);
    rec_emg1_signal   =     abs(high_emg1_signal);    
    low_rec_high_emg1_signal  =     LowPassFilter1.step(rec_emg1_signal);
    emg1_signal = low_rec_high_emg1_signal;
    
    emg1_threshhold = 0.5*emg1_max; //
    
    if (calibration_done && (emg1_signal>emg1_threshhold))
    { emg1_active=true;}
    else {emg1_active=false;}
    
}    

void emg2_processing()
{
    emg2_raw_signal=emg2.read();

    high_emg2_signal  =     highfilter2.step(emg2_raw_signal);
    rec_emg2_signal   =     abs(high_emg2_signal);    
    low_rec_high_emg2_signal  =     LowPassFilter2.step(rec_emg2_signal);
    emg2_signal = low_rec_high_emg2_signal;
    
        emg2_threshhold = 0.5*emg2_max; //
    
    if (calibration_done && (emg2_signal>emg2_threshhold))
    { emg2_active=true;}
    else {emg2_active=false;}
}    

void emg3_processing()
{
    emg3_raw_signal=emg3.read();

    high_emg3_signal  =     highfilter3.step(emg3_raw_signal);
    rec_emg3_signal   =     abs(high_emg3_signal);    
    low_rec_high_emg3_signal  =     LowPassFilter3.step(rec_emg3_signal);
    emg3_signal = low_rec_high_emg3_signal;
    
    emg3_threshhold = 0.5*emg3_max; //
    
    if (calibration_done && (emg3_signal>emg3_threshhold))
    { emg3_active=true;}
    else {emg3_active=false;}
}        
 
    
    double Angle_motor1()
    {
        a = atan2(y , x);
        b = asin((length_link2 * sin(theta_motor2)) / (sqrt(pow(x, 2.0) + pow(y, 2.0))));
        theta_motor1 = b + a;
        return theta_motor1;
    }

    double Angle_motor2()
    {
        c = (pow(x, 2.0) + pow(y, 2.0) - pow(length_link1, 2.0) - pow(length_link2, 2.0));
        d = (2 * length_link1 * length_link2);
        theta_motor2 = acos(c / d);
        return theta_motor2;
    }
    
    double RKI()
    {
        if (emg0_active==true)
        { 
         led1=1;
          if (x>x_max) {x=x_max;}
          else { x=x+ x_vergroting ;  }  
        }
        
        if (emg1_active==true)
        { 
          led2=1;
          if (y>y_max) {y=y_max;}
          else {y=y + y_vergroting ;  }
         }
        
        if (emg2_active==true)
        { 
         led1=0;
        if (x<x_min) {x=x_min;}
          else {x = x - x_vergroting; } 
        }
        
        if (emg3_active==true)
        {
         led2=1;
          if (y<y_min) {y=y_min;}
         else {y=y - y_vergroting;} 
        } 
        
        Angle_motor1();
        Angle_motor2();
    } 

     
    
    // 4.3 State-Machine *******************************************************

    void state_machine()
{
    if (sw2==0) {State = EmergencyMode;}
    switch(State)
    {       
        case MotorCalibration: 
//            pc.printf("\r\n State: MotorCalibration");
            led_blue.write(0);
            led_red.write(1);
            led_green.write(1); //Green Led on when in this state
            
            if (motor1_calibrated==true&&motor2_calibrated==true)
            {
                pc.printf("\r\n Motor Calibration is done!");

                encoder_motor1.reset();
                encoder_motor2.reset();

                
                State=StartWait; 
            }
            else {;} //pc.printf("\r\n Motor Calibration is not done!");}
            
            break; 
                
        case StartWait:  
 //           pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo"); 
            led_blue.write(0);
            led_red.write(1);
            led_green.write(0);
            
            emg_tijd =0;
            calibration_done=false;
            State=EMGCalibration;
            break;  
                       
        case EMGCalibration: 
   //         pc.printf("\r\n State: EMGCalibration");
            led_blue.write(1);
            led_red.write(1);
            led_green.write(1);
            
            emg_tijd++;
            
            if (0<=emg_tijd&&emg_tijd<=3000)
            { led_green.write(1); }
            else if (3000<emg_tijd&&emg_tijd<6000)
            {   led_green.write(0);
                if (emg0_signal > emg0_max)
                {
                emg0_max=emg0_signal;
                }     
                
                if (emg1_signal > emg1_max)
                {
                emg1_max=emg1_signal;
                }   
                
                if (emg2_signal > emg2_max)
                {
                emg2_max=emg2_signal;
                }  
                
                if (emg3_signal > emg3_max)
                {
                emg3_max=emg3_signal;
                }         
            }
            else if (6000<emg_tijd)
            {led_green.write(1);
            
               
                if(button1==0) {State=StartWait;}
                if(button2==0) {State=Homing;}
            }
            
            break; 
        case Homing: 
       //     pc.printf("\r\n State: Homing");
            led_blue.write(0);
            led_red.write(1);
            led_green.write(1);
            led2=1;

            homing_tijd++;
            x= x_home;
            y= y_home;
            if (homing_tijd>1000) //Voorkomt dat het automatisch naar Demo springt, omdat je bij de vorige nog knop 2 hebt ingedrukt
            {
                if(button1==0) {State=Demo;} 
                if(button2==0) {State=Operating;}
                
            }
            
            break; 
  
        case Operating:
//          pc.printf("\r\n State: Operating");
            calibration_done=true;
            led_blue.write(1);
            led_red.write(1);
            led_green.write(0);
            led2=0;
            break; 
        
        case Demo:
          // pc.printf("\r\n State: Demo");
            led_blue.write(1);
            led_red.write(0);
            led_green.write(1); 
            
     //       motor1_calibrated=true;
     //       motor2_calibrated=true;
     //       emg0_threshhold=100;
     //       emg1_threshhold=100;
     //       emg2_threshhold=100;
     //       emg3_threshhold=100;
            
            calibration_done=false;
            
            if (button1==0)
            {    led1=1;
                if (y>y_max) {y=y_max;}
                else { y=y+ y_vergroting ;}
            }
            if (button2==0)
            {   led1=0;
                if (y<y_min) {y=y_min;}
                else { y=y - y_vergroting ;}
            }
            
            
            x = 30*pot2_rechts.read();

            break;
            
        case EmergencyMode:
            pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
            
            motor1=0; 
            motor2=0;
            
            led_blue.write(1);
            led_green.write(1);
            //SOS start
            led_red.write(0); // S
            wait(0.5);
            led_red.write(1); //pause
            wait(0.25);
            led_red.write(0); // O
            wait(1.5);
            led_red.write(1); // pause
            wait(0.25);
            led_red.write(0); // S
            wait(0.5);
            //SOS end
            break; 
        case Idle:
   /*         pc.printf("\r\n Idling..."); */
            break;

        }
}
    
//******************************************************************************
// 5. Main Loop ****************************************************************
//******************************************************************************

void main_loop() { //Beginning of main_loop()




//    pc.printf("main_loop is running succesfully \r\n"); //confirmation that main_loop is running (als je dit erin zet krijg je elke duizendste dit bericht. Dit is niet gewenst)






//test stuff:
//Yref_motor1= 4200*pot2_rechts.read();
//Yref_motor2= 4200*pot1_links.read();

// 5.1 Measure Analog and Digital input signals ********************************
emg0_processing();
emg1_processing();
emg2_processing();
emg3_processing();

fencoder_motor1() ;
fencoder_motor2() ;

// 5.2 Run state-machine(s) ****************************************************
state_machine() ;

// 5.3 Run controller(s) *******************************************************

RKI();

PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
PID_controller_motor2(error_integral_motor2, error1_prev_motor2);

// 5.4 Send output signals to digital and PWM output pins **********************
motor1_controller();
motor2_controller();
                    

}                   //Ending of main_loop()
  
//******************************************************************************
// 6. Main function ************************************************************
//******************************************************************************
int main() 
{ //Beginning of Main() Function  //All the things we do only once, some relevant things are now missing here: set pwmperiod to 60 microsec. Set Serial comm. Etc. Etc.
// 6.1 Initialization **********************************************************
    pc.baud(115200);
    pc.printf("\r\nStarting Project BioRobotics - Opening a Door " //print Project information
                "- Group 13 2019/2020 \r\n"
                "Dion ten Berge       - s1864734 \r\n"
                "Bas Rutteman         - s1854305 \r\n"
                "Nick in het Veld     - s1915584 \r\n"
                "Marleen van der Weij - s1800078 \r\n"
                "Mevlid Yildirim      - s2005735 \r\n");
    led_green.write(1);
    led_red.write(1);
    led_blue.write(1);
    State = MotorCalibration ; // veranderen naar MotorCalibration;
    ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
    ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
    
   motor_calibration();
    
    
// 6.2 While loop in main function**********************************************   
    while (true) { wait(0.5); pc.printf("\r\n x = %f, y = %f, \r\n theta_motor1 = %f, theta_motor2 = %f, error1_motor1 = %f", x,y, theta_motor1, theta_motor2, error1_motor1);
                    pc.printf("\r\n emg0 = %f, emg1 = %f, emg2 = %f, emg3 = %f", emg0_signal, emg1_signal, emg2_signal, emg3_signal);} //Is not used but has to remain in the code!!
    
}                  //Ending of Main() Function