Diff: main.cpp

Revision:

4:0251290a2fc0

Parent:

3:f1fc5216f6a5

Child:

5:7007230db09c

Child:

6:10ac8c7cac26

--- a/main.cpp	Fri Oct 19 11:04:43 2018 +0000
+++ b/main.cpp	Fri Oct 19 12:02:58 2018 +0000
@@ -2,6 +2,7 @@
 #include "math.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
+#include "BiQuad.h"
 
 //Tickers
 Ticker TickerMeasureAndControl;
@@ -23,6 +24,7 @@
 volatile float PosRefPrint; // for printing value on screen
 volatile float PosMotorPrint; // for printing value on screen
 volatile float ErrorPrint;
+
 //-----------------------------------------------------------------------------
 //The double-functions
 
@@ -56,13 +58,21 @@
 
 
 ///The controller
-double P_Controller(double Error)
+double PI_Controller(double Error)
 {
-   double Kp = 35*Potmeter2.read(); // 35 is just a try
+
+   double Ts = 0.01; //Sampling time 100 Hz
+   double Kp = 2; // proportional gain
+   double Ki = 2*Potmeter2.read(); //integral gain ranging from 0-2.
+   static double ErrorIntegral = 0;
    
+   //Proportional part:
    double u_k = Kp * Error;
    
-   return u_k; //This will become the MotorValue
+   //Integral part:
+   ErrorIntegral = ErrorIntegral + Error*Ts;
+   double u_i = Ki * ErrorIntegral;
+   return u_k + u_i; //This will become the MotorValue
 }
 
 //Ticker function set motorvalues
@@ -93,13 +103,13 @@
 {
     double PositionRef = GetReferencePosition();
     double PositionMotor = GetActualPosition();
-    double MotorValue = P_Controller(PositionRef - PositionMotor); // input is error
+    double MotorValue = PI_Controller(PositionRef - PositionMotor); // input is error
     SetMotor(MotorValue);
     
     //for printing on screen
     PosRefPrint = PositionRef;
     PosMotorPrint = PositionMotor;
-    ErrorPrint = MotorValue;
+    ErrorPrint = PositionRef - PositionMotor;
     
 }
 
@@ -124,8 +134,9 @@
     {
         if(PrintFlag) // if-statement for printing every second four times to screen
         {
-            double KpPrint = 35*Potmeter2.read();
-            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f and Kp = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint);
+            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);
             PrintFlag = false;
         }
     }