PID控制示例C编程

typedef struct PID_zhs
{
    double P,I,D;//P、I、D系数
    float curError;//当前误差
    float lastError;//上次误差
    float preError;//上上次误差
    double sumError;//积分项
    float dError;//微分项
}PID_zhs;

//初始化结构体数据

void pid_zhs_init()
{
    pid_v_zhs.P=0.0;//比例系数
    pid_v_zhs.I=0.0;//积分系数
    pid_v_zhs.D=0.0;//微分系数
    pid_v_zhs.curError=0.0;//当前误差
    pid_v_zhs.lastError=0.0;//上次误差
    pid_v_zhs.preError=0.0;//上上次误差
    pid_v_zhs.sumError=0.0;//积分项
    pid_v_zhs.dError=0.0;//微分项
}

void SpeedTitl_init()//云台动态值清空
{
    int i;

    for(i=2;i<7;i++)
    {
        SpeedTitl[i]=0x00;
    }
}
long Speed=0;

void SpeedOut(void)
{
    SpeedTitl_init();
    float angle = Q_ANGLE.X;//Q_ANGLE.X为全局变量角度 由互补滤波函数求得 可在我相关博客中查阅

    //位置式
    pid_v_zhs.curError = angle;//比例项
    pid_v_zhs.sumError+=pid_v_zhs.curError;//积分项
    pid_v_zhs.dError = pid_v_zhs.curError - pid_v_zhs.lastError;//微分项
    Speed = pid_v_zhs.curError * pid_v_zhs.P + pid_v_zhs.sumError * pid_v_zhs.I + pid_v_zhs.dError * pid_v_zhs.D;
    pid_v_zhs.preError=pid_v_zhs.lastError;
    pid_v_zhs.lastError=pid_v_zhs.curError;

    if(Speed < 0)
    {
        Speed=-Speed;
        //俯仰向下控制：FF address 00 10 00 Vspeed checksum
        SpeedTitl[3]=0x10;
    }
    //如果云台前倾 摄像头则应该向上运动
    //向前倾时Q_ANGLE.X为正值
    else if(Speed > 0)
    {
        //俯仰向上控制：FF address 00 08 00 Vspeed checksum
        SpeedTitl[3]=0x08;
    }
    //云台水平 摄像头不需运动
    else    ;

    SpeedTitl[5]=Speed;
    SpeedTitl[6]=SpeedTitl[3]+SpeedTitl[4]+SpeedTitl[5]+1;

    if(Speed>0)
    {
        Uart1_Send_Buf(SpeedTitl,7);//将云台转动方向速度发送至云台
    }

}

// 增量式未实践成功 不建议使用

    //增量式
    pid_v_zhs.curError=angle;
    Speed+=(pid_v_zhs.P * pid_v_zhs.curError - pid_v_zhs.I * pid_v_zhs.lastError + pid_v_zhs.D * pid_v_zhs.preError);

    pid_v_zhs.preError=pid_v_zhs.lastError;
    pid_v_zhs.lastError=pid_v_zhs.curError;