Diff: main.cpp

Revision:

5:7007230db09c

Parent:

4:0251290a2fc0

Child:

7:d63ec7fe96ae

--- a/main.cpp	Fri Oct 19 12:02:58 2018 +0000
+++ b/main.cpp	Fri Oct 19 13:03:56 2018 +0000
@@ -7,6 +7,7 @@
 //Tickers
 Ticker TickerMeasureAndControl;
 Ticker TickerPrintToScreen;
+
 //Communication
 MODSERIAL pc(USBTX,USBRX);
 QEI Encoder(D10,D11,NC,32);
@@ -58,13 +59,16 @@
 
 
 ///The controller
-double PI_Controller(double Error)
+double PID_Controller(double Error)
 {
 
    double Ts = 0.01; //Sampling time 100 Hz
    double Kp = 2; // proportional gain
-   double Ki = 2*Potmeter2.read(); //integral gain ranging from 0-2.
+   double Ki = 0.85;
+   double Kd = 20*Potmeter2.read(); //integral gain ranging from 0-20.
    static double ErrorIntegral = 0;
+   static double ErrorPrevious = Error;
+   static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    //Proportional part:
    double u_k = Kp * Error;
@@ -72,7 +76,15 @@
    //Integral part:
    ErrorIntegral = ErrorIntegral + Error*Ts;
    double u_i = Ki * ErrorIntegral;
-   return u_k + u_i; //This will become the MotorValue
+   
+   //Derivative part:
+   double ErrorDerivative = (Error - ErrorPrevious)/Ts;
+   double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
+   double u_d = Kd * FilteredErrorDerivative;
+   ErrorPrevious = Error;
+   
+   // sum of parts and return it
+   return u_k + u_i + u_d; //This will become the MotorValue
 }
 
 //Ticker function set motorvalues
@@ -103,7 +115,7 @@
 {
     double PositionRef = GetReferencePosition();
     double PositionMotor = GetActualPosition();
-    double MotorValue = PI_Controller(PositionRef - PositionMotor); // input is error
+    double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
     SetMotor(MotorValue);
     
     //for printing on screen
@@ -135,8 +147,9 @@
         if(PrintFlag) // if-statement for printing every second four times to screen
         {
             double KpPrint = 2;
-            double KiPrint = 2*Potmeter2.read();
-            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f and Ki = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint);
+            double KiPrint = 0.86;
+            double KdPrint = 20*Potmeter2.read();
+            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint);
             PrintFlag = false;
         }
     }