PID控制电机程序

AVR单片机PID控制电机程序2011-04-10 20:53#include #include #include #include sfrw ICR1=0x26; / 补充定义16位寄存器ICR1地址为0x26(mega16.h中未定义) unsigned int icp_v1;unsigned int icp_v2;unsigned char icp_n;unsigned char max_icp; /量程定义字bit icp_ok;bit time_4ms_ok;bit freq_ok;bit begin_m; /定时器1溢出，重新测量标志字bit full_ok; /定时器1溢出long fv; struct PID unsigned int SetPoint; / 设定目标 Desired Value unsigned int Proportion; / 比例常数 Proportional Const unsigned int Integral; / 积分常数 Integral Const unsigned int Derivative; / 微分常数 Derivative Const unsigned int LastError; / Error-1 unsigned int PrevError; / Error-2 unsigned int SumError; / Sums of Errors ; struct PID spid; / PID Control Structurelong rout; / PID Response (Output) long rin; / PID Feedback (Input) void PID_Init(struct PID *pp) memset ( pp,0,sizeof(struct PID); /全部初始化为0 unsigned int PID_Calc( struct PID *pp, unsigned int NextPoint ) unsigned int dError,Error; Error = pp-SetPoint - NextPoint; / 偏差 pp-SumError += Error; / 积分 dError = pp-LastError - pp-PrevError; / 当前微分 pp-PrevError = pp-LastError; pp-LastError = Error; return (pp-Proportion * Error / 比例项 + pp-Integral * pp-SumError / 积分项 + pp-Derivative * dError); / 微分项 / Timer 2 比较匹配中断服务，4ms定时 interrupt TIM2_COMP void timer2_comp_isr(void) #asm(sei) / 开放全局中断,允许中断嵌套 time_4ms_ok = 1;/ Timer 1 溢出中断服务interrupt TIM1_OVF void timer1_ovf_isr(void) full_ok = 1;/ Timer 1 输入捕捉中断服务interrupt TIM1_CAPT void timer1_capt_isr(void) if (icp_n = max_icp) / 第N个上升沿到 icp_v2 = ICR1; / 记录第N个上升沿时间 TIMSK = 0x80; / 禁止T/CI输入捕捉和溢出中断 icp_ok = 1; else if (icp_n = 0) icp_v1 = ICR1; / 记录第1个上升沿时间 icp_n+; void main(void) unsigned int icp_1,icp_2; PORTB=0x00; DDRB=0x08; PORTD=0x40; / PD6(icp)输入方式，上拉有效 / T/C0 定时初始化 TCCR0=0x79; / Mode: Fast PWM top=FFh TCNT0=0x00; / OC0 output: Non-Inverted PWM OCR0=0xff; / T/C1 计数初始化 TCCR1B = 0x41; / T/C1正常计数方式，上升沿触发输入捕捉，4M/1计数时钟 TIMSK = 0xA4; / 允许T/C2比较匹配中断，允许T/C1输入捕捉、溢出中断 / T/C2 定时初始化 TCCR2=0x0C; / 内部时钟，64分频（4M/64=62.5KHz），CTC模式 OCR2=0xff;/ OCR2 = 0xf9(249),(249+1)/62.5=4ms icp_n = 0; max_icp =1; /定义上升沿的位序 #asm(sei) / 开放全局中断 INIT_FYD(); Show_Text(3,0,chinese); /(列，行，显示字) PID_Init(&spid); / Initialize Structure spid.SetPoint = 168; /设定目标 Desired Value spid.Proportion = 6; /比例常数 Proportional Const spid.Integral =5; /积分常数 Integral Const spid.Derivative =2; /微分常数 Derivative Const FM_Num(0,1,spid.Proportion); /显示比例 FM_Num(3,1,spid.Integral); /显示积分 FM_Num(6,1,spid.Derivative); /显示微分 FM_Num(0,2,spid.SetPoint); /显示目标 while (1) if (icp_ok = 1) / 完成一次测量 if (icp_v2 = icp_v1) / 计算N个上升沿的时钟脉冲个数， icp_2 = icp_v2 - icp_v1; /两次连续的ICR1的差值 else icp_2 = 65536 - icp_v1 + icp_v2;/定时器1溢出，加上65535 if (!(icp_v2 = icp_v1 & full_ok)/ 有溢出，数据无效 /if (icp_2 = icp_1) / 两次个数相等，测量有效 fv = 1000000 * (long)max_icp / icp_2; / 换算成频率值 freq_ok = 1; /频率换算完成 if (fv 4000) max_icp = 20; / 如果频率大于4Khz，N=64 else max_icp = 1; / N=1 else max_icp = 1; / 有溢出，N=1 icp_1 = icp_2; icp_ok = 0; begin_m = 1; if (time_4ms_ok) /定时器2，定时到 if(freq_ok) /判断频率是否换算完成 rin=fv; FM_Num(0,3,rin); rout = PID_Calc ( &spid,rin ); rout=rout/100; FM_Num(3,3,rout); if(rout100&rout256) OCR0=rout; freq_ok = 0;