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

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

#include #include

#define uchar unsigned char #define uint unsigned int #define THC0 0xf8

#define TLC0 0xcD //2ms

unsigned char code Duan[]={0x3F, 0x06,0x5B,0x4F,0x66,0x6D,0x7D,0x07,0x7F,0x6F};//共阴极数码管，0-9段码表

unsigned char Data_Buffer[8]={0,0,0,0,0,0,0,0};

uchari=0;

sbitAddSpeed=P1^1; sbitSubSpeed=P1^2; sbit PWM_FC=P1^0;

int e ,e1 ,e2 ; //pid偏差 float uk ,uk1 ,duk ;//pid输出值

float Kp=5,Ki=10,Kd=1.9;//pid控制系数 int out=0;

uintSpeedSet=1000; uintcnt=0;

uintInpluse=0,num=0;//脉冲计数

uintPWMTime=100;// 脉冲宽度 unsigned char arry[]; voidSendString(uintch);

void PIDControl(); //PID控制 void SystemInit(); //系统初始化 void delay(uchar x);

void PWMOUT(); //PWM输出 void SetSpeed(); //速度设置 voidSegRefre();

/**************主函数************/ void main() { SystemInit(); while(1) { SetSpeed(); SegRefre(); PWMOUT(); } }

void PIDControl() //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(out>1000) { out=1000; } else if(out<0) { out=0; } uk1=uk; e2=e1; e1=e; PWMTime=out; }

void delay(uchar x) { uinti,j; for(i=x;i>0;i--) for(j=50;j>0;j--); }

void PWMOUT() { 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(SpeedSet>9999) { SpeedSet=9999; } } } if(SubSpeed==0) { delay(200); if(SubSpeed==0) { SpeedSet-=100; if(SpeedSet<0) SpeedSet=0; } } }

void SegRefre() //显示刷新 { Data_Buffer[0]=SpeedSet/1000; Data_Buffer[1]=SpeedSet00/100; Data_Buffer[2]=SpeedSet0/10; Data_Buffer[3]=SpeedSet;

Data_Buffer[4]=num/1000; Data_Buffer[5]=num00/100; Data_Buffer[6]=num0/10; Data_Buffer[7]=num; }

void int0() interrupt 0 { Inpluse++; }

void t0() interrupt 1 {

static unsigned char Bit=0;//静态变量，退出程序值保留 static unsigned int time=0; static unsigned intaa=0; TH0=THC0; TL0=TLC0; aa++; if(aa==50) { aa=0; flag0=1; } Bit++;

time++; //转速测量周期 if(Bit>8) Bit=0; P0=0xff;

P2=Duan[Data_Buffer[Bit]]; switch(Bit) { case 0:P0=0X7F;break; case 1:P0=0XBF;break; case 2:P0=0XDF;break; case 3:P0=0XEF;break; case 4:P0=0XF7;break; case 5:P0=0XFB;break; case 6:P0=0XFD;break; case 7:P0=0XFE;break; }

if(time>100) { time=0; num=Inpluse*15*5;

Inpluse=0; PIDControl(); } }

void timer_1() interrupt 3 { cnt++; //cnt越大占空比越高2.5Khz }