双闭环PID程序（位置环加速度环）

#include "pid.h" PID pid; //存放PID算法所需要的数据 PID1 pid1; void PID_Init() { pid.Sv=03000; //用户设定值 pid.Pv=00.0; //检测值 pid.OUT0 =00; //PID常数 pid.Ek=0.0; //当前误差值 pid.Ek_1=0.0; //上一个误差值 pid.Ek_2=0.0; //上上一个误差值 pid.Kp=36; //比例常数 pid.Ki=0.25; //积分常数 pid.Kd=135; //微分常数 pid.uplimit=6000; //上限值 pid.lowlimit=-6000;//下限值 } void PID1_Init() { pid1.Sv=1400.525; //用户设定值 pid1.Pv=00.0; //检测值 pid1.OUT0 =0; //PID常数 pid1.Ek=0.0; //当前误差值 pid1.Ek_1=0.0; //上一个误差值 pid1.Ek_2=0.0; //上上一个误差值 pid1.Kp=6.5; //比例常数 pid1.Ki=0.36; //积分常数 pid1.Kd=0.00005; //微分常数 pid1.uplimit=6000; //上限值 pid1.lowlimit=-6000;//下限值 pid1.PIDOUT=-6000; } float Set_Pwm(float moto1) { float OUT; if(moto1>0) AIN2=0, AIN1=1; else AIN2=1, AIN1=0; OUT=myabs(moto1); return OUT; } //绝对值函数 float myabs(float a) { int temp; if(a<0) temp=-a; else temp=a; return temp; } int PID1_output () { float PIDOUT; pid1.Ek=pid1.Sv-pid1.Pv; PIDOUT=pid1.Kp*(pid1.Ek-pid1.Ek_1)+pid1.Ki*pid1.Ek+pid1.Kd*(pid1.Ek_2*pid1.Ek_1+pid1.Ek_2)+pid1.OUT0; pid1.Ek_2=pid1.Ek_1; pid1.Ek_1=pid1.Ek; pid1.PIDOUT+=PIDOUT; pid1.PIDOUT=(pid1.PIDOUT<pid1.lowlimit) ? pid1.lowlimit:(pid1.PIDOUT>pid1.uplimit) ? pid1.uplimit : pid1.PIDOUT; return pid1.PIDOUT; } int PID_output () { float PIDOUT; pid.Ek=pid.Sv-pid.Pv; PIDOUT=pid.Kp*pid.Ek+pid.Ki*(pid.Ek+pid.Ek_1+pid.Ek_2)+pid.Kd*(pid.Ek-pid.Ek_1)+pid.OUT0; pid.Ek_2=pid.Ek_1; pid.Ek_1=pid.Ek; pid.PIDOUT=PIDOUT; pid.PIDOUT=(pid.PIDOUT<pid.lowlimit) ? pid.lowlimit:(pid.PIDOUT>pid.uplimit) ? pid.uplimit : pid.PIDOUT; return pid.PIDOUT; }