File content as of revision 0:e63996fd7d3e:

#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