Diff: PID.cpp

Revision:

0:2ac3074cfdb3

diff -r 000000000000 -r 2ac3074cfdb3 PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Tue Aug 27 09:32:17 2013 +0000
@@ -0,0 +1,53 @@
+/* Copyright (c) <2013> MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
+ * and associated documentation files (the "Software"), to deal in the Software without restriction, 
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or 
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Nested PID for Quadcopter Control
+ *
+ 
+ */
+ 
+#include "PID.h"
+
+PIDClass::PIDClass(float AngleKp,float AngleKi, float RateKp, float RateKd){
+
+        Angle_Kp=AngleKp;
+        Angle_Ki=AngleKi;
+        Rate_Kp=RateKp;
+        Rate_Kd=RateKd;
+}
+
+int PIDClass::update(float Setpoint, float CurrentPosition, float Rate, float dt){
+      
+      AngleError = Setpoint - CurrentPosition;
+      
+      Angle_I +=AngleError*dt;
+      
+      if(Angle_I>100.0f) Angle_I=100.0f;
+      else if(Angle_I<-100.0f) Angle_I= -100.0f;
+      
+      RateError = (AngleError*Angle_Kp)- Rate;
+      
+      Rate_D = (RateError-PreviousError)/dt;
+      
+      Output = (int)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd));
+      
+      PreviousError = RateError;
+      
+      return Output;
+}
\ No newline at end of file