Peter Knoben
PID
Dependents: kinematics_control kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds ... more
Revision 0:b076b5e36978, committed 2017-10-26
- Comitter:
- peterknoben
- Date:
- Thu Oct 26 10:53:46 2017 +0000
- Child:
- 1:c7f0b343df31
- Commit message:
- PID;
Changed in this revision
| PIDControl.cpp | Show annotated file Show diff for this revision Revisions of this file |
| PIDControl.h | Show annotated file Show diff for this revision Revisions of this file |
--- /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;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDControl.h Thu Oct 26 10:53:46 2017 +0000
@@ -0,0 +1,21 @@
+#ifndef _PIDCONTROL_H_INCLUDED_
+#define _PIDCONTROL_H_INCLUDED_
+
+#include "mbed.h"
+
+class PIDControl
+{
+public:
+ /**
+ *Constructor
+ */
+ PIDControl(void);
+
+
+ // Calculate the PID control value
+ double get(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2);
+
+private:
+
+};
+#endif
\ No newline at end of file