Diff: main.cpp

Revision:

17:4a0912c93771

Parent:

15:c2cfab737a4c

Child:

18:e522dfbab4c6

Child:

19:1353ba4d94db

diff -r c2cfab737a4c -r 4a0912c93771 main.cpp
--- a/main.cpp	Fri Oct 19 09:30:44 2018 +0000
+++ b/main.cpp	Fri Oct 19 12:08:31 2018 +0000
@@ -14,17 +14,15 @@
 //Tickers
 Ticker MeasureControl;
 Ticker print;
-/*Ticker MakeMotorRotate;*/ //Waar zal deze ticker voor dienen?
 
 //Global variables
 volatile double measuredPosition = 0.0;
 volatile double referencePosition = 0.0; 
 volatile double motorValue= 0.01;
-volatile double Kp = 0.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
+volatile double Kp = 5.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
 volatile double Ki = 1.0; //dit moeten we bepalen met een plot bijvoorbeeld
+volatile double Kd = 0.0;
 volatile double Ts = 0.01;
-volatile double Error_integral = 0.0;
-
 
 //------------------------------------------------------------------------------
 // Functions
@@ -45,14 +43,22 @@
 
 double FeedbackControl(double Error)
 {  
+    static double Error_integral = 0;
+    static double Error_prev = Error;
+    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
     // Proportional part: 
-    Kp = 20*potMeter2.read(); //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
+    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
     double u_k = Kp * Error;
     // Integral part:
     Error_integral = Error_integral + Error * Ts; 
     double u_i = Ki * Error_integral;
+    // Derivative part
+    double Error_derivative = (Error - Error_prev)/Ts;
+    Kd = 20*potMeter2.read();
+    double u_d = Kd * Error_derivative;
+    Error_prev = Error;
     // Sum all parts and return it 
-    return u_k + u_i; //motorValue
+    return u_k + u_i + u_d; //motorValue
  }
  
 void SetMotor1(double motorValue)
@@ -99,7 +105,7 @@
     pc.printf("Motorvalue/Error %f \r\n", motorValue);
     pc.printf("Proportional gain %f \r\n", Kp);
     pc.printf("Integral gain %f \r\n", Ki);
-    pc.printf("Error integral %f \r\n", Error_integral);
+    pc.printf("Derivative gain %f \r\n", Kd);
 }
 //-----------------------------------------------------------------------------
 int main()