Diff: PID_Yaw.cpp

Revision:

0:e63996fd7d3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Yaw.cpp	Fri Jul 03 11:16:02 2015 +0000
@@ -0,0 +1,77 @@
+#ifndef _MBED_H
+#define _MBED_H
+#include "mbed.h"
+#endif
+#include "PID_Yaw.h"
+#define Error_operation_limits 0.25
+#define sigmoid_fun(h) (2/(1+exp(-h)))-1
+#define NULLAREA 0.015
+//constructor
+PID_Yaw::PID_Yaw(float KP,float KI,float KD,float KDD ) /*, debugSerial(USBTX, USBRX)*/
+{
+    //debugSerial.baud(D_BAUDRATE);
+    Out_PWM=0.001;
+    Out_PWM2=0.001;
+    _KP=KP;
+    _KD=KD;
+    _KI=KI;
+    _KDD=KDD;
+    h=0;
+    ErrorDD=0;
+    ErrorD=0;
+    //debugSerial.printf("\r\n\r\n\r\nhere\r\n");
+    PowerSetPoint=0.001;
+}
+void PID_Yaw::PWM_cal(float Theta_req,float Theta_curr,float req_power_SP,float * xPlanePWM,float *yPlanePWM )
+{
+    PowerSetPoint=req_power_SP;
+  
+    //debugSerial.printf("\r\nthetareq:%f thetacurr:%f\r\n",Theta_req,Theta_curr);
+
+    Check_IN_Limits(&Theta_req);
+    Pulse_Error_cal(Theta_req,Theta_curr);
+
+    Out_PWM= PowerSetPoint-h;
+    Out_PWM2= PowerSetPoint+h;
+    Check_OUT_Limits(&Out_PWM);
+    Check_OUT_Limits(&Out_PWM2);
+    *xPlanePWM=Out_PWM;
+    *yPlanePWM=Out_PWM2;
+}
+//error calculating and out the relative output to the relative error.
+void PID_Yaw::Pulse_Error_cal(float RPM_req,float RPM_curr)
+{
+    //debugSerial.printf("req:%f curr:%f",RPM_req,RPM_curr);
+    Error=RPM_req-RPM_curr;
+        ErrorI=ErrorI+Error;
+        ErrorD=Error-ErrorOLD;
+        ErrorOLD=Error;
+        ErrorDD=ErrorD-ErrorDOLD;
+        ErrorDOLD=ErrorD;
+        h=_KP*Error+_KI*ErrorI+_KD*ErrorD+_KDD*ErrorDD;
+         
+}
+//setting Output Limits.
+void PID_Yaw::Set_OUT_Limits(float LowerLimt,float UpperLimit)
+{
+    O_Upper_Limit=UpperLimit;
+    O_Lower_Limit=LowerLimt;
+}
+//setting Input Limits.
+void PID_Yaw::Set_IN_Limits(float LowerLimit,float UpperLimit)
+{
+    I_Upper_Limit=UpperLimit;
+    I_Lower_Limit=LowerLimit;
+}
+//checking Input Limits in the Realtime operation.
+void PID_Yaw::Check_OUT_Limits(float *RPMSetPoint)
+{
+    if(*RPMSetPoint>O_Upper_Limit)*RPMSetPoint=O_Upper_Limit;
+    if(*RPMSetPoint<O_Lower_Limit)*RPMSetPoint=O_Lower_Limit;
+}
+//checking Output Limits in Realtime operation.
+void PID_Yaw::Check_IN_Limits(float *O_PWM)
+{
+    if(*O_PWM>I_Upper_Limit)*O_PWM=I_Upper_Limit;
+    if(*O_PWM<I_Lower_Limit)*O_PWM=I_Lower_Limit;
+}