NOT FINISHED YET!!!
My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
Dependencies:
mbed
MODI2C
Diff: PID/PID.cpp
- Revision:
- 29:8b7362a2ee14
- Child:
- 33:fd98776b6cc7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp Sat Dec 15 08:42:36 2012 +0000
@@ -0,0 +1,45 @@
+#include "PID.h"
+
+PID::PID(float P, float I, float D, float Integral_Max)
+{
+ Integral = 0;
+ LastTime = 0;
+ SetPoint = 0;
+ Integrate = true;
+ PID::P = P;
+ PID::I = I;
+ PID::D = D;
+ PID::Integral_Max = Integral_Max;
+ dtTimer.start();
+}
+
+float PID::compute(float SetPoint, float ProcessValue)
+{
+ // meassure dt
+ float dt = dtTimer.read() - LastTime; // time in us since last loop
+ LastTime = dtTimer.read(); // set new time for next measurement
+
+ // Proportional
+ float Error = ProcessValue - SetPoint;
+
+ // Integral
+ if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
+ Integral = 0;
+ else if (abs(Integral + Error) <= Integral_Max)
+ Integral += Error * dt;
+
+ // Derivative
+ float Derivative = (Error - PreviousError) / dt;
+
+ // Final Formula
+ float Result = P * Error + I * Integral + D * Derivative;
+
+ PreviousError = Error;
+
+ return Result;
+}
+
+void PID::setIntegrate(bool Integrate)
+{
+ PID::Integrate = Integrate;
+}
\ No newline at end of file
Matthias Grob
