|
本帖最后由 xihua13104 于 2015-9-19 19:31 编辑
我刚接触PID不久,查阅了不少pid算法的资料,对pid基础有大致的了解,目前想把pid算法实现在巡线小车上。最开始我只用到了P来调节,调试过程中两个电机都震荡的很厉害,完全看不出调节的效果,从理论上分析我觉得有没有什么错误,出现的问题也是很奇怪,请大家帮我看看。
程序代码如下
/*软件实现的pwm调速h文件*/
#ifndef __pwm_h__ //单片机晶振12MHZ,pwm频率50hz(即周期20ms)
#define __pwm_h__ //定时器初值计算
#define uint int //(1/100/100)/(12/12000000)=100
#define uchar char
uint timer;
sbit left1 =P2^0; //电机控制端
sbit left2 =P2^1;
sbit right1=P2^2;
sbit right2=P2^3;
void init_timer0()
{
TMOD|=0x01;
TL0 = (65536-100);
TH0 = (65536-100)>>8;
ET0 = 1;
EA = 1;
TR0 = 1;
}
void PWMSET(uint pwm1,uint pwm2,uint pwm3,uint pwm4)
{
if(timer < pwm1) //改变pwm1控制电机1速度
{
left1=1;
}
else
{
left1=0;
}
if(timer < pwm2) //改变pwm2控制电机2速度
{
left2=1;
}
else
{
left2=0;
}
if(timer < pwm3) //改变pwm3控制电机3速度
{
right1=1;
}
else
{
right1=0;
}
if(timer < pwm4) //改变pwm4控制电机4速度
{
right2=1;
}
else
{
right2=0;
}
}
#endif
#include<reg52.h>
#include"math.h"
#include"pwm.h"
#include"stdio.h"
#include"intrins.h"
#define uint int
#define uchar char
uint sampledata=0; //采样数据
uint offset; //当前赛道位置
uint lastoffset[2]={0};//上一次赛道位置
static int SPEED1=100; //电机pwm初值
static int SPEED2=100;
long clean1=0,clean2=1; //采样周期控制位
sbit eye5=P1^0; //传感器1-5
sbit eye4=P1^1;
sbit eye3=P1^2;
sbit eye2=P1^3;
sbit eye1=P1^4;
void delay(unsigned char d);
void uart_init();
void sample();
void confirlocation();
void filter();
void savedata();
float LocPIDCalc(uint NextPoint);
void motorctl();
struct PID
{
uint SetPoint; //设定目标 Desired Value
uint SumError; //误差累计
float Proportion; //比例常数 Proportional Cons
// float Integral; //积分常数 Integral Const
float Derivative; //微分常数 Derivative Const
uint LastError; //Error[-1]
uint PrevError; //Error[-2]
} locapid={0,0,1.5,0.01,0,0} ;
void delay(unsigned char e) //误差 0us,延时ms
{
unsigned char a,b,c,d;
for(d=0;d<e;d++)
{
for(c=1;c>0;c--)
for(b=142;b>0;b--)
for(a=2;a>0;a--);
}
}
void uart_init()
{
SCON=0x40; //串口通信工作方式1
REN=1; //允许接收
TMOD|=0x20; //定时器1的工作方式2
TH1=0xfd;TL1=0xfd;
TI=1; //这里一定要注意
TR1=1;
}
void sample() //数据采样函数
{
sampledata=0;
if(eye1==1)
sampledata|=0x01;
if(eye2==1)
sampledata|=0x02;
if(eye3==1)
sampledata|=0x04;
if(eye4==1)
sampledata|=0x08;
if(eye5==1)
sampledata|=0x10;
}
void confirlocation() //确定赛道位置
{
switch(sampledata)
{
case 0x10:offset=4; break; //10000最左边1个传感器检测到黑线
case 0x18:offset=3; break; //11000最左边2个传感器检测到黑线
case 0x08:offset=2; break; //01000
case 0x0c:offset=0; break; //01100
case 0x04:offset=0; break; //00100
case 0x06:offset=0;break; //00110
case 0x02:offset=-2;break; //00010
case 0x03:offset=-3;break; //00011
case 0x01:offset=-4;break; //00001
default: offset=5; break; //其余情况包括跑道丢失于检测错误
}
}
void filter() //赛道位置数据滤波(减少某次采样错误对系统的干扰
{
if(offset==5)//滤除错误信号,没有检测到黑线(是需要保持上一次测量值的)
{
offset=lastoffset[0];
}
else if(abs(offset-lastoffset[0])>4)
{
offset=lastoffset[0];
}
}
void savedata()
{
static uint TCnt2=0;
TCnt2++;
lastoffset[0]=offset;
if(TCnt2==20)
{
lastoffset[1]=lastoffset[0];
TCnt2=0;
}
}
float LocPIDCalc(uint NextPoint) //增量式pid
{
float Error,dError;
struct PID *sptr;
sptr=&locapid;
Error = sptr->SetPoint - NextPoint; //偏差
sptr->SumError += Error; //积分
dError = sptr->SumError -2* sptr->LastError+sptr->PrevError; //微分
sptr->PrevError = sptr->LastError;
sptr->LastError = Error;
return (sptr->Proportion * Error //比例项
/*+sptr->Integral * sptr->SumError *///积分项
/*+sptr->Derivative * dError*/ ); //微分项
}
void motorctrl() //电机pid算法控制
{
if((offset>=2&&offset<=4)||(offset>=-4&&offset<=-2))
{
SPEED1=(int)(SPEED1+LocPIDCalc(offset));
if((SPEED1>=170)||(SPEED1<=30))
{ SPEED1=100; }
SPEED2=(int)(SPEED2-LocPIDCalc(offset));
if((SPEED2>=170)||(SPEED2<=30))
{ SPEED2=100; }
}
else
{
SPEED1=100;SPEED2=100;
}
PWMSET(0,SPEED1,0,SPEED2);
delay(5);
}
void main()
{
init_timer0();
uart_init();
PWMSET(0,SPEED1,0,SPEED2);
while(1)
{
sample(); //数据采样
confirlocation(); //确认赛道信息
filter(); //滤波
printf("SPEED1= %d,SPEED2= %d,offset= %d,LocPIDCalc= %.1f\n",SPEED1,SPEED2,offset,LocPIDCalc(offset));
while(clean2)
{
motorctrl(); //电机控制
clean2=0;
}
savedata(); //保存赛道信息
}
}
void timer0(void) interrupt 1 //100us
{
TL0 = (65536-100);
TH0 = (65536-100)>>8;
timer++;
clean1++;
if(timer>200) //PWM周期为 2/100S=20ms
{
timer=0;
}
if(clean1==500) //控制采样周期,50ms
{
clean2=1;
clean1=0;
}
}
小车实物连接如图
因为网上都说先只用p来调节,直到小车大致可以巡线了再加上I和D,慢慢进行微调,所以我最开始只用的P |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
曾经有一段真挚的爱情摆在我的面前,我没有珍惜,现在想起来,还好我没有珍惜……
|