利用PID算法实现倒立摆控制

利用PID算法实现倒立摆控制#includereg52.h#includestdio.h#defineucharunsignedchar#defineuintunsignedint#defineTHC00xf8#defineTLC00xcD//2msunsignedcharcodeDuan[]={0x3F,0x06,0x5B,0x4F,0x66,0x6D,0x7D,0x07,0x7F,0x6F};//共阴极数码管，0-9段码表unsignedcharData_Buffer[8]={0,0,0,0,0,0,0,0};uchari=0;sbitAddSpeed=P1^1;sbitSubSpeed=P1^2;sbitPWM_FC=P1^0;inte,e1,e2;//pid偏差floatuk,uk1,duk;//pid输出值floatKp=5,Ki=10,Kd=1.9;//pid控制系数intout=0;uintSpeedSet=1000;uintcnt=0;uintInpluse=0,num=0;//脉冲计数uintPWMTime=100;//脉冲宽度unsignedchararry[];voidSendString(uintch);voidPIDControl();//PID控制voidSystemInit();//系统初始化voiddelay(ucharx);voidPWMOUT();//PWM输出voidSetSpeed();//速度设置voidSegRefre();/**************主函数************/voidmain(){SystemInit();while(1){SetSpeed();SegRefre();PWMOUT();}}voidPIDControl()//pid偏差计算{e=SpeedSet-num;duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/50;//+Kd*(e-2e1+e2)uk=uk1+duk;out=(int)uk;if(out1000){out=1000;}elseif(out0){out=0;}uk1=uk;e2=e1;e1=e;PWMTime=out;}voiddelay(ucharx){uinti,j;for(i=x;i0;i--)for(j=50;j0;j--);}voidPWMOUT(){if(cntPWMTime){PWM_FC=1;}else{PWM_FC=0;}if(cnt1000)cnt=0;}voidSystemInit(){TMOD=0X21;//t1用来串口t2定时TH0=THC0;TL0=TLC0;TH1=0xC0;TL1=0XC0;ET1=1;ET0=1;TR0=1;TR1=1;EX0=1;//中断0用来测量转速IT0=1;EA=1;e=0;e1=0;e2=0;}voidSetSpeed(){if(AddSpeed==0){delay(200);if(AddSpeed==0){SpeedSet+=100;if(SpeedSet9999){SpeedSet=9999;}}}if(SubSpeed==0){delay(200);if(SubSpeed==0){SpeedSet-=100;if(SpeedSet0)SpeedSet=0;}}}voidSegRefre()//显示刷新{Data_Buffer[0]=SpeedSet/1000;Data_Buffer[1]=SpeedSet%1000/100;Data_Buffer[2]=SpeedSet%100/10;Data_Buffer[3]=SpeedSet%10;Data_Buffer[4]=num/1000;Data_Buffer[5]=num%1000/100;Data_Buffer[6]=num%100/10;Data_Buffer[7]=num%10;}voidint0()interrupt0{Inpluse++;}voidt0()interrupt1{staticunsignedcharBit=0;//静态变量，退出程序值保留staticunsignedinttime=0;staticunsignedintaa=0;TH0=THC0;TL0=TLC0;aa++;if(aa==50){aa=0;flag0=1;}Bit++;time++;//转速测量周期if(Bit8)Bit=0;P0=0xff;P2=Duan[Data_Buffer[Bit]];switch(Bit){case0:P0=0X7F;break;case1:P0=0XBF;break;case2:P0=0XDF;break;case3:P0=0XEF;break;case4:P0=0XF7;break;case5:P0=0XFB;break;case6:P0=0XFD;break;case7:P0=0XFE;break;}if(time100){time=0;num=Inpluse*15*5;Inpluse=0;PIDControl();}}voidtimer_1()interrupt3{cnt++;//cnt越大占空比越高2.5Khz}