//P1.1(T0):Count They Distance
//P0.4:Tx
//P0.5:Rx
#include <C8051F310.h> //SFR declarations
#include <stdio.h> //Standard I/O definition file
#include <math.h> //Math library file
#include <Intrins.h>
#include <absacc.h>
unsigned int j,i;
char a=0;
unsigned int t=0;
//sbit led=P0^2;
//P0.0(PWM0):给定左轮速度.
sbit vls=P0^4; //P0.4(GPIO):给定左轮方向.
sbit vlf=P0^6; //P0.6(T0) :反馈左轮速度.
sbit dlf=P1^0; //P1.0(GPIO):反馈左轮方向.
//P0.2(PWM0):给定右轮速度.
sbit vrs=P0^5; //P0.5(GPIO):给定右轮方向.
sbit vrf=P0^7; //P0.7(T0) :反馈右轮速度.
sbit drf=P1^1; //P1.1(GPIO):反馈右轮方向.
int ol; //左轮给定值
int len;
int len_1,len_2;
int lyn_1,lyn_2;
int vl1,vl2; //反馈左轮速度值(取样周期内的方波数)
int lfz; //运算后赋给PWM的值
int lyn,lynn;
int lun=0,lun_1=0; //偏差校正值 即校正PWM输出
int lunp,luni,lund; //PID 校正值
int or; //右轮给定值
int ren;
int ren_1,ren_2;
int ryn_1,ryn_2;
int vr1,vr2; //反馈右轮速度值(取样周期内的方波数)
int rfz; //运算后赋给PWM的值
int ryn,rynn;
int run=0,run_1=0; //偏差校正值 即校正PWM输出
int runp,runi,rund; //PID 校正值
float kp=2.0; //比例系数1.8
float kd=0.2; //微分系数0.4
float lki; //积分系数
void pio_init(void);
void sys_init(void);
void t01_init(void);
void TIME3_INT(void);
void PID(void);
void interrupt_init(void);
void delay(unsigned int x);
void pwm1_1(void);
void main(void)
{
PCA0MD &= ~0x40; //关闭
pio_init(); //P11为测距输入端
sys_init();
t01_init();
pwm1_1();
TIME3_INT();
interrupt_init();
vls=1;vrs=0;
while(1)
{
ol=50;
or=50;
delay(1000);
ol=100;
or=100;
delay(1000);
ol=-50;
or=50;
delay(1000);
}
}
void PID(void)
{
/****************左轮PID调节******************/
if(dlf==1)
{
lyn=(vl2*256+vl1); //dlf是左轮反馈方向，0表示向前 vl=TL0
}
else
{
lyn=-(vl2*256+vl1); //dlf=1表示是向后退，速度应该为负值
}
len=ol-lyn; //误差=给定速度-反馈速度(取样周期内的方波数)
if(abs(len)<8)//30
{
lki=1.4; //ki值的确定1.4
}
else
{
lki=0.05; //积分系数：如果 | 给定值-反馈值 | 太大
} //则就可以不引入积分，或者引入的很小0.05
lunp=kp*(len-len_1); //比例校正
luni=lki*len; //积分校正
lund=kd*(len-2*len_1+len_2); //微分校正
lun=lunp+luni+lund+lun_1; //总校正
/*************新旧数据更新*************************/
len_2=len_1;
len_1=len; //len:当前取样周期内出现的速度偏差;len_1:上次取样周期内出现的速度偏差
lun_1=lun; //lun:当前取样周期内得出的PWM校正值;lun_1:上次取样周期内得出的PWM校正值
/*************新旧数据更新*************************/
if(lun>255)
{
lun=255; //正速度
}
if(lun<-255)
{
lun=-255; //负速度
}
if(lun<0)
{
vls=1;
PCA0CPH0=-lun;
}
if(lun>=0)
{
vls=0;
PCA0CPH0=lun;
}
/****************右轮PID调节******************/
if(drf==0)
{
ryn=(vr2*256+vr1); //drf是右轮反馈方向，0表示向前 vl=TL0
}
else
{
ryn=-(vr2*256+vr1); //dlf=1表示是向后退，速度应该为负值
}
ren=or-ryn; //误差=给定速度-反馈速度(取样周期内的方波数)
if(abs(ren)<8)//30
{
lki=1.4; //ki值的确定1.4
}
else
{
lki=0.05; //积分系数：如果 | 给定值-反馈值 | 太大
} //则就可以不引入积分，或者引入的很小0.05
runp=kp*(ren-ren_1); //比例校正
runi=lki*ren; //积分校正
rund=kd*(ren-2*ren_1+ren_2); //微分校正
run=runp+runi+rund+run_1; //总校正
/*************新旧数据更新*************************/
ren_2=ren_1;
ren_1=ren; //len:当前取样周期内出现的速度偏差;len_1:上次取样周期内出现的速度偏差
run_1=run; //lun:当前取样周期内得出的PWM校正值;lun_1:上次取样周期内得出的PWM校正值
/*************新旧数据更新*************************/
if(run>255)
{
run=255; //正速度
}
if(run<-255)
{
run=-255; //负速度
}
if(run<0)
{
vrs=1;
PCA0CPH1=-run;
}
if(run>=0)
{
vrs=0;
PCA0CPH1=run;
}
//因为这里的PCA0CPH0越大，对应的电机速度越小，所以要255来减一下
}
void pio_init(void)
{
XBR0=0x00; //0000 0001
XBR1=0x72; //0111 0010 时能弱上拉 T0T1连接到脚口P06、P07 CEX0、CEX1连接到脚口P00、P01
P0MDIN=0xff; //模拟(0);数字(1) 1111 0011
P0MDOUT=0xc3;//开漏(0);推挽(1) 1111 1111
P0SKIP=0x3c; //0011 1100
P1MDIN=0xff; //1111 1111
P1MDOUT=0xfc;//
P1SKIP=0x00; //1111 1111
}
void sys_init(void) //12MHz
{
OSCICL=0x43;
OSCICN=0xc2;
CLKSEL=0x00;
}
void pwm1_1(void) //PWM的初始化
{
PCA0MD=0x08; //PCA时钟为12分频
PCA0CPL0=200; //左轮
PCA0CPM0=0x42; //设置左轮为8位PWM输出
PCA0CPH0=200;
PCA0CPL1=200; //平衡校正
PCA0CPM1=0x42; //设置为8位PWM输出
PCA0CPH1=200;
PCA0CN=0x40; //允许PCA工作
}
void t01_init(void)
{
TCON=0x50; //计数器1、2允许
TMOD=0x55; //定时器1、2采用16位计数功能
CKCON=0x00;
TH1=0x00; //用于采集左轮的速度
TL1=0x00;
TH0=0x00; //用于采集右轮的速度
TL0=0x00;
}
void TIME3_INT(void)
{
TMR3CN = 0x00; //定时器3为16位自动重载
CKCON &= ~0x40;
TMR3RLL = 0xff;
TMR3RLH = 0xd7;
TMR3L = 0xff;
TMR3H = 0xd7;
TMR3CN |= 0x04;
}
void T3_ISR() interrupt 14 //定时器3中断服务程序
{
//led=~led;
EA=0;
TCON &=~0x50; //关闭计数器0、1
vl1=TL0; //取左轮速度值
vl2=TH0;
vr1=TL1; //取右轮速度值
vr2=TH1;
TH1=0x00;
TL1=0x00;
TH0=0x00;
TL0=0x00;
PID(); //PID处理
TMR3CN &=~0x80; //清中断标志位
TCON |=0x50; //重新开计数器0、1
EA=1;
}
void interrupt_init(void)
{ IE=0x80;
IP=0x00;
EIE1|=0x80;
EIP1|=0x80;
}
void delay(unsigned int m) //延时程序
{
for(i=0;i<2000;i++)
{
for(j=0;j<m;j++){_nop_(); _nop_();}
}
}