File content as of revision 1:ce626b794a6c:

//////////////////////////////////////////////////////////////////////////////////
// Company: edinburgh of university
// Engineer: ZEjun DU
// 
// Create Date: 2017/08/20 13:06:52
// Design Name: Inverted Pendulum Balancer
// Module Name: basic PID algorithm
// Tool Versions: “Keil 5” or “Mbed Complie Online”
// Description: this part is to build the basic PID control theroy
//                           
// 
//////////////////////////////////////////////////////////////////////////////////

#include "PID.h"

float I_term_parameter;
////////////////////////////////////////////////////////////
//this part is to initialize the PID struct 
//system can invoke this function in main loop ot built different PID controller
////////////////////////////////////////////////////////////
void pidInit(PidObject* pid, const float desired, const float kp,const float ki, const float kd, const float iLimit, const float dt)
{
    pid->kp = kp;//PID coefficients
    pid->ki = ki;//PID coefficients
    pid->kd = kd;//PID coefficients     
    pid->desired = desired;//PID desired, the default parameter is 0
    pid->prevError = 0;
    pid->error = 0;
    pid->integ = 0;
    pid->deri = 0;
    pid->prevDeri = 0;
    pid->outP = 0;//the output of PID coefficients
    pid->outI = 0;//the output of PID coefficients
    pid->outD = 0;//the output of PID coefficients
    pid->iLimit = iLimit;//it is to limit the output of I term
    pid->iLimitLow = -iLimit;//it is to limit the output of I term
    pid->dt = dt;//shampling period 
}

////////////////////////////////////////////////////////////
//this part is a independent function which calculates the P_term's output
////////////////////////////////////////////////////////////
float get_p(PidObject* pid)
{
    pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy 
    return pid->outP;
}

////////////////////////////////////////////////////////////
//this part is a independent function which calculates the I_term's output, but this part has been changed to adapt veloctiy PID controller
////////////////////////////////////////////////////////////
float get_i(PidObject* pid, float distance)
{
    pid->outI = -pid->ki * distance * 10; //calculate the output of I_term according to PID control theroy 
    if(pid->outI > pid->iLimit)//this part is to limit the output of I_term
    {
        pid->outI = pid->iLimit;
    }
    else if(pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
    {
        pid->outI = pid->iLimitLow;
    }
    return pid->outI;
}

////////////////////////////////////////////////////////////
//this part is a independent function which calculates the D_term's output
////////////////////////////////////////////////////////////
float get_d(PidObject* pid)
{
    pid->deri = (pid->error - pid->prevError) / pid->dt;    //calculate the output of D_term according to PID control theroy 
    pid->deri = pid->prevDeri + ( pid->dt / ( PID_D_TERM_FILTER + pid->dt ) ) * ( pid->deri - pid->prevDeri );//this is a filter
    
    pid->prevDeri = pid->deri;
    pid->prevError = pid->error;
    
    pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy 
    
    return pid->outD;
}

////////////////////////////////////////////////////////////
//this part is a function which calculates the P_term's I_term's and D_term's output together
////////////////////////////////////////////////////////////
float show_I_term(void)// in order to oberse the change of I term
    {
        return I_term_parameter;
    }


////////////////////////////////////////////////////////////
//this part is basic the PID controller for Velocity PID
////////////////////////////////////////////////////////////
float get_pid_vel(PidObject* pid, float distance)
{
    float output;
    
    pid->outP = pid->kp * pid->error;   //calculate the output of P_term according to PID control theroy 
    
    //pid->outI += pid->ki * pid->error * pid->dt;    //I
    pid->outI = -pid->ki * distance * 10;//calculate the output of I_term according to PID control theroy
                                        //this code is the difference between Velocity PID and Angle PID

    if (pid->outI > pid->iLimit)//this part is to limit the output of I_term
    {
        pid->outI = pid->iLimit;
    }   
    else if (pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
    {
        pid->outI = pid->iLimitLow;
    }
    
    I_term_parameter = pid->outI;
    
    pid->deri = (pid->error - pid->prevError) / pid->dt;   //calculate the output of D_term according to PID control theroy 
    pid->prevError = pid->error;
    
    pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy 
    
    output = pid->outP + pid->outI + pid->outD;//calculate the P I D terms' output and returen the OUTPUT 
    
    return output;//returen the total OUTPUT 
}


////////////////////////////////////////////////////////////
//this part is basic the PID controller for Angle PID
////////////////////////////////////////////////////////////
float get_pid(PidObject* pid)
{
    float output;
    
    pid->outP = pid->kp * pid->error;//calculate the output of P_term according to PID control theroy 
    
    pid->outI += pid->ki * pid->error * pid->dt;//calculate the output of I_term according to PID control theroy 
    

    if (pid->outI > pid->iLimit)//this part is to limit the output of I_term
    {
        pid->outI = pid->iLimit;
    }   
    else if (pid->outI < pid->iLimitLow)//this part is to limit the output of I_term
    {
        pid->outI = pid->iLimitLow;
    }//to limit the output of the PID controller
    
    I_term_parameter = pid->outI;// in order to oberse the change of I term
    
    pid->deri = (pid->error - pid->prevError) / pid->dt;//calculate the output of D_term according to PID control theroy 
    pid->prevError = pid->error;
    
    pid->outD = pid->kd * pid->deri;//calculate the output of D_term according to PID control theroy 
    
    output = pid->outP + pid->outI + pid->outD;//calculate the P I D terms' output and returen the OUTPUT 
    
    return output;//returen the total OUTPUT 
}
////////////////////////////////////////////////////////////
//this part is to reset the PID struct 
//if the I_term is so lager, this function can be used to reset the I_term!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//otherwise, the balancer would be unstable
////////////////////////////////////////////////////////////
void pidReset(PidObject* pid)
{
  pid->error = 0;//reset the error to 0
  pid->prevError = 0;//reset the preverror to 0
  pid->integ = 0;//reset the integ to 0
  pid->deri = 0;//reset the deri to 0
}

////////////////////////////////////////////////////////////
//this part is to set the PID error manually  
////////////////////////////////////////////////////////////
void pidSetError(PidObject* pid, const float error)
{
  pid->error = error;//this part is to set the PID error manually  
}
////////////////////////////////////////////////////////////
//this part is to set the PID desire manually  
////////////////////////////////////////////////////////////
void pidSetDesired(PidObject* pid, const float desired)
{
  pid->desired = desired;//this part is to set the PID error manually  
}