float PIDcal(float setpoint,float actual_position)  
{  
    static float pre_error = 0;         // 定义前一次误差  
    static float integral = 0;          // 定义积分项累加值  
    float error;                        // 定义最新误差  
    float derivative;                   // 定义微分项  
    float output;                       // 定义输出  
  
    // 计算误差  
    error = setpoint - actual_position;  
  
    // 死区设置，在误差极小的情况下不做积分处理，保证稳定性  
    if(my_abs(error) > epsilon)  
    {  
       integral = integral + error*dt;   // 积分项,做完壁障之后，积分清零  
    }  
  
    derivative = (error - pre_error)/dt; // 微分项  
  
    output = Kp*error + Ki*integral + Kd*derivative; // 各项乘以系数得到PID输出  
  
    // 输出限幅，确保输出不会太夸张  
    if(output > MAX)  
    {  
        output = MAX;  
    }  
    else if(output < MIN)  
    {  
        output = MIN;  
    }  
  
        // 更新误差  
        pre_error = error;  
  
        return output;  
}