PID.rar_M 测速_M法T法_M法T法测速_T 测速_t测速

#include <mega16.h> #include<math.h> #include<delay.h> #include "lcdbn.h" uchar data1[4]={0,2,5,0}; uchar data2[4]={0,0,0,0}; uchar j,key=0; uchar chr[]="pre: *2%"; uchar *chr1[3]={"openc V: ", "shutc V: ", "acceler: "} ; uchar counter1=0x84; uchar counter2=0xc8; uchar flag; //flag为开闭使能 int E1r; int E2r=0,E3r=0; uint speedr=15,c_r=0;//maxc=700; void init_intr(void) { SREG=0x80; TIMSK=0x40; TCCR2=0x06; TCCR1A=0xe3; //A比较高电平，B比较低电平 TCCR1B=0x0a; MCUCR=0x0a; ///外部INT1/0中断上升有效 GICR=0xc0; } //////////////////////////////////// /////左右电机pwm设置 /////////////////////////////////// void setpwmr(uchar *p) { uint temp1; temp1=(uint)p[1]*10+p[2]; if(temp1<25) { speedr=(uint)28-temp1*28/25; } else { speedr=(uint)temp1*28/25-28; } temp1=1023*temp1/50; OCR1A=temp1; OCR1B=temp1; } /////转速加/减调速////// void add(uchar*p) { if(p[1]!=5) { p[2]++; if(p[2]==10) { p[2]=0;p[1]++; } } } void sub(uchar *p) { uchar temp; temp=p[1]*20+p[2]*2; if(temp!=0) temp=temp-2; p[1]=temp/20; //设定值还原// p[2]=(temp-p[1]*20)/2; } void display() { write_string(0x80,chr); write_string(0xc0,chr1[key]); write_order(counter1+0); write_Data(data1[0]+0x30); for(j=1;j<3;j++) {write_order(counter1+j); write_Data(data1[j]+0x30); } for(j=0;j<4;j++) { write_order(counter2+j); write_Data(data2[j]+0x30); } } //////速度调节//// void speed_change(void) { if(PINA.1==0) { delay_ms(100); if(PINA.1==0) {add(data1); } } if(PINA.2==0) {delay_ms(100); if(PINA.2==0) {sub(data1);} } } /////键盘读取//// void testkey(void) { uchar flag2=0; if(PINA!=0x0f) { delay_ms(10); if(PINA!=0x0f) {flag2=1;} } if(flag2) { while(PINA.3!=0) { while(PINA.0==0) { delay_ms(100); if(PINA.0==0) { if(key<2) {key++;} else key=0;} } switch (key) {case 0: //模式1PID调节 flag=0; speed_change(); setpwmr(data1); display(); break; case 1: //模式2开环 flag=1; speed_change(); setpwmr(data1); init_intr(); display(); break; case 2: display(); break; default:break; } } flag2=0; } } /* void speed_convert(uint c) //测速转化成pwm值 { }*/ void new_pid(uint cl,uchar k) //积分分离法 { int temp3,z,p; //p为积分系数误差越大P越小 if(flag) { E1r=speedr-cl; if(speedr>0&abs(E1r)<11) { switch(abs(E1r)/2) { case 5: p=1; break; case 4: p=3; break; case 3: p=5; break; case 2: p=8; break; case 1: p=10; break; default : break; } z=23*(E1r-E2r)+p*(E1r-2*E2r+E3r)-10*E3r; z=(int)z*102/k; temp3=OCR1A; temp3=temp3+z; OCR1A=temp3; OCR1B=temp3; E3r=E2r; E2r=E1r; } } } /*void pid_motr(uint cr,uchar k) { int temp,u; if(flag) { if(speedr>0) { E1r=speedr-cr; if(E1r==speedr) //E误差很大时最大额定电流加速 { OCR1A=maxc; OCR1B=maxc; } else if(abs(E1r)>0&&abs(E1r)<7) { u=(int)(24*E1r-35*E2r+14*E3r)*102/k; temp=OCR1A; temp=temp+u; OCR1A=temp; OCR1B=temp; E3r=E2r; E2r=E1r; } } } } */ ////////速度数据分解/////// void speed_count(uint r) { data2[3]=r%10; r=r/10; data2[2]=r%10; r=r/10; data2[1]=r%10; r=r/10; data2[0]=r%10; } /////采用外部中断测速////////// interrupt[EXT_INT0]void int_0(void) { c_r++; } /////T2定时80ms对速度采样//////// interrupt[TIM2_OVF]void time_2(void) { MCUCR=0x00; GICR=0x00; new_pid(c_r,500) ; //pid_motr(c_r,100); speed_count(c_r) ; MCUCR=0x0f; ///外部INT1/0中断上升有效 GICR=0xc0; c_r=0; } void initall_IO(void) { DDRB=0xf0; PORTB=0x00; DDRA=0xF0; PORTA=0x0F; DDRC=0XFF; PORTC=0X00; DDRD=0xF0; PORTD=0x0F; } void main(void) { initall_IO(); init_lcd(); while(1) { DDRA=0xF0; PORTA=0x0F; testkey(); init_intr(); display(); } } PID算法公式有点错:应改为 z=100*(E1r-E2r)+p*E1r+125*(E1r-2*E2r+E3r); 经过老师的指导:速度采样用M法不适应低,所以对于调速范围大的就用M/T法,适合于高低测速!对测速还要加以数字滤波!可用一阶惯性/中值法,平均法等!