GMC_DCL(char**, argv) { boolean Abort, NewFlag; int i; Init_IO(); Init_Err(); Init_Env(); Init_Sigs(FALSE); IPC_Init(); IPC_Action = TopLevelCI; if (IsServer) { Init_Sigs(TRUE); Read_DrvGrf(); Init_Info(&NewFlag); Init_FilHdrs(); Init_FilInps(); Init_FilElms(); Init_FilPrm(); Init_FilHdrTree(); Activate_Client(LocalClient); if (NewFlag) { Write_ENV2(); }/*if*/; }/*if*/; if (IsClient) { IsTTY = GetIsTTY(); Init_Vars(); Init_CWD(); }/*if*/; Read_ENV2(); if (IsClient) { if (VerifyLevel >= 1) Test_All(); if (argc <= 1) { Get_Commands(&Abort); Exit((Abort?1:0)); }/*if*/; for (i=1; i<argc; i+=1) { ;/*select*/{ if (strlen(argv[i]) == 0) { Get_Commands(&Abort); }else{ TopLevelCI(&Abort, argv[i]); };}/*select*/; if (Abort) { Exit(1); }/*if*/; }/*for*/; Exit(0); }/*if*/; IPC_Get_Commands(&Abort, (tp_Str)NIL); Exit((Abort?1:0)); return 0; /*to make lint happy*/ }/*main*/
/***************************************************************** 功能:定点清扫程序 *****************************************************************/ void Do_Spot(void) { static uint8 piv_out; //机器是否向外展开,1为向外展开,0为向里缩小 static uint8 piv_left; //机器是否向左转,1为向左转,0为向右转 uint32 m ; //=============电池电压低时进入自动回充 if(battery_voltage < LOWPOWER) { if(battery_voltage < NOPOWER) { giv_sys_err = 18; Init_Err(); } else { Init_Docking() ; Enable_Beep_Five(); } return; } //============在最开始启动时判断左右轮是否离地,否则退出扫地模式 if((giv_sys_time - mode.time) < 5000) { if((l_lidi.key == 0)||(r_lidi.key == 0)) { Init_Cease(); } return ; } if((giv_sys_time - mode.time) < 10000) { if(CheckMDustMotor()) { return; } } //===========遥控器按键管理 if(Spot_YaoKong_Manage() == 1) { return; } //===========在面板上有三个按键中的任意一个按下了 if((key1.sign == 0) || (key2.sign == 0) || (key3.sign == 0)) { key1.sign = 1; key2.sign = 1; key3.sign = 1; Init_Cease(); Enable_Beep(); return; } //===========显示显示屏的内容 Display_Real(); comm_rap(); clr_hw_effect(&l_hw); clr_hw_effect(&m_hw); clr_hw_effect(&r_hw); clr_hw_effect(&up_hw); clr_hw_effect(&dummy_wall); //=========下面是一些需要驱动的行动======== Io_Driver(); Dis_Power = Get_Dispower(); //=========有遥控器时机器停止按照遥控方向运动 if(YaoKongFangXiang()) { return; } //========是否有需要保护的内容 if(Action_Protect()) { return; } m=read_bump1();//故障检测 switch(mode.bump) //故障种类分析以及处理 { //--------------------------- case 1://E_L switch (mode.step_bp) { case 0: if(do_action(7,angle360)) //左半身旋转向左旋转,左边抽身后退 { giv_sys_err = mode.bump; Init_Err(); return ; } if((m==1)) // mode.bump=mode.step_bp=m; do not action { return ; } stop_rap(); mode.step_bp=1; case 1: if(do_action(7,angle30)) //继续抽身后退 { mode.step_bp++; } return ; case 2: if(do_action(8,angle15)) //右边半身抽身后退 { mode.step_bp++; } read_earth(); return ; case 3: if(do_action(2,angle30)) //全身右向旋转 { mode.bump = 0; } read_earth(); return; default : stop_rap(); mode.step_bp = 0; } return ; //-------------------------------------------- case 4://E_R: switch (mode.step_bp) { case 0: if(do_action(8,angle360)) { giv_sys_err = mode.bump; Init_Err(); return ; } if((m==4)) { return ; } stop_rap(); mode.step_bp=1; case 1: if(do_action(8,angle30)) { mode.step_bp++; } return ; case 2: if(do_action(7,angle15)) { mode.step_bp++; } read_earth(); return ; case 3: if(do_action(1,angle30)) { mode.bump = 0; } read_earth(); return; default : stop_rap(); mode.step_bp = 0; } return ; //-------------------------------mode.bump case 2://E_LM case 3://E_RM: case 5://L_BUMP: case 6://R_BUMP: case 7://W_L: case 8://W_LM: case 9://W_M: case 10://up_hw case 11://l_hw case 12://m_hw case 13://r_hw case 14://W_R: case 15://W_RM: switch(mode.step_bp) { case 0: if(do_action(4,angle360)) { giv_sys_err = mode.bump; Init_Err(); return ; } read_sideearth(); if(m == 0) { stop_rap(); mode.step_bp = 1; } return ; case 1: if(do_action(1,angle30)) { mode.bump = 0; } read_earth(); return; default : mode.bump = 0; } return ; default : mode.bump = 0; break; } //结束 switch(mode.bump) //====================================================== switch (mode.step) { case 0: left(angle360);//原地转圈1圈 mode.step++; piv_out = 1; piv_left = 1; break; case 1: case 2: case 3: case 4: if((!l_rap.sign)&&(!r_rap.sign)) { action.step++; if(piv_out == 0) { Init_Cease(); return; } if(piv_left != 0) //全身向左旋转 { enable_rap(BACK,(angle180-mode.step*angle45), FRONT,(angle180+mode.step*angle45)); //1940 } else //全身向右旋转 { enable_rap(FRONT,(angle180+mode.step*angle45),BACK,(angle180-mode.step*angle45) ); } mode.step++; } break; case 5: case 6: case 7: case 8: case 9: case 10: case 11: case 12: case 13: case 14: case 15: case 16: case 17: if((!l_rap.sign)&&(!r_rap.sign)) { action.step++; if(piv_left != 0) { enable_rap(FRONT, (mode.step-4)*angle45, FRONT, (mode.step-4)*angle45+angle360); } else { enable_rap(FRONT, (mode.step-4)*angle45+angle360, FRONT, (mode.step-4)*angle45); } if(mode.step == 17) { piv_out = 0; } if(piv_out != 0) //向外展开 { mode.step++; } else //向内收拢 { mode.step--; } } } }
/****************************************************************** 功能:在清扫时的工作 ******************************************************************/ void Do_Sweep(void) { static uint8 y = 0;//有碰撞信号时 y = 1;否则为0 //static uint8 z = 0;//在执行来回扫时 z = 1,否则为0 //NVIC_GenerateSystemReset(); //-----------电池电压低时进入自动回充 if(battery_voltage < LOWPOWER) { if(battery_voltage < NOPOWER) { giv_sys_err = 18; Init_Err(); } else { Init_Docking() ; Enable_Beep_Five(); } return; } //-----------在最开始启动时判断左右轮是否离地,否则退出扫地模式 if((giv_sys_time - mode.time) < 5000) { if((l_lidi.key == 0)||(r_lidi.key == 0)) { Init_Cease(); } return ; } if((giv_sys_time - mode.time) < 10000) { if(CheckMDustMotor()) { return; } } //----------遥控器按键管理 if(Sweep_YaoKong_Manage() == 1) { return; } //----------在面板上有三个按键中的任意一个按下了 if((key1.sign == 0) || (key2.sign == 0) || (key3.sign == 0)) { key1.sign = 1; key2.sign = 1; key3.sign = 1; Init_Cease(); Enable_Beep(); return; } //-----------显示显示屏的内容 Display_Real(); comm_rap(); clr_hw_effect(&l_hw); clr_hw_effect(&m_hw); clr_hw_effect(&r_hw); clr_hw_effect(&up_hw); clr_hw_effect(&dummy_wall); //----------下面是一些需要驱动的行动----------------- Io_Driver(); Dis_Power = Get_Dispower(); //----------在靠近墙壁时机器减速 if((w_l.high_sign == NEAR)||(w_lm.high_sign == NEAR)||(w_m.high_sign == NEAR)|| (w_r.high_sign == NEAR)||(w_rm.high_sign == NEAR)) { if((l_rap.rap > 1000)||(r_rap.rap > 1000)) { l_rap.rap = (l_rap.rap*2)/3; r_rap.rap = (r_rap.rap*2)/3; } } else { if((l_rap.rap < Speed)&&(r_rap.rap < Speed)&&(mode.bump == 0)) { l_rap.rap = (l_rap.rap*3)/2; r_rap.rap = (r_rap.rap*3)/2; } } //-----------在回充座附近将墙检的灵敏度设置为高 if((up_hw.sign != 0) && ((up_hw.data&0x8f)==0)) { Wall_SensitivityHigh(); mode.angle_wall = 0; } else { Wall_SensitivityLow(); } //-------------有遥控器时机器停止按照遥控方向运动 if(YaoKongFangXiang()) { return; } //-------------是否有需要保护的内容 if(Action_Protect()) { return; } if(mode.fangxiang>2)mode.fangxiang = 0; if((mode.fangxiang != 0)&&(mode.fangxiang != 1)&&(mode.fangxiang != 2)) { mode.fangxiang = 0; } //--------------下面是正常扫地的路径 if(action_bump()) { y=1; return; } //============================下面是正常在扫地时的路径=========================== //--------------------------有过碰撞,碰撞后主路径只能是直线和Z字形动作 if(1 == y) { y = 0; if((mode.step==0xfe) || (mode.step==0xff)) { mode.step = 0; } do { mode.step++; if(22 < mode.step) { mode.step = 0; } } while(3 != zhulujing[mode.step]); } if(mode.step==0xfe) //在最开始的时候 { if(!do_action(3,METE)) { return; } mode.step++; } if(mode.step ==0xff) //掉头 { if(!do_action(1,angle180)) { return; } mode.step++; } if((18 == zhulujing[mode.step]) || (19 == zhulujing[mode.step])) { //mode.step=1,5,9,11,17,21 mode.sum_bump = 0; mode.fangxiang = 0; } if((18 == zhulujing[mode.step]) || (19 == zhulujing[mode.step])||(mode.sum_bump > 10)) { //mode.step=1,5,9,11,17,21 mode.ybs_sign = 1; mode.angle_wall=angle720; mode.long_wall = 0; } else { mode.ybs_sign = 0; Enable_wall(); } if(do_action(zhulujing[mode.step],zhulujinglong[mode.step])) { mode.step++; if(mode.step > 22) { mode.step=0; } } }