广茂达机器人灭火程序纯C语言版.docx

广茂达机器人灭火程序纯C语言版.docx

《广茂达机器人灭火程序纯C语言版.docx》由会员分享，可在线阅读，更多相关《广茂达机器人灭火程序纯C语言版.docx（30页珍藏版）》请在冰豆网上搜索。

广茂达机器人灭火程序纯C语言版

广茂达机器人灭火程序（纯C语言版）.txt看一个人的的心术，要看他的眼神；看一个人的身价，要看他的对手；看一个人的底牌，要看他的朋友。

明天是世上增值最快的一块土地，因它充满了希望。

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）;

}

/*********************************