机器人大赛程序设计
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
#include<BoeBot.h>
#include<uart.h>
#include<intrins.h>
#define LeftIR P1_2 //左边红外接收连接到P1_2
#define RightIR P3_5 //右边红外接收连接到P3_5
#define LeftLaunch P1_3 //左边红外发射连接到P1_3
#define RightLaunch P3_6 //右边红外发射连接到P3_6
#define Kpl 70
#define Kpr -70
#define SetPoint 3
#define CenterPulse 1500
unsigned int time;
int leftdistance,rightdistance;//左边和右边的距离
int delayCount,distanceLeft,distanceRight,irDetectLeft,irDetectRight;
unsigned int frequency[5]={29370,31230,33050,35700,38460};
int P1_4state(void)//获取P1_4的状态,右胡须
{
return (P1&0x10)?1:0;
}
int P2_3state(void)//获取P2_3的状态,左胡须
{
return (P2&0x08)?1:0;
}
void timer_init(void)
{
IE=0x82; //开总中断EA，允许定时器0中断ET0 TMOD |= 0X01; //定时器0工作在模式1：16位定时器模式}
void FreqOut(unsigned int Freq)
{
time = 256 - (50000/Freq);
TH0 = 0XFF ;
TL0 = time ;
TR0 = 1;
delay_nus(800);
TR0 = 0;
}
void Timer0_Interrupt(void) interrupt 1
{
LeftLaunch = ~LeftLaunch;
RightLaunch= ~ RightLaunch;
TH0 = 0XFF;
TL0 = time;
}
void Get_lr_Distances()
{
unsigned char count;
leftdistance = 0; //初始化左边的距离
rightdistance = 0; //初始化右边的距离
for(count = 0;count<5;count++)
{
FreqOut(frequency[count]);
irDetectRight = RightIR;
irDetectLeft = LeftIR;
if (irDetectLeft == 1)
leftdistance++;
if (irDetectRight == 1)
rightdistance++;
}
}
void Send_Pulse(unsigned int pulseLeft,unsigned int pulseRight) {
P1_1=1;
delay_nus(pulseLeft);
P1_1=0;
P1_0=1;
delay_nus(pulseRight);
P1_0=0;
delay_nms(18);
}
void Forward(void)
{
P1_1=1;
delay_nus(1700);
P1_1=0;
P1_0=1;
delay_nus(1300);
P1_0=0;
delay_nms(20);
}
void bForward(void)
{ int i;
for(i=1;i<=150;i++)
{P1_1=1;
delay_nus(1700);
P1_1=0;
P1_0=1;
delay_nus(1300);
P1_0=0;
delay_nms(20);
}
}
void Left_Turn(void)
{
int i;
for(i=1;i<=41;i++)
{
P1_1=1;
delay_nus(1300);
P1_1=0;
P1_0=1;
delay_nus(1300);
P1_0=0;
delay_nms(20);
}
}
void Right_Turn(void) {
int i;
for(i=1;i<=41;i++)
{
P1_1=1;
delay_nus(1700);
P1_1=0;
P1_0=1;
delay_nus(1700);
P1_0=0;
delay_nms(20);
}
}
void Backward(void)
{
int i;
for(i=1;i<=65;i++)
{
P1_1=1;
delay_nus(1300);
P1_1=0;
P1_0=1;
delay_nus(1700);
P1_0=0;
delay_nms(20);
}
}
void whiskers(unsigned int p1_4state,unsigned int p2_3state)
{
while(1)
{
if((P1_4state()==0)&&(P2_3state()==0))
{
Backward(); //向后
Left_Turn();//向左
Left_Turn();//向左
}
else if(P1_4state()==0)
{
Backward();//向后
Left_Turn();//向左
}
else if(P2_3state()==0)
{
Backward();//向后
Right_Turn();//向右
}
else
Forward();//向前
}
}
int main(void)
{ unsigned int p1_4state,p2_3state;
unsigned int pulseLeft,pulseRight;
uart_Init();
timer_init();
while(1)
{
Get_lr_Distances();
pulseLeft=(SetPoint-leftdistance)*Kpl+CenterPulse;
pulseRight=(SetPoint-rightdistance)*Kpr+CenterPulse;
printf("distanceLeft=%d distanceRight=%d\n", leftdistance, rightdistance);
Send_Pulse(pulseLeft,pulseRight);
whiskers(p1_4state,p2_3state);
}
}。