#include "pid.h"
#include "mathfunc.h"

//PIDṹ
PID_Structure motor_l,motor_r,dir,target_speed_pid;



void PID_Init(PID_Structure* pid)
{
   pid->p=0;
   pid->i=0;
   pid->d=0;
   pid->error0=0;
   pid->error00=0;
   pid->out=0;
   pid->integrator=0;
   pid->i_max=0;
   pid->out_max=0;
   pid->out_min=0;
}

void PID_Config(PID_Structure* pid, float p, float i, float d, float i_max, float out_max, float out_min)
{
    pid->p=p;
    pid->i=i;
    pid->d=d;
    pid->i_max=i_max;
    pid->out_max=out_max;
    pid->out_min=out_min;
}

float PID_Position_Controller(PID_Structure* pid,float error)
{
    float p_out,i_out,d_out;
    pid->integrator += error;   //»
    Constrain_int16(pid->integrator,-pid->i_max,pid->i_max);    //޷
    p_out = pid->p * error+dynamic_p_coef*error*error*error/6000;
    i_out = pid->i * pid->integrator;
    d_out = pid->d * (error - pid->error0);
    pid->error00=pid->error0;
    pid->error0 = error;    //
    pid->out=p_out+i_out+d_out;    //pid
    pid->out=Constrain_float(pid->out,pid->out_max,pid->out_min);    //޷
    return pid->out;
}

float PID_Incremental_Controller(PID_Structure* pid,float error)
{
    float p_out,i_out,d_out;
    p_out = pid->p * (error - pid->error0);
    i_out = pid->i * error;
    d_out = pid->d * (error - (pid->error0)/2 + pid->error00);
    pid->error00=pid->error0;
    pid->error0=error;      //
    pid->out+=p_out + i_out + d_out;
    pid->out=Constrain_float(pid->out,pid->out_max,pid->out_min);    //޷
    return pid->out;
}
