Diff: PID/PID_yaw.cpp

Revision:

0:b0f9c5ac0305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_yaw.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,33 @@
+#include "PID_yaw.h"
+
+RawSerial pc_pid_yaw(USBTX, USBRX);
+ 
+PID_YAW_Class::PID_YAW_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){
+ 
+        Angle_Kp=AngleKp;
+        Angle_Ki=AngleKi;
+        Rate_Kp=RateKp;
+        Rate_Kd=RateKd;
+}
+ 
+float PID_YAW_Class::update_yaw(float Setpoint, float CurrentPosition, float Rate, float dt){
+      
+      AngleError = 0;
+      AngleError = Setpoint - CurrentPosition;
+      //pc_pid.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError);
+      
+      Angle_I +=AngleError*dt;
+      
+      if(Angle_I>1023.0f) Angle_I=1023.0f;
+      else if(Angle_I<-1023.0f) Angle_I= -1023.0f;
+      
+      RateError = (AngleError*Angle_Kp)- Rate;
+      
+      Rate_D = (RateError-PreviousError)/dt;
+      
+      Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd));
+      
+      PreviousError = RateError;
+      
+      return Output;
+}
\ No newline at end of file