File content as of revision 0:fb4269aa5fb4:

#include "pid.h"


void PID_Struct_Init(PID_Struct *pid, float Kp, float Kd, float Ki)
{
    pid->kp = Kp;
    pid->kd = Kd;
    pid->ki = Ki;
    pid->diff[0] = 0;
    pid->diff[1] = 0;
    pid->i_term = 0;
    
    pid->is_initialized = 1;
    pid->is_enabled = PID_CONTROL_DISABLE;
}

void PID_Enable_Control(PID_Struct *pid)
{
    pid->is_enabled = PID_CONTROL_ENABLE;
}

void PID_Disable_Control(PID_Struct *pid)
{
    pid->is_enabled = PID_CONTROL_DISABLE;
}

uint32_t PID_Check_Status(PID_Struct *pid)
{
    return pid->is_enabled;
}

float PID_Control(PID_Struct *pid, float new_input, float target)
{
    pid->diff[1] = -pid->diff[0];               //diff[1] = -e(k-1)
    pid->diff[0] = target - new_input;      //e(k) = target - input
    pid->diff[1] += pid->diff[0];             //diff[1] = diff[1] + e(k)
                                                        //Eventually, diff[1] = e(k) - e(k-1)
    pid->i_term += pid->ki * pid->diff[0];
    
    return (pid->kp * pid->diff[0] + pid->kd * pid->diff[1] + pid->i_term);
}

/*---------------These functions below are faster than PID_Control()----------------*/
float PD_Control(PID_Struct *pid, float new_input, float target)
{
    pid->diff[1] = -pid->diff[0];               //diff[1] = -e(k-1)
    pid->diff[0] = target - new_input;      //e(k) = target - input
    pid->diff[1] += pid->diff[0];             //diff[1] = diff[1] + e(k)
                                                        //Eventually, diff[1] = e(k) - e(k-1)
    return (pid->kp * pid->diff[0] + pid->kd * pid->diff[1]);
}

float PI_Control(PID_Struct *pid, float new_input, float target)
{
    pid->diff[0] = target - new_input;      //e(k) = target - input
    pid->i_term += pid->ki * pid->diff[0];
    
    return (pid->kp * pid->diff[0] + pid->i_term);
}
/*---------------------------------------------------------------------------*/