广茂达机器人灭火程序(纯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_1750){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_7gi_8){gi_2=AI(9);gi_4=AI(11);if((gi_2gi_10)||(gi_4gi_10)){//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_7gi_8)&&(gi_2300))/*四号灭火*/{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_8gi_8){gi_2=AI(13);gi_4=AI(15);if((gi_2gi_10)||(gi_4gi_10)){//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_8gi_8)&&(gi_2300))/*三号灭火*/{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_7gi_8){gi_2=AI(9);gi_4=AI(11);if((gi_2gi_10)||(gi_4gi_10))/*此处不用改（第一次去二号门口测光，不进）*/{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_8gi_8){gi_2=AI(13);gi_4=AI(15);if((gi_2gi_10)||(gi_4gi_10)){//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_8gi_8)&&(gi_2300))/*一号灭火*/{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_7gi_8){gi_2=AI(9);gi_4=AI(11);if(gi_7==1)gi_2=10;/*第一次已*//********************/if((gi_21023)||(gi_4gi_10)){//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_7gi_8)&&(gi_2300))/*二号灭火*/{StopMotor(0x1111);DO(0