File content as of revision 8:b4cf0a68e37f:

// 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; //defines 'State' which can contain a state  (i.e. StartWait) from 'enum States'.


//*****************************************************************************
// 3. (Global) Variables ***********************************************************
//*****************************************************************************
// 3.1 Tickers *****************************************************************
Ticker ticker_mainloop;      // The ticker which runs the mainloop
Ticker hidscope_ticker; // 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
const double Ts = f/1000;            // Sampletime
HIDScope scope(1);                  // Amount of HIDScope servers
int test = 1;
// 3.3 BiQuad Filters **********************************************************
static BiQuad notchfilter(9.97761e-01, -1.97095e+00, 9.97761e-01, -1.97095e+00, 9.95522e-01);
static BiQuad highfilter(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
static BiQuad LowPassFilter( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 ); 

// 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(D8);         //CODE DOES NOT WORK WITH D8 PIN DEFINED      //Defines Led1 on the BioRobotics Shield (1=on, 0 = off)
    FastPWM led2(D9);               //Defines Led2 on the BioRobotics Shield (1=on, 0 = off)
    
    //3.4b Potmeters and buttons
    AnalogIn pot1_links(A5); //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)
 
// 3.5 PID variabelen ***************************************************************   
    //3.5a PID-controller motor 1
    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 positie_motor1;                              //counts encoder
    double positie_verschil_motor1;
    double positie_prev_motor1 = 200;
    double error1_motor1;
    double error1_prev_motor1;
    double error1_derivative_motor1;
    double error1_derivative_filtered_motor1;
    double P_motor1;

    //3.5b PID-controller motor 2
    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 positie_motor2;                              //counts encoder
    double positie_verschil_motor2;
    double positie_prev_motor2;
    double error1_motor2;
    double error1_prev_motor2;
    double error1_derivative_motor2;
    double error1_derivative_filtered_motor2;
    double P_motor2;

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

    // 4.1 Hidscope ****************************************************************
    void HIDScope() //voor HIDscope
    {
        scope.set(0, test);
//        scope.set(0, positie_motor1); //nog te definieren wat we willen weergeven
//        scope.set(1, positie_verschil_motor1); //nog te definieren wat we willen weergeven
//        scope.set(2, positie_prev_motor1);
        scope.send();   
    }

    // 4.2 Encoders ****************************************************************
        // 4.2a Encoder motor1 ****************************************************************
        double Fun_encoder_motor1()                                    // bepaalt de positie van de motor
        {
            positie_motor1 = encoder_motor1.getPulses();                // haalt encoder waardes op
            positie_verschil_motor1 =  (positie_motor1-positie_prev_motor1)/Ts;
            positie_prev_motor1 = positie_motor1;
            
            return positie_motor1;                                 //geeft positie van motor
        }        
        // 4.2b Encoder motor2 ****************************************************************

    // 4.2a PID-Controller motor 1**************************************************
    double PID_controller_motor1()
    {
        //Proportional part
        kp_motor1 = 0.01 ;      // moet nog getweaked worden
        double Up_motor1 = kp_motor1 * error1_motor1;
    
        //Integral part
        Ki_motor1 = 0.0001;     // moet nog getweaked worden
        error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
        double Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
    
        //Derivative part 
        Kd_motor1 = 0.01 ;// moet nog getweaked worden  
        error1_derivative_motor1 = (error1_motor1  -  error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
        error1_derivative_filtered_motor1 = LowPassFilter.step(error1_derivative_motor1); //derivative wordt gefiltered
        double Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
        error1_prev_motor1 = error1_motor1;

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

        return P_motor1; 
    }

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

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

        return P_motor2; 
    }
    
    // 4.4 motor_calibration *******************************************************
        // 4.4a Motor1_calibration *******************************************************

        void motor1_calibration()
        {
            motor1DirectionPin=0; //direction of the motor
            motor1=0.2;
            wait(0.5);
            while (abs(positie_verschil_motor1)>100)
            {
                motor1=0.2  ;
                pc.printf("\r\n While loop werkt");  
            }
            motor1=0.0;
            pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0");  pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); 
            //State=EMGCalibration;
        
        }
    
    // 4.6 State-Machine *******************************************************
    void state_machine()
    {
        if (sw2==0) {State = EmergencyMode;}
        switch(State)
        {   

            case StartWait:  
                led_blue.write(0); //Turns Led blue on
                if(button1==0) {State=EMGCalibration;}//{State=MotorCalibration;}
                if(button2==0) {State=Demo;}
                break;    
               
           case MotorCalibration: 
                pc.printf("\r\n State: MotorCalibration");
                motor1_calibration();
                //When motor1_calibration is done it defines State=EMGCalibration;
                break;
               
            case EMGCalibration: 
                pc.printf("\r\n State: EMGCalibration");
                led_blue.write(0);
                led_red.write(1);
                led_green.write(1);
    //            wait(2);
                State=Homing;
                break;
                
            case Homing: 
                pc.printf("\r\n State: Homing");
                State=Operating;
                break;
                
            case Demo:
                pc.printf("\r\n State: Demo");
                wait(2);
                led_red.write(1);   //Turns Led red off
                led_blue.write(1); //Turns Led blue off
                led_green.write(0); //Turns Led green on
                State=Idle;
                break;
                
            case Operating:
                pc.printf("\r\n State: Operating");
                //motor1=1;
                break;
            case Idle:
                pc.printf("\r\n Idling...");
                break;             
                            
            case EmergencyMode:
                pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
                motor1=0; //Turns motor1 off
                motor2=0; //Turns motor2 off
                //led1=0; //led1 is disabled //Turns Led1 off
                led2=1; //Turns Led2 off
                
                led_blue.write(1); //Turns Led blue off
                led_green.write(1); //Turns Led green off
                
                
                //SOS signal with Led Red 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;
    
            }
    }
    
//******************************************************************************
// 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)
Fun_encoder_motor1()  ;

// 5.1 Measure Analog and Digital input signals ********************************
// 5.2 Run state-machine(s) ****************************************************
    state_machine();
// 5.3 Run controller(s) *******************************************************
// 5.4 Send output signals to digital and PWM output pins **********************
                    

}                   //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 off
    led_red.write(1);   //Led off
    led_blue.write(1);  //Led off
    State = StartWait; //defines the starting State
    pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo"); 
    
    ticker_mainloop.attach(&main_loop,0.001f); // change back to 0.001f //Run the function main_loop 1000 times per second       
    hidscope_ticker.attach(&HIDScope, 0.01f); //Ticker for Hidscope, different frequency compared to motors
    
// 6.2 While loop in main function**********************************************   
    while (true) { } //Is not used but has to remain in the code!!
    
}                  //Ending of Main() Function