广茂达机器人灭火程序纯C语言版.docx
- 文档编号:11117587
- 上传时间:2023-02-25
- 格式:DOCX
- 页数:30
- 大小:17.47KB
广茂达机器人灭火程序纯C语言版.docx
《广茂达机器人灭火程序纯C语言版.docx》由会员分享,可在线阅读,更多相关《广茂达机器人灭火程序纯C语言版.docx(30页珍藏版)》请在冰豆网上搜索。
广茂达机器人灭火程序纯C语言版
floattim_1=0.0;/*时间变量:
不用*/
floatgf_1=0.0;/*速度功率值*/
intmic_1=0;/*声音值,AI8或AI12*/
intgi_1=0;/*用控制是否跳出某房间循环的量:
任务完成与否控制量*/
intgi_2=0;/*相对的左边火焰亮度值*/
intgi_3=0;/*相对的左右边火焰亮度值的差值*/
intgi_4=0;/*相对的右边火焰亮度值*/
intgi_5=0;/*高中、小学程序切换*/
intgi_6=0;/*二号房回家时用来数线的量*/
intgi_7=0;/*第一次去二号房时看到的有没有火的标记量*/
intgi_8=0;/*地面灰度设置值*/
intgi_9=0;/*侧面PSD测距值*/
intgi_10=0;/*区分环境光与蜡烛光的参考值*/
intma_1=0;/*前左PSD值AI0*/
intma_2=0;/*正前PSD值AI1*/
intma_3=0;/*前右PSD值AI2*/
intma_4=0;/*后右PSD值AI3*/
intma_5=0;/*正后PSD值AI4*/
intma_6=0;/*后左PSD值AI5*/
intma_7=0;/*前地面灰度值AI6*/
intma_8=0;/*后地面灰度值AI7*/
intma_9=0;/*回家时所用的地面灰度的二次测量值*/
intma_10=0;/*正前正后PSD测距值,例值220*/
intsci_1=0;/*指南针读值*/
voidmain()
{
SCI_Set(0,9600,0,8,1);/*串口设置:
指南针的端口设置,小学、初中不用*/
/*****************************************************************************/
/*以下为调试参数*****************************************************************/
gi_10=450;/*区分环境光与蜡烛光的参考值,约比环境光最小值小50,例值:
450*/
gi_8=310;/*地面灰度设置值,原则:
稳定地区分黑白,取黑白值的中间值,例值:
300*/
gi_9=280;/*侧面PSD测距值,例值:
280*/
ma_10=280;/*正前正后PSD测距值,例值280*/
gi_5=1;/*小学取1,初高中取0*****/
gf_1=1.0;/*功率值,范围0.0~1.0*/
/*****************************************************************************/
while
(1)
{
mic_1=AI(8);
if(mic_1>750)
{
break;
}
}
//SetMotor(0x1111,100,100,0,0);
SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0);
wait(0.300000);
while
(1)
{/*四号房*****************************/
/************************************************************/
while
(1)
{
ma_7=AI(6);
if(ma_7 { gi_2=AI(9); gi_4=AI(11); if((gi_2 { //SetMotor(0x1111,90,10,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(10*gf_1),0,0);/*有火时进四号房*************/ wait(0.100000); //SetMotor(0x1111,80,80,0,0); SetMotor(0x1111,(int)(80*gf_1),(int)(80*gf_1),0,0);/*有火时进四号房*************/ wait(0.050000); while (1) { ma_7=AI(6); gi_2=AI(9); if((ma_7 { StopMotor(0x1111); //SetMotor(0x1111,100,-100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(-100*gf_1),0,0); wait(0.080000); StopMotor(0x1111); DO(0x1,1); tim_1=seconds(); wait(0.500000);/*机器人停顿灭火时间*/ DO(0x1,0); /*gi_5=1;*小学***/ break; } else { SubRoutine_2(); } } if(gi_5==1) { gi_1=1; break; } SubRoutine_3(); gi_1=1; break; } else/*出四号房*************/ { StopMotor(0x1111); wait(0.180000); //SetMotor(0x1111,-60,-100,0,0); SetMotor(0x1111,(int)(-40*gf_1),(int)(-100*gf_1),0,0); wait(0.300000); break; } } else { SubRoutine_1(); } } if(gi_1==1) { break; } /*三号房*****************************/ /************************************************************/ while (1) { ma_8=AI(7); if(ma_8 { gi_2=AI(13); gi_4=AI(15); if((gi_2 { //SetMotor(0x1111,-80,-10,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(-10*gf_1),0,0);/*有火时进三号房*************/ wait(0.100000); while (1) { ma_8=AI(7); gi_2=AI(15); if((ma_8 { StopMotor(0x1111); DO(0x10,1); tim_1=seconds(); wait(0.500000);/*机器人停顿灭火时间*/ DO(0x10,0); /*gi_5=1;*小学***/ break; } else { SubRoutine_5(); } } if(gi_5==1) { gi_1=1; break; } SubRoutine_6(); gi_1=1; break; } else { StopMotor(0x1111);/*没火时出三号房*************/ wait(0.180000); //SetMotor(0x1111,100,15,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(15*gf_1),0,0); wait(0.400000); break; } } else { SubRoutine_4(); } } if(gi_1==1) { break; } /*一(-2-1)号房*****************************/ /************************************************************/ while (1) { ma_7=AI(6); if(ma_7 { gi_2=AI(9); gi_4=AI(11); if((gi_2 { gi_7=1; } StopMotor(0x1111);/*第一次从二号门出***********/ wait(0.180000); //SetMotor(0x1111,-80,-20,0,0); SetMotor(0x1111,(int)(-10*gf_1),(int)(-80*gf_1),0,0); wait(0.40000); while (1) { ma_8=AI(7); if(ma_8 { gi_2=AI(13); gi_4=AI(15); if((gi_2 { //SetMotor(0x1111,-80,-15,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(-15*gf_1),0,0);/*有火时进一号房*************/ wait(0.200000); while (1) { ma_8=AI(7); gi_2=AI(15); if((ma_8 { StopMotor(0x1111); DO(0x10,1); tim_1=seconds(); wait(0.500000);/*机器人停顿灭火时间*/ DO(0x10,0); /*gi_5=1;*小学***/ break; } else { SubRoutine_5(); } } if(gi_5==1) { gi_1=1; break; } SubRoutine_10(); gi_1=1; break; } else { StopMotor(0x1111); wait(0.180000); //SetMotor(0x1111,-60,60,0,0); SetMotor(0x1111,(int)(-60*gf_1),(int)(60*gf_1),0,0);/**最后去2号房之前(即从一号去二号)**/ wait(0.020000); StopMotor(0x1111); //SetMotor(0x1111,100,55,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(55*gf_1),0,0); ma_2=AI (1); ma_3=AI (2); while((ma_2<=ma_10)&&(ma_3<=gi_9))/**最后去2号房之前**/ { ma_2=AI (1); ma_3=AI (2); }/**最后去2号房之前**/ SetMotor(0x1111,(int)(70*gf_1),(int)(70*gf_1),0,0); wait(0.2); SetMotor(0x1111,(int)(50*gf_1),(int)(-50*gf_1),0,0); wait(0.2); gi_1=2; break; } } else { SubRoutine_4(); } if((gi_1==2)||(gi_1==1)) { break; } } if((gi_1==2)||(gi_1==1)) { break; } } else { SubRoutine_7(); } } if(gi_1==1) { break; } /*二号房*****************************/ /************************************************************/ while (1) { ma_7=AI(6); if(ma_7 { gi_2=AI(9); gi_4=AI(11); if(gi_7==1)gi_2=10;/*第一次已*//********************/ if((gi_2<1023)||(gi_4 { //SetMotor(0x1111,50,80,0,0); SetMotor(0x1111,(int)(50*gf_1),(int)(80*gf_1),0,0); wait(0.100000); //SetMotor(0x1111,25,70,0,0); SetMotor(0x1111,(int)(10*gf_1),(int)(100*gf_1),0,0); wait(0.3500); while (1) { ma_7=AI(6); gi_2=AI(11); if((ma_7 { StopMotor(0x1111); DO(0x1,1); tim_1=seconds(); wait(0.500000);/*机器人停顿灭火时间*/ DO(0x1,0); /*gi_5=1;*小学***/ break; } else { SubRoutine_2(); } } if(gi_5==1) { gi_1=1; break; } StopMotor(0x1111); SubRoutine_9(); gi_1=1; break; } else { StopMotor(0x1111); wait(0.180000); break; } } else { SubRoutine_7(); } } if(gi_1==1) { break; } break;/**/ } StopMotor(0x1111); } /***********************************************************************/ /***********************************************************************/ /***********************************************************************/ /***********************************************************************/ /**以下为子程序*********************************************************/ voidSubRoutine_1()/*沿左墙*/ { ma_1=AI(0); ma_2=AI (1); if((ma_1>gi_9)&&(ma_2 { //SetMotor(0x1111,100,100,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(100*gf_1),0,0); } else { if((ma_1 { //SetMotor(0x1111,100,25,0,0); SetMotor(0x1111,(int)(100*gf_1),(int)(20*gf_1),0,0);/*行走过程中转弯速度***/ } else { if((ma_1>gi_9)&&(ma_2>ma_10)) { //SetMotor(0x1111,-40,40,0,0); SetMotor(0x1111,(int)(-40*gf_1),(int)(40*gf_1),0,0); } else { //SetMotor(0x1111,90,25,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(25*gf_1),0,0);/*行走过程中转弯速度***/ } } } //wait(0.001000); } voidSubRoutine_2()/*趋光*/ { gi_3=AI(9)-AI(11); if(gi_3<-10) { //SetMotor(0x1111,50,13,0,0); SetMotor(0x1111,(int)(80*gf_1),(int)(13*gf_1),0,0); } else { if(gi_3>10) { //SetMotor(0x1111,13,50,100,100); SetMotor(0x1111,(int)(13*gf_1),(int)(80*gf_1),100,100); } else { //SetMotor(0x1111,90,90,0,0); SetMotor(0x1111,(int)(90*gf_1),(int)(90*gf_1),0,0); } } //wait(0.001000); } /****************************************************/ voidSubRoutine_3()/*4号回家************/ { //SetMotor(0x1111,-100,-40,100,100); SetMotor(0x1111,(int)(-100*gf_1),(int)(-40*gf_1),100,100); wait(0.250000); DO(0x111111,0); ma_9=1023; resettime(); while (1) { ma_8=AI(7); if(ma_8 { wait(0.080000); ma_9=AI(7); } if(ma_9<=gi_8) { StopMotor(0x1111); break; } else { tim_1=seconds(); if(tim_1<1.100000) { SubRoutine_4(); } else { SubRoutine_8(); } } } } /****************************************************/ voidSubRoutine_4()/*沿右墙B*/ { ma_5=AI(4); ma_6=AI(5); if((ma_6>gi_9)&&(ma_5 { //SetMotor(0x1111,-100,-100,0,0); SetMotor(0x1111,(int)(-100*gf_1),(int)(-100*gf_1),0,0); } else { if((ma_6 { //SetMotor(0x1111,-100,-25,0,0); SetMotor(0x1111,(int)(-100*gf_1),(int)(-20*gf_1),0,0);/*行走过程中转弯速度***/ } else { if((ma_6>gi_9)&&(ma_5>ma_10)) { //SetMotor(0x1111,40,-40,0,0); SetMotor(0x1111,(int)(40*gf_1),(int)(-40*gf_1),0,0); } else { //SetMotor(0x1111,-90,-25,0,0); SetMotor(0x1111,(int)(-90*gf_1),(int)(-20*gf_1),0,0);/*行走过程中转弯速度***/ } } } //wait(0.001000); } voidSubRoutine_5()/*趋光B*/ { gi_3=AI(13)-AI(15); if(gi_3<-10) { //SetMotor(0x1111,-13,-60,0,0); SetMotor(0x1111,(int)(-13*gf_1),(int)(-80*gf_1),0,0); } else { if(gi_3>10) { //SetMotor(0x1111,-60,-13,0,0); SetMotor(0x1111,(int)(-80*gf_1),(int)(-13*gf_1),0,0); } else { //SetMotor(0x1111,-90,-90,0,0); SetMotor(0x1111,(int)(-90*gf_1),(int)(-90*gf_1),0,0); } } //wait(0.001000); } /****************************************************/ voidSubRoutine_6()/*三号回家*/ { //SetMotor(0x1111,-100,100,0,0);
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 广茂达 机器人 灭火 程序 语言版
![提示](https://static.bdocx.com/images/bang_tan.gif)