Diff: main.cpp

Revision:

29:d8e51f4cf080

Parent:

28:aec0d9bdb949

Child:

30:c4a3e868ef04

--- a/main.cpp	Tue Oct 30 09:17:31 2018 +0000
+++ b/main.cpp	Tue Oct 30 09:51:51 2018 +0000
@@ -11,10 +11,9 @@
 AnalogIn    pot2(A2);
 InterruptIn button1(D0);
 InterruptIn button2(D1);
-InterruptIn emergencybutton(SW2);  /* This is not yet implemented! 
-The button SW2 on the K64F is the emergency button: if you press this, 
-everything will abort as soon as possible
-*/
+InterruptIn emergencybutton(SW2);   
+//The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
+
 
 DigitalIn   pin8(D8);     // Encoder 1 B
 DigitalIn   pin9(D9);     // Encoder 1 A
@@ -50,6 +49,7 @@
 const double    dt = 0.5;
 const double    Kp = 17.5;    // given value is 17.5
 const double    Ki = 1.02;  // given value is 1.02
+const double    Kd = 23.2;      // given value is 23.2
 //const double    Ts = 0.0025; // Sample time in seconds
 
 // Functions
@@ -97,7 +97,7 @@
     }
 
     double  PIcontroller(double yref,double CurAngle)
-    {   double  error = yref - CurAngle;    
+    {   double  error = ErrorCalc(yref,CurAngle);    
         static double   error_integral = 0; 
         // Proportional part:
         double  u_k = Kp * error;
@@ -108,16 +108,34 @@
         return  u_k + u_i;
     }
     
+    double PIDcontroller(double yref,double CurAngle)
+    {   double  error = ErrorCalc(yref,CurAngle);
+        static double error_integral = 0;
+        static double error_prev = error; // 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 * error;
+        // Integral part
+        error_integral = error_integral + error * dt;
+        double u_i = Ki * error_integral;
+        // Derivative part
+        double error_derivative = (error - error_prev)/dt;
+        double filtered_error_derivative = LowPassFilter.step(error_derivative);
+        double u_d = Kd * filtered_error_derivative;
+        error_prev = error;
+        // Sum all parts and return it
+        return u_k + u_i + u_d;
+    }
 
 void    turn1()    // main function, all below functions with an extra tab are called
 {   
     double  refvalue1 = pi/4;
     int     counts1 = Counts1input();
     float   angle1 = CurrentAngle(counts1);
-    double  PIcontr = PIcontroller(refvalue1,angle1);
+    double  PIDcontr = PIDcontroller(refvalue1,angle1);
     double  error = ErrorCalc(refvalue1,angle1);
      
-    pin6 = fabs(PIcontr)/pi;
+    pin6 = fabs(PIDcontr)/pi;
     if  (error > 0)
     {   pin7 = true;
     }
@@ -146,14 +164,13 @@
 }
 */
 
-/*
+
 double ActualPosition(int counts, int offsetcounts)
 {   // After calibration, the counts are returned to 0;
-    double MotorPos = - (counts - offsetcounts) / dCountsRad;
+    double MotorPos = - (counts - offsetcounts) / fCountsRad;
     // minus sign to correct for direction convention
     return MotorPos;
 }
-*/
 
 void Emergency()
 {   // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on