Diff: main.cpp

Revision:

12:0cf4e70f6b8e

Parent:

11:eda4fbf91948

Child:

13:f92e918af729

diff -r eda4fbf91948 -r 0cf4e70f6b8e main.cpp
--- a/main.cpp	Mon Oct 24 09:51:54 2016 +0000
+++ b/main.cpp	Mon Oct 24 12:34:45 2016 +0000
@@ -2,6 +2,7 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "math.h"
+#include "BiQuad.h"
 //#include "HIDScope.h"
 
 //*****************Defining ports********************
@@ -34,13 +35,18 @@
 const int velocityChannel = 0;
 const double transmissionShoulder =94.4/40.2;
 const double transmissionElbow = 1.0;
-const double motor1_Kp = 1;
-const double motor1_Ki = 0.1;
-const double CONTROLLER_TS = 0.01;
-double m1_error_int=0;
+// controller constants 
+const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100;
+double m1_v1 = 0, m1_v2 = 0; // Memory variables
+const double m1_Ts = 0.01; // Controller sample time
+double v1 = 0;
+double v2 = 0;
+// position variable
 volatile double radians;
-volatile double proportionalErrormotor1;
+//plant variable
 volatile double motor1;
+
+
 //*****************Angles Arms***********************
 
 double O1=1.7633;
@@ -57,12 +63,21 @@
 double B6=-0.8994;
 
 //**********functions******************************
-double PI(volatile double error, const double Kp, const double Ki, double Ts, double &error_int)
-{
-    error_int += Ts*error;
-    return Kp * error + Ki*error_int;
-    
-    }
+ double PID( double err, const double Kp, const double Ki, const double Kd,
+ const double Ts, const double N, double &v1, double &v2 ) {
+ // These variables are only calculated once!
+ const double a1 = (-4.0/(N*Ts+2));
+ const double a2 = -(N*Ts-2)/(N*Ts+2);
+ const double b0 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+ const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
+ const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+
+ double v = err - a1*v1 - a2*v2;
+ double u = b0*v + b1*v1 + b2*v2;
+ v2 = v1; v1 = v;
+ return u;
+ }
+
 
 void getAngPosition() 
 {   
@@ -78,9 +93,9 @@
  {
  double reference = 2*pi;
  volatile double error1 = reference - radians;
-motor1 = PI(error1,motor1_Kp,motor1_Ki, m1_ts, m1_error_int);
-pc.printf("%d\n\r",motor1);
-// scope.set(velocityChannel,motor1);
+ motor1 = PID( error1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, v1, v2 );
+//pc.printf("%d\n\r",motor1);
+//scope.set(velocityChannel,motor1);
 //scope.send();
  }