Diff: main.cpp

Revision:

5:7e2c6d2235fe

Parent:

4:784cc0f3c97b

Child:

6:79e42e1b87cb

Child:

9:c4fa72ffa1c2

--- a/main.cpp	Wed Sep 25 09:16:59 2019 +0000
+++ b/main.cpp	Mon Oct 21 23:28:27 2019 +0000
@@ -5,7 +5,11 @@
 // 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 ******************************************************************
@@ -20,41 +24,165 @@
 //*****************************************************************************
 // 2. States ******************************************************************
 //*****************************************************************************
-enum States {waiting}; //All robot states
+enum States {StartWait, MotorCalibration, EMGCalibration, Homing, Operating, Emergency, Demo}; //All robot states
+States state;
 
 //*****************************************************************************
-// 3. Global Variables ***********************************************************
+// 3. (Global) Variables ***********************************************************
 //*****************************************************************************
-Ticker loop_ticker; // The Ticker object that will ensure perfect timing of our looping code
+// 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
+const double Ts = f/10;            // Sampletime
+HIDScope scope(2);                  // Amount of HIDScope servers
+
+// 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 ); 
 
-DigitalOut led(LED_RED); //Geen idee waar we dit kwijt willen
-MODSERIAL pc(USBTX, USBRX); //Geen idee waar we dit kwijt willen
+// 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
+    FastPWM led2(D9);               //Defines Led2 on the BioRobotics Shield
+    
+    //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 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 error1_motor2;
+    double error1_prev_motor2;
+    double error1_derivative_motor2;
+    double error1_derivative_filtered_motor2;
+    double P_motor2;
 
 //******************************************************************************
-// 4. Main Loop ****************************************************************
+// 4. Functions ****************************************************************
+//******************************************************************************
+
+    // 4.1 Hidscope ****************************************************************
+    void HIDScope() //voor HIDscope
+    {
+        scope.set(0, Yref_motor1); //nog te definieren wat we willen weergeven
+        scope.set(1, positie_motor1); //nog te definieren wat we willen weergeven
+        scope.send();   
+    }
+
+    // 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.3 State-Machine *******************************************************
+
+//******************************************************************************
+// 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
-    
-
-// 4.1 Measure Analog and Digital input signals ********************************
-// 4.2 Run state-machine(s) ****************************************************
-// 4.3 Run controller(s) *******************************************************
-// 4.4 Send output signals to digital and PWM output pins **********************
+//    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)
+ 
+// 5.1 Measure Analog and Digital input signals ********************************
+// 5.2 Run state-machine(s) ****************************************************
+// 5.3 Run controller(s) *******************************************************
+// 5.4 Send output signals to digital and PWM output pins **********************
                     
 
 }                   //Ending of main_loop()
 
 //******************************************************************************
-// 5. Main function ************************************************************
+// 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.
-// 5.1 Initialization **********************************************************
+{ //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"
@@ -63,10 +191,14 @@
                 "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(0);
     
-    loop_ticker.attach(&main_loop,0.1f); // change back to 0.001f //Run the function main_loop 1000 times per second       
+    ticker_mainloop.attach(&main_loop,0.001f); // change back to 0.001f //Run the function main_loop 1000 times per second       
+    ticker_hidscope.attach(&HIDScope, 0.1f); //Ticker for Hidscope, different frequency compared to motors
     
-// 5.2 While loop in main function**********************************************   
+// 6.2 While loop in main function**********************************************   
     while (true) { } //Is not used but has to remain in the code!!
     
 }                  //Ending of Main() Function