File content as of revision 0:e63996fd7d3e:

#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;
}