Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: kinematics_control kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds ... more
Diff: PIDControl.cpp
- Revision:
- 0:b076b5e36978
- Child:
- 1:c7f0b343df31
diff -r 000000000000 -r b076b5e36978 PIDControl.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDControl.cpp Thu Oct 26 10:53:46 2017 +0000
@@ -0,0 +1,23 @@
+#include "PIDControl.h"
+#include "mbed.h"
+
+PIDControl::PIDControl(void)
+{
+
+}
+
+//Calculate the value after PID control.
+double PIDControl::get(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2) {
+ const double a1 = -4 / (N*Ts+2);
+ const double a2 = -(N*Ts-2) / ( N*Ts+2);
+ const double b0 = (4*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*Kp - 4*Kd*N) / (N*Ts + 2);
+ 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 = error - a1*v1 - a2*v2;
+ double u = b0*v + b1*v1 + b2*v2;
+ v2=v1;
+ v1=v;
+ return u;
+}
+