Diff: PID_Alt.h

Revision:

0:e63996fd7d3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Alt.h	Fri Jul 03 11:16:02 2015 +0000
@@ -0,0 +1,149 @@
+#ifndef _PID_Alt
+#define _PID_Alt
+#include "SHARPIR.h"
+#include "mbed.h"
+#define D_BAUDRATE            115200
+
+class PID_Alt{
+    public:
+    PID_Alt(int Mnum,float KP,float KI,float KD,float KDD,PinName AnalogPort);
+    float findNominalVal(float altitude,float CosR,float CosP,char Calibrated);
+    void Set_OUT_Limits(float LowerLimit,float UpperLimit);//setting Output Limits.
+    void Set_IN_Limits(float LowerLimit,float UpperLimit);//setting Input Limits.
+    void Check_OUT_Limits(float *RPMSetPoint);//checking Input Limits in the Realtime operation.
+    void Check_IN_Limits(float *O_PWM);//checking Output Limits in Realtime operation.
+    float Get_IR_Readings();
+    float Get_OUT_POWER_SP();
+    float Get_Current_IR();
+    float GetErrorV();
+    float GetErrorVD();
+    float GetErrorVDD();
+    float GetErrorVDOLD();
+    float Get_reqestedAlt();
+    
+    float MeanIR(int num);
+    private:
+    SHARPIR _IR;
+    float PowerSetPoint;//,RPMSetPoint;
+    float Out_PWM,Out_PWM2;
+    float O_Upper_Limit,O_Lower_Limit;
+    float I_Upper_Limit,I_Lower_Limit;
+    float ErrorV,ErrorI,ErrorVD,ErrorVold,ErrorVDD,ErrorVDOLD,altitudeOld,nominalVal2;
+    float V_req,V_curr,CURRENT_Alt;
+    float h;
+    float EVDOLDGetter,reqestedAlt;
+    float _KP,_KD,_KI,_KDD;
+    int Meannum;
+    Serial debugSerial;
+
+    };
+PID_Alt::PID_Alt(int Mnum,float KP,float KI,float KD,float KDD,PinName AnalogPort):_IR(AnalogPort), debugSerial(USBTX, USBRX){
+    _KP=KP;
+    _KD=KD;
+    _KI=KI;
+    _KDD=KDD;
+    Meannum=Mnum;
+    V_req=0;
+    V_curr=0;
+    ErrorV=0;
+    ErrorI=0;
+    ErrorVD=0;
+    ErrorVold=0;
+    ErrorVDD=0;
+    ErrorVDOLD=0;
+    altitudeOld=0;
+    nominalVal2=0.00115;
+    debugSerial.baud(D_BAUDRATE);
+ 
+    
+    }
+float PID_Alt::findNominalVal(float altitude,float CosR,float CosP,char Calibrated){
+        float curr_alt=0;
+        Check_IN_Limits(&altitude);
+        reqestedAlt=altitude;
+        curr_alt=MeanIR(Meannum)*CosR*CosP;
+        if(curr_alt<=9){
+                curr_alt=150;
+            }
+        CURRENT_Alt=curr_alt;
+        Check_IN_Limits(&curr_alt);
+        V_req=altitude-curr_alt;
+        V_curr=curr_alt-altitudeOld;
+        altitudeOld=curr_alt;
+        ErrorV= V_curr-V_req;
+        ErrorVD= ErrorV-ErrorVold;
+        ErrorVDD=ErrorVD-ErrorVDOLD;
+        EVDOLDGetter=ErrorVDOLD;
+        debugSerial.printf("E:%f ED:%f EDD:%f EDOld:%f\r\n",ErrorV,ErrorVD,ErrorVDD,ErrorVDOLD);
+        if(Calibrated)
+        nominalVal2=nominalVal2-_KP*ErrorV-_KD*ErrorVD-_KDD*ErrorVDD;
+        ErrorVold=ErrorV;
+        ErrorVDOLD=ErrorVD;
+        
+        //debugSerial.printf("error:%f aError:%f alt:%f NV:%f Real:%f\r\n",(MeanIR(Meannum)-altitude), alpha*(MeanIR(Meannum)-altitude),altitude,nominalVal2,MeanIR(Meannum));
+        Check_OUT_Limits(&nominalVal2);
+        return nominalVal2;
+    }
+float PID_Alt::MeanIR(int num)
+{
+    float MeanReading=0;
+    for(int i=0;i<num;i++)MeanReading+=Get_IR_Readings();
+    return MeanReading/num;    
+}
+float PID_Alt::Get_reqestedAlt(){
+    return reqestedAlt;
+    }
+float PID_Alt::GetErrorVDOLD(){
+    return EVDOLDGetter;
+    }
+float PID_Alt::GetErrorV(){
+    return ErrorV;
+    }
+float PID_Alt::GetErrorVD(){
+    return ErrorVD;
+    }
+float PID_Alt::GetErrorVDD(){
+    return ErrorVDD;
+    }
+float PID_Alt::Get_OUT_POWER_SP()
+{
+    return nominalVal2;
+}
+float PID_Alt::Get_IR_Readings()
+{
+    return _IR.cm();
+}
+float PID_Alt::Get_Current_IR()
+{
+    return CURRENT_Alt;
+}
+//setting Output Limits.
+void PID_Alt::Set_OUT_Limits(float LowerLimt,float UpperLimit)
+{
+    O_Upper_Limit=UpperLimit;
+    O_Lower_Limit=LowerLimt;
+    //initializations
+   // power=O_Lower_Limit;
+   // prev_power=O_Lower_Limit;
+}
+//setting Input Limits.
+void PID_Alt::Set_IN_Limits(float LowerLimit,float UpperLimit)
+{
+    I_Upper_Limit=UpperLimit;
+    I_Lower_Limit=LowerLimit;
+   // prev_alt=I_Lower_Limit;
+}
+//checking Input Limits in the Realtime operation.
+void PID_Alt::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_Alt::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;
+}
+
+#endif
\ No newline at end of file