Diff: main.cpp

Revision:

24:b9dd6cf5c366

Parent:

23:c027e5c57cc8

Child:

25:c719346df3cd

diff -r c027e5c57cc8 -r b9dd6cf5c366 main.cpp
--- a/main.cpp	Tue Oct 30 09:37:29 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:02:26 2018 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "BiQuad.h"
 
 AnalogIn potmetervalue1     (A1);
 AnalogIn potmetervalue2     (A2);
@@ -19,7 +20,17 @@
 
 
 //Global variables
-int encoder = 0;
+int encoder = 0;                //Start value encoder
+int T = 0.002f;                 //Ticker period
+
+double Kp = 17.5;
+double Ki = 1.02;
+double Kd = 23.2;
+
+double err = 0;
+
+//Tickers
+Ticker function_tick;
    
 
 //Functions
@@ -71,6 +82,43 @@
     }
 }
 
+void encoder_count()
+{
+    encoderA.rise(&encoderA_rise);
+    encoderA.fall(&encoderA_fall);
+    encoderB.rise(&encoderB_rise);
+    encoderB.fall(&encoderB_fall);
+}
+
+double PID_controller()
+{
+  static double err_integral = 0;
+  static double err_prev = err; // initialization with this value only done once!
+  
+  static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+  // Proportional part:
+  double u_k = Kp * err;
+
+  // Integral part
+  err_integral = err_integral + err * T;
+  double u_i = Ki * err_integral;
+
+  // Derivative part
+  double err_derivative = (err - err_prev)/T;
+  double filtered_err_derivative = LowPassFilter.step(err_derivative);
+  double u_d = Kd * filtered_err_derivative;
+  err_prev = err;
+
+  // Sum all parts and return it
+  return u_k + u_i + u_d;   
+}
+
+void call_all_functions()
+{
+    PID_controller();
+    encoder_count();
+}
 
 // Main function start.
 
@@ -80,27 +128,14 @@
     pc.printf("hello\n\r");
     pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 
 
-    encoderA.rise(&encoderA_rise);
-    encoderA.fall(&encoderA_fall);
-    encoderB.rise(&encoderB_rise);
-    encoderB.fall(&encoderB_fall);
+    function_tick.attach(&call_all_functions,T);
+    
     
     while (true)
     {
-          
-          float u1 = potmetervalue1;
-          float u2 = potmetervalue2;
-          
-          float m1 = ((u1*2.0f)-1.0f);
-          float m2 = ((u2*2.0f)-1.0f);
         
-        pwmpin1 = fabs(m1*0.6f)+0.4f;     //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.        
-        directionpin1.write(m1>0);        //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. 
-        wait(0.01f);                   //zodat de code niet oneindig doorgaat.
-        pwmpin2 = fabs(m2*0.6f)+0.4f;    
-        directionpin2.write(m2>0);   
+
                 
-        pc.printf("Encoder count: %i \n\r",encoder);   //We moeten de encoder counts nog omzetten naar radialen of graden? 
     }
 }