RoboCup机器人灭火冠军比赛程序.docx
- 文档编号:5842641
- 上传时间:2023-01-01
- 格式:DOCX
- 页数:36
- 大小:20.19KB
RoboCup机器人灭火冠军比赛程序.docx
《RoboCup机器人灭火冠军比赛程序.docx》由会员分享,可在线阅读,更多相关《RoboCup机器人灭火冠军比赛程序.docx(36页珍藏版)》请在冰豆网上搜索。
RoboCup机器人灭火冠军比赛程序
#include"ASMF09LIB.h"
/***********设置电机参数*****************/
#defineMOTO_L0
#defineMOTO_R1
#defineMOTO_L_SET1
#defineMOTO_R_SET1
#defineSpeedMax50
#defineLR30//左向右转速度
#defineLRT0.35//左转时间
#defineRL-30//右向左转速度
#defineRLT0.35//右转时间
/**********设置红外参数******************/
#defineIAR_F0
#defineIAR_LF5
#defineIAR_LB4
#defineIAR_B3
#defineIAR_RF1
#defineIAR_RB2
/***********设置复眼参数******************/
#defineEYE_F0
#defineEYE_B1
#defineFIRE_MIN_VALUE480//参数待定
#defineFIRE_MIN_VALUE2430
#defineFIRE_MAX_VALUE200//参数待定
#defineFRONT_FIND0//前部发现火焰
#defineNOT_FIND1//未发现火焰
/***********设置灰度传感器参数************/
#defineGREY_F10//灰度传感器通道值
#defineGREY_B11//同上
#defineGREY_NULL0//没发现白线
#defineGREY_FRONT1//前端发现白线
#defineGREY_BACK2//后端发现白线
#defineGREY_BOTH3//前后均检测到白线
#defineBlackValue100//数值待定
#defineground90
#defineWhiteValue30//数值待定
/***********设置风扇参数******************/
#defineFAN_F0
#defineFAN_B1
#defineFAN_OFF1
/*************麦克风参数*****************/
#defineMIC_CH9
#defineMIC_SET_VALUE70
/*************转弯函数参数****************/
#defineLEFT_900左转90度
#defineRIGHT_901右转90度
/******************定义变量************************/
constintData[6][12]=
{
{660,520,420,350,300,267,230,215,200,183,169,160},
{655,521,420,350,295,260,230,210,192,175,162,156},
{672,510,409,335,282,246,220,195,180,163,152,140},
{649,503,405,323,276,244,215,190,173,161,150,144},
{653,523,430,349,301,267,235,215,196,183,173,166},
{675,539,435,355,303,266,235,213,195,176,164,155}
};
//=================滤波函数=============//
/********************************************/
//函数名称:
Filter
//入口参数:
unsignedcharCh数据通道
//出口参数:
int对应通道数据值
//功能:
采集特定通道信息,同时进行滤波处理
/********************************************/
intFilter(unsignedcharCh);
//================启动函数====================//
/**********************************************/
//函数名称:
START
//入口参数:
无
//返回参数:
无
//函数功能:
如果接收到启动信号,启动机器
/**********************************************/
voidSTART(void);
//===============电机驱动函数=================//
/**********************************************/
//函数名称:
MOTOR
//入口参数:
intLSpeed左轮速度
//intRSpeed右轮速度
//出口参数:
无
//功能:
设定左右轮速度值
/**********************************************/
voidMOTOR(intLSpeed,intRSpeed);
//================查表函数=====================//
/***********************************************/
//函数名称:
Search
//入口参数:
unsignedcharCh通道
//:
intValue滤波后的数值
//出口参数:
int测量的实际距离值
//功能:
将采集到的数据值转化为实际的距离值
/**********************************************/
intSearch(unsignedcharCh,intValue);
//=================火焰识别函数======================//参数待定,函数未实现
/*****************************************************/
//函数名称:
FIND_FIRE
//入口参数:
无
//出口参数:
返回火焰状态位
//FRONT_FIND前部发现火焰
//BACK_FIND后部发现火焰
//NOT_FIND未发现火焰
//功能:
判断是否有火焰
/*****************************************************/
unsignedcharFIND_FIRE(void);
unsignedcharFIND_FIRE2(void);
voidoutfire(void);//灭火
voidGout_1();//一号房出门
voidGout_2();//二号房出门
voidGout_3();//三号房出门
voidGout_4();//四号房出门
voidgohome_1();//一号房回家程序
voidgohome_2();//二号房回家程序
voidgohome_3();//三号房回家程序
voidgohome_4();//四号房回家程序
voidRBW(void);//右边靠墙
voidLBW(void);//左边靠墙
voidGoStr(intv);//走直线
voidSkip_01(void);//原点跳转到1号房
voidSkip_12(void);//1号房跳转到2号房
voidSkip_23(void);//2号房跳转到3号房
voidSkip_34(void);//1号房跳转到4号房
voidFIVE(void);//五段法运行
voidFIVE_R(void);
voidFIVE_34(void);
voidTurn_R(void);//原地右转90度
voidTurn_L(void);//原地左转90度
voidFanRun(void);//开启风扇
voidFanStop(void);//关闭风扇
voidRunLine_F_L(void);//依靠左墙前进,动态调整
voidRunLine_F_R(void);//依靠右墙前进,动态调整
voidRunLine_B_L(void);//依靠左墙后退,动态调整
voidRunLine_B_R(void);//依靠右墙后退,动态调整
intFrontPSD(void);//读出前端PSD与墙体的距离
intBackPSD(void);//读出后端PSD与墙体的距离
intRightBackPSD(void);//读出右后端PSD与墙体的距离
intLeftBackPSD(void);//读出左后端PSD与墙体的距离
intRightFrontPSD(void);//读出右前端PSD与墙体的距离
intLeftFrontPSD(void);//读出左前端PSD与墙体的距离
intmain(void)
{
introom_1=0;
InitArmRobot();
START();//声控开关
while
(1)
{
switch(room_1)
{
case0:
{MOTOR(30,30);
//while(LeftBackPSD()>30);//?
估计时间
while
(1)
{
if((LeftBackPSD()<=30)&&(LeftFrontPSD()<=30))
{
wait(0.05);
if((LeftBackPSD()<=30)&&(LeftFrontPSD()<=30))
{
break;
}
}
}
Skip_01();//原点跳转到1号房,停在一号门口,判断火焰
room_1=1;
break;
}
case1:
{
if(FIND_FIRE()==0)//一号房有火
{
outfire();//灭火
Gout_1();//出一号门
gohome_1();//一号房回家程序
}
else//一号房无火,去二号房
{
MOTOR(-30,-30);//后退
while(BackPSD()>25);
Turn_L();//左转
Stop();
wait(0.1);
LBW();//调整与墙平行
wait(0.5);
Skip_12();//1号房跳转到二号房
Turn_L();//左转
Stop();
wait(0.5);
MOTOR(-20,-20);
wait(0.5);
MOTOR(30,30);
while(AI(GREY_B)>ground);//停在门口,判断火焰
Stop();
wait(0.1);
if(FIND_FIRE2()==0)//二号房有火
{
outfire();//灭火
Gout_2();//2号房出门程序
gohome_2();//2号房回家程序
}
else//二号房无火,去三号房
{
Skip_23();//从2号房跳到3号房,停在门口,判断火焰
wait(0.2);
if(FIND_FIRE()==0)//三号房有火
{
outfire();//灭火
Gout_3();//出三号门
gohome_3();//三号回家程序
}
else//三号房无火,去四号房
{
Skip_34();
outfire();//灭火
Gout_4();//四号出门
gohome_4();//四号回家
}
}
}
}
default:
break;
}
}
return0;
}
intFilter(unsignedcharCh)//滤波函数
{
intTemp[10]={0};
intExTmp=0;
intSum=0;
chari=0,j=0;
for(i=0;i<10;i++)
{
Temp[i]=AI(Ch);
}
for(j=0;j<9;j++)
{
for(i=0;i<9-j;i++)
{
if(Temp[i]>Temp[i+1])
{
ExTmp=Temp[i];
Temp[i]=Temp[i+1];
Temp[i+1]=ExTmp;
}
}
}
Sum+=Temp[3];
Sum+=Temp[4];
Sum+=Temp[5];
Sum+=Temp[6];
Sum>>=2;
returnSum;
}
voidSTART(void)//声控开关
{resettime();
while
(1)
{
if(seconds()>2)
{
break;
}
}
}
voidMOTOR(intLSpeed,intRSpeed)
{
SetMoto(MOTO_L,MOTO_L_SET*LSpeed);
SetMoto(MOTO_R,MOTO_R_SET*RSpeed);
}
intSearch(unsignedcharCh,intValue)
{
inti;
intTemp;
intm;
for(i=0;i<12;i++)
{
if(Data[Ch][i]<=Value)
{
if(i==0)
{
return5;
}
else
{
m=(Data[Ch][i]-Data[Ch][i-1])/5;
Temp=0;
while((Data[Ch][i-1]+Temp*m)>=Value)
{
++Temp;
}
returnTemp+5*i;
}
}
if(i==11)
{
return70;
}
}
return0;
}
unsignedcharFIND_FIRE(void)
{
intt;
inti,j;
intTemp[2][10];
intSum_1;
for(i=0;i<10;i++)
{
Temp[EYE_F][i]=EyeInMaxEx(EYE_F);
}
for(j=0;j<9;j++)
{
for(i=0;i<9-j;i++)
{
if(Temp[EYE_F][i]>Temp[EYE_F][i+1])
{
t=Temp[EYE_F][i];
Temp[EYE_F][i]=Temp[EYE_F][i+1];
Temp[EYE_F][i+1]=t;
}
}
}
Sum_1=0;
Sum_1+=Temp[EYE_F][3];
Sum_1+=Temp[EYE_F][4];
Sum_1+=Temp[EYE_F][5];
Sum_1+=Temp[EYE_F][6];
Sum_1>>=2;
if(Sum_1>FIRE_MIN_VALUE)
{
returnFRONT_FIND;
}
else
{
returnNOT_FIND;
}
}
unsignedcharFIND_FIRE2(void)
{
intt;
inti,j;
intTemp[2][10];
intSum_1;
for(i=0;i<10;i++)
{
Temp[EYE_F][i]=EyeInMaxEx(EYE_F);
}
for(j=0;j<9;j++)
{
for(i=0;i<9-j;i++)
{
if(Temp[EYE_F][i]>Temp[EYE_F][i+1])
{
t=Temp[EYE_F][i];
Temp[EYE_F][i]=Temp[EYE_F][i+1];
Temp[EYE_F][i+1]=t;
}
}
}
Sum_1=0;
Sum_1+=Temp[EYE_F][3];
Sum_1+=Temp[EYE_F][4];
Sum_1+=Temp[EYE_F][5];
Sum_1+=Temp[EYE_F][6];
Sum_1>>=2;
if(Sum_1>FIRE_MIN_VALUE2)
{
returnFRONT_FIND;
}
else
{
returnNOT_FIND;
}
}
voidoutfire(void)//灭火程序
{intTempData;
intFlag=0;
while
(1)
{
if(AI(GREY_F) { Stop();//停车 while (1) { FanRun();//开启风扇 wait(0.6); FanStop();//关闭风扇 wait(0.3); if(FIND_FIRE()==1) { wait(0.5); if(FIND_FIRE()==1) { Flag=1; break; } } } if(Flag) { break; } } TempData=EyeChMaxEx(EYE_F);//读取复眼传感器,得到火焰通道位置 if(TempData<3) { MOTOR(-30+10*TempData,30+10*TempData); } elseif(TempData==3) { MOTOR(30,30); } else { MOTOR(30+10*TempData,30-10*TempData); } } } voidGoStr(intv) { MOTOR(v+1,v); wait(0.05); MOTOR(v,v); wait(0.03); } voidRBW()//右边调整靠墙平行 { if((RightFrontPSD()-RightBackPSD())>0) { Drive(0,10); while((RightFrontPSD()-RightBackPSD())>0); } else { Drive(0,-11); while((RightFrontPSD()-RightBackPSD()<0)); } Stop(); } voidLBW()//左边调整靠墙平行 {if(LeftFrontPSD()-LeftBackPSD()>-1) { Drive(0,-11); while(LeftFrontPSD()-LeftBackPSD()>-1); } else { Drive(0,10); while(LeftFrontPSD()-LeftBackPSD()<0); } Stop(); } voidGout_1()//一号出房门程序 { Gout_2(); } voidGout_2()//二号出房与一号相同 { intData_1,Data_2; intDiff; MOTOR(20,-20); wait(0.2); Stop(); wait(0.3); while(BackPSD()>=25) { RunLine_B_L(); } MOTOR(30,-30); wait(0.3); Stop(); wait(0.3); while (1) { Data_1=RightFrontPSD(); Data_2=RightBackPSD(); if(Data_1>=45) { break; } Diff=Data_1-Data_2; MOTOR(30+5*Diff,30-5*Diff); } MOTOR(30,30); while(FrontPSD()>=20); Stop(); wait(0.3); } voidGout_3()//三号出房 { RBW(); MOTOR(-30,-30); wait(0.3); Stop(); wait(0.1); RBW(); wait(0.1); MOTOR(-30,-30); while(BackPSD()>25); Turn_R();//右转 Stop(); wait(0.5); RBW();//? wait(0.1); do { GoStr(30); }while(FrontPSD()>20); Stop(); wait(0.35); MOTOR(-20,-20); wait(0.2); Stop(); wait(0.2); } voidGout_4()//四号出房与一号相同 { intData_1,Data_2,Data_3; intDiff; MOTOR(20,-20); wait(0.2); Stop(); wait(0.3); while(BackPSD()>=25) { RunLine_B_L(); } MOTOR(30,-30); wait(0.3); Stop(); wait(0.3); while (1) { Data_1=RightFrontPSD(); Data_2=RightBackPSD(); Data_3=FrontPSD(); if(Data_3<=25) { break; } Diff=Data_1-Data_2; MOTOR(30+5*Diff,30-5*Diff); } //MOTOR(30,30); //while(FrontPSD()>=20); Stop(); wait(0.3); } voidgohome_1()//一号回家程序 {inti; Turn_R();//右转 Stop(); wait(0.5);
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- RoboCup 机器人 灭火 冠军 比赛 程序
![提示](https://static.bdocx.com/images/bang_tan.gif)