智能消防機(jī)器人

1、智能消防機(jī)器人目錄第一章 引言.21.1 課題背景2 1.2 Intelligent Design and manufacture of electric cars Fire.21.3 實(shí)現(xiàn)功能31.4 模擬房子介紹3第二章 系統(tǒng)整體方案設(shè)計(jì).42.1 系統(tǒng)硬件設(shè)計(jì).42.2 系統(tǒng)軟件設(shè)計(jì).4第三章 硬件設(shè)計(jì).5 3.1 電源管理模塊5 3.11穩(wěn)壓芯片LM7805、7806CV5 3.12電源模塊電路原理圖.5 3.2 電機(jī)驅(qū)動(dòng)芯BTS7960.6 3.21 BTS7960的邏輯功能.6 3.22 外形及封裝.6 3.23 BTS7960電路原理圖.73.3地面灰度檢測(cè)傳感器 ST188.7

2、3.3.1 ST188特點(diǎn).73.3.2 檢測(cè)原理73.3.3 應(yīng)用范圍73.3.4 外形尺寸（單位mm）73.3.5 ST188原理圖.83.4火焰?zhèn)鞲衅?83.4.1火焰?zhèn)鞲衅魇褂?8 3.5報(bào)警電路.8第四章 軟件設(shè)計(jì)94.1 滅火機(jī)器人行進(jìn)路線分析94.2 軟件流程圖114.3軟件開(kāi)發(fā)平臺(tái)介紹.11第五章 調(diào)試記錄及實(shí)驗(yàn)心得125.1 調(diào)試記錄.12參考文獻(xiàn).13附錄: 程序清單.13第一章 引言1.1課題背景如今國(guó)內(nèi)外對(duì)消防設(shè)備的研究越來(lái)越重視，投入也越來(lái)越多。慢慢趨向于自動(dòng)化、智能化。實(shí)現(xiàn)滅火、火場(chǎng)偵查、危險(xiǎn)物品泄露探測(cè)、破拆等功能。本文設(shè)計(jì)主要完成的功能是撲火救人。本設(shè)計(jì)是基于S

3、TC89C52單片機(jī)對(duì)電動(dòng)車(chē)進(jìn)行控制的自動(dòng)控制系統(tǒng)，研究的內(nèi)容有：主要方案論證、硬件設(shè)計(jì)、軟件設(shè)計(jì)、系統(tǒng)實(shí)物調(diào)試。硬件設(shè)計(jì)主要有電機(jī)驅(qū)動(dòng)電路、熱光源采集電路、聲音采集電路、電風(fēng)扇驅(qū)動(dòng)電路、停車(chē)信號(hào)采集電路、LCD顯示電路、電源電路及單片機(jī)最小系統(tǒng)。本系統(tǒng)以STC89C52單片機(jī)作為控制核心，通過(guò)接受到熱光源采集電路傳送的信號(hào)和聲音采集電路傳送的信號(hào)，對(duì)電動(dòng)車(chē)電機(jī)進(jìn)行控制，從而實(shí)現(xiàn)對(duì)電動(dòng)車(chē)的轉(zhuǎn)向控制。當(dāng)兩處著火，一處是物品，另一處是人著火；電動(dòng)車(chē)通過(guò)聲音識(shí)別，優(yōu)先將人身上的火撲滅。其所實(shí)現(xiàn)的功能相當(dāng)于簡(jiǎn)易消防機(jī)器人?！娟P(guān)鍵詞】 消防車(chē) 熱光源 STM32單片機(jī) LM298 ST1781.2 In

4、telligent Design and manufacture of electric cars FireAbstract Today, fire-fighting equipment at home and abroad more and more emphasis on the study, input more and more. Slowly tends to automation and intelligence. To achieve fire fighting, fire detection, hazardous materials leak detection, ripper

5、 and other functions. This function is primarily designed to complete fire fighting to save people. The design is based STC89C52 microcontroller to control for electric vehicle control system to study the contents of the following: the main program feasibility studies, hardware design, software desi

6、gn, system debugging in kind. Hardware design, main motor drive circuit, thermal light source acquisition circuit, the sound collection circuit, fan drive circuit, stopping the signal acquisition circuit, LCD display circuit, power circuit and microcontroller minimum system. The system STC89C52 micr

7、ocomputer as the control core, through the acquisition circuit receives light transmitted thermal signal and voice signal acquisition circuit transmission of electric vehicle motors to be controlled in order to achieve steering control for electric vehicles. When the two fire, one is the items, anot

8、her is a human on fire; electric vehicle through voice recognition, give priority to the human body fire. They achieve the functional equivalent of simple fire-fighting robot.【Key words】： fire engine 、hot light、 STM32 MCU 、 LM298 ST1781.3 實(shí)現(xiàn)功能制造一個(gè)自主控制的機(jī)器人在一間平面結(jié)構(gòu)房子模型里運(yùn)動(dòng)，找到一根蠟燭并盡快將它熄滅，這個(gè)工作受地面摩擦、機(jī)器人慣性、

9、機(jī)器人電機(jī)的轉(zhuǎn)數(shù)差、齒輪箱與輪子的摩擦、電壓變化等多個(gè)因素影響，它模擬了現(xiàn)實(shí)家庭中機(jī)器人處理火警的過(guò)程，蠟燭代表家里燃起的火源，機(jī)器人必須找到并熄滅它。1.4 模擬房子介紹模擬房子平面圖單位：mm比賽場(chǎng)地的墻壁22cm高，由KT板做成。墻壁為白色。比賽場(chǎng)地的地板將是貼有導(dǎo)航黑線的KT板。所有的房間和走廊的地板上都是光滑的。場(chǎng)地中所有的門(mén)口并沒(méi)有門(mén)，而是一個(gè)適當(dāng)寬度的開(kāi)口。第二章 系統(tǒng)整體方案設(shè)計(jì)2.1 系統(tǒng)硬件設(shè)計(jì)本次設(shè)計(jì)的目的是設(shè)計(jì)一個(gè)在規(guī)定區(qū)域能自主搜索火源并實(shí)施滅火的智能機(jī)器人小車(chē)，本次設(shè)計(jì)使用的主控芯片使用了STC89C52單片機(jī)，所以設(shè)計(jì)重點(diǎn)在傳感器和電機(jī)驅(qū)動(dòng)上。系統(tǒng)總體設(shè)計(jì)框圖如圖

10、2.1：MCU小車(chē)電機(jī)驅(qū)動(dòng)傳感器模塊傳感器模塊電源部分水槍電機(jī)圖2.1 系統(tǒng)總體設(shè)計(jì)框圖2.2 系統(tǒng)軟件設(shè)計(jì)轉(zhuǎn)彎子程序軟件設(shè)計(jì)方案是以上述硬件電路為基礎(chǔ)的，包括電機(jī)控制模塊、傳感器模塊的程序設(shè)計(jì)與實(shí)現(xiàn)。程序設(shè)計(jì)采用C語(yǔ)言編寫(xiě)，編程環(huán)境是集成Keil STM32編譯器的集成編譯環(huán)境。滅火機(jī)器人設(shè)計(jì)的軟件設(shè)計(jì)結(jié)構(gòu)框圖如圖2.2所示。滅火機(jī)器人系統(tǒng)軟件電機(jī)控制模塊傳感器模塊前進(jìn)子程序停止子程序灰度子程序壁障子程序火焰子程序火焰子程序轉(zhuǎn)彎子程序轉(zhuǎn)彎子程序 圖2.2 系統(tǒng)軟件設(shè)計(jì)框圖 第三章 硬件設(shè)計(jì)3.1電源管理模塊電源是任何一個(gè)系統(tǒng)穩(wěn)定運(yùn)行的前提條件，為了使機(jī)器人運(yùn)行穩(wěn)定，單片機(jī)和電機(jī)的供電系統(tǒng)采用

11、獨(dú)立供電的方法。3.1.1穩(wěn)壓芯片LM7805CV、LM7806CVLM7805CV的技術(shù)指標(biāo)如下表：表3-1 穩(wěn)壓芯片7805參數(shù)LM7806CV的技術(shù)指標(biāo)如下表：表3-2 穩(wěn)壓芯片7806參數(shù)3.1.2電源模塊電路原理圖由于單片機(jī)及所有的傳感器系統(tǒng)供電采用的是5V和6V的電源,所以在電源的處理上采用了穩(wěn)壓芯片7805CV和7806CV?；鹧孀映绦?.2電機(jī)驅(qū)動(dòng)芯片 BTS7960BTS7960是NovalithIC家族三個(gè)獨(dú)立的芯片的一部分:一是p型通道的高電位場(chǎng)效應(yīng)晶體管，二是一個(gè)n型通道的低電位場(chǎng)效應(yīng)晶體管，結(jié)合一個(gè)驅(qū)動(dòng)晶片,形成一個(gè)完全整合的高電流半橋。所有三個(gè)芯片是安裝在一個(gè)共同的

12、引線框,利用芯片對(duì)芯片和芯片芯片技術(shù)。電源開(kāi)關(guān)應(yīng)用垂直場(chǎng)效應(yīng)管技術(shù)來(lái)確保最佳的阻態(tài)。由于p型通道的高電位開(kāi)關(guān)，需要一個(gè)電荷泵消除電磁干擾。通過(guò)驅(qū)動(dòng)集成技術(shù)，邏輯電平輸入、電流取樣診斷、轉(zhuǎn)換速率調(diào)整器，失效發(fā)生時(shí)間、防止欠電壓、過(guò)電流、短路結(jié)構(gòu)輕易地連接到一個(gè)微處理器上。BTS7960可結(jié)合其他的BTS7960形成全橋和三相驅(qū)動(dòng)結(jié)構(gòu)。3.2.2外形及引腳結(jié)構(gòu)：3.2.3 BTS7960電路原理圖：3.3地面灰度檢測(cè)傳感器ST1783.3.1 ST178簡(jiǎn)介： ST178H紅外光電傳感器模塊是基于ST178H傳感器設(shè)計(jì)的一款紅外反射式光電開(kāi)關(guān)。傳感器采用高發(fā)射功率紅外光電二極管和高靈敏度光電晶體管

13、組成，輸出信號(hào)經(jīng)施密特電路整形，穩(wěn)定可靠。3.3.2 ST178特點(diǎn)：1、采用高發(fā)射功率紅外光電二極管和高靈敏度光電晶體管組成。2、檢測(cè)距離可調(diào)整范圍大，4-10mm可用。3、采用非接觸檢測(cè)方式。3.3.3 檢測(cè)原理： 傳感器的紅外發(fā)射二極管不斷發(fā)射紅外線，當(dāng)發(fā)射出的紅外線沒(méi)有被反射回來(lái)或被反射回來(lái)但強(qiáng)度不夠大時(shí)，光敏三極管一直處于關(guān)斷狀態(tài)，此時(shí)模塊的輸出端為低電平，指示二極管一直處于熄滅狀態(tài)；被檢測(cè)物體出現(xiàn)在檢測(cè)范圍內(nèi)是，紅外線被反射回來(lái)且強(qiáng)度足夠大，光敏三極管飽和，此時(shí)模塊的輸出端為高電平，指示二極管被點(diǎn)亮。3.3.4 外形尺寸（單位mm）：圖3.5 ST188實(shí)物圖3.3.5 ST178

14、與單片機(jī)連接原理圖： 圖3.4 ST178電路圖3.4火焰?zhèn)鞲衅鳎捍藗鞲衅鞅酒房蓮V泛應(yīng)用于滅火機(jī)器人比賽中測(cè)量火焰值、足球比賽時(shí)，用于確定足球的方向。下圖為火焰?zhèn)鞲衅鲗?shí)物圖。圖3.8 火焰?zhèn)鞲衅鲗?shí)物圖3.5報(bào)警電路：當(dāng)單片機(jī)的P1.0 I/O口輸出一個(gè)高電平時(shí)，通過(guò)非門(mén)后使三極管基級(jí)為低電平，此時(shí)三極管處于截止?fàn)顟B(tài)，蜂鳴器不工作；當(dāng)單片機(jī)的P1.0 I/O口輸出一個(gè)低電平，通過(guò)非門(mén)后使三極管基級(jí)變?yōu)楦唠娖?，三極管處于放大工作狀態(tài)，驅(qū)動(dòng)蜂鳴器發(fā)出報(bào)警聲音。聲音報(bào)警電路如圖3.11所示。 第四章 軟件設(shè)計(jì) 4.1 滅火機(jī)器人行進(jìn)路線分析結(jié)合我們小車(chē)的特點(diǎn)和前面分析，我們選擇3214的遍歷順序。開(kāi)始

15、時(shí)，小車(chē)處于3號(hào)和4號(hào)房間中間，由圖可知，沿著右走的方案比較好，因此我們采用是右手規(guī)則，首先搜索的是3號(hào)房間，如圖。當(dāng)在3號(hào)房間發(fā)現(xiàn)火源時(shí)，小車(chē)進(jìn)入房間并滅火，滅火后按原路返回；如沒(méi)有發(fā)現(xiàn)火源，小車(chē)?yán)^續(xù)按右手規(guī)則搜索房間，直到搜索4號(hào)房間，不管有沒(méi)有搜索到火源，從4號(hào)房間出來(lái)都繞著4號(hào)房間返回起點(diǎn)，因?yàn)榛丶疫^(guò)程中的時(shí)間不記入總時(shí)間，而繞行比較安全，小車(chē)比較好控制。4.2 軟件流程圖圖4.2 滅火小車(chē)軟件設(shè)計(jì)流程圖4.3 軟件開(kāi)發(fā)平臺(tái)介紹本次設(shè)計(jì)軟件的開(kāi)發(fā)主要采用Keil uVision4軟件編寫(xiě)。使用Keil uVision4工具時(shí)，項(xiàng)目開(kāi)發(fā)流程和其它軟件開(kāi)發(fā)項(xiàng)目的流程極其相似：1、創(chuàng)建一個(gè)項(xiàng)

16、目，從器件庫(kù)中選擇目標(biāo)器件，配置工具設(shè)置；2、用C語(yǔ)言或匯編語(yǔ)言創(chuàng)建源程序；3、用項(xiàng)目管理器生成你的應(yīng)用；4、修改源程序中的錯(cuò)誤；5、測(cè)試，連接應(yīng)用。編程語(yǔ)言選用C語(yǔ)言。它可以作為工作系統(tǒng)設(shè)計(jì)語(yǔ)言，編寫(xiě)系統(tǒng)應(yīng)用程序，也可以作為應(yīng)用程序設(shè)計(jì)語(yǔ)言，并且C語(yǔ)言以其結(jié)構(gòu)化，容易維護(hù)，編寫(xiě)不依賴計(jì)算機(jī)硬件的應(yīng)用程序，容易移植的優(yōu)勢(shì)滿足開(kāi)發(fā)的需要。而匯編語(yǔ)言作為傳統(tǒng)的嵌入式系統(tǒng)的編程語(yǔ)言，具有執(zhí)行效率高的優(yōu)點(diǎn)，但其本身是低級(jí)語(yǔ)言，編程效率較低，可移植性和可讀性差，維護(hù)極不方便。而Keil STM32編譯器完全遵照ANSI C語(yǔ)言標(biāo)準(zhǔn)，支持C語(yǔ)言的所有標(biāo)準(zhǔn)特性。另外，直接支持STM32結(jié)構(gòu)的幾個(gè)特性被添加到

17、里面。Keil宏匯編器支持STM32及其派生系列的全部指令集。 第五章 調(diào)試記錄5.1 調(diào)試記錄n 地面灰度傳感器：測(cè)試距離2.5cm,黑地面輸出電壓1.3-1.5V；白紙輸出3.8-4.5V；n 前方火焰?zhèn)鞲衅髯钸h(yuǎn)測(cè)試距離2.5m，此次使用有效距離0.8m，輸出電壓0.6V，探測(cè)角度+30°。n 熱光源信號(hào)采集電路調(diào)試：熱光源采集電路分為四路，對(duì)四個(gè)方向的光強(qiáng)進(jìn)行采集。根據(jù)光強(qiáng)電壓轉(zhuǎn)換原理：光越強(qiáng)，則電壓越高；光強(qiáng)越弱，電壓越低。將熱光源信號(hào)采集模塊中的靈敏度調(diào)節(jié)到最佳狀態(tài)。將蠟燭火焰靠近紅外接收二極管，調(diào)節(jié)對(duì)應(yīng)的參考電壓的可調(diào)電位器，使對(duì)應(yīng)輸出指示燈變亮（即接收到了熱光源信號(hào)，輸

18、出低電平）。再不斷改變蠟燭與紅外接收二極管之間的距離，將檢測(cè)距離調(diào)節(jié)到最遠(yuǎn)時(shí)，達(dá)2.1米。此時(shí)靈敏度也最佳。但是在外界環(huán)境光比較強(qiáng)的地方，熱光源信號(hào)采集電路常出現(xiàn)錯(cuò)誤判斷。數(shù)據(jù)記錄如下表1：測(cè)量輸出左路中左路中右路右路正確輸出左路中左路中右路右路測(cè)量101110111測(cè)量210011011測(cè)量31011101測(cè)量401101110表一根據(jù)分析，外界光也有近紅外，當(dāng)紅外接收二極管暴露在外時(shí)，受外界紅外光的影響，產(chǎn)生錯(cuò)誤判斷。解決的辦法是，將紅外接收二極管用直紙筒卷起來(lái)，防止外界環(huán)境影響。其它模塊電路出現(xiàn)的小問(wèn)題比較容易解決，因?yàn)殡娐废鄬?duì)來(lái)說(shuō)簡(jiǎn)單，方便調(diào)試。整體電路連接調(diào)試后，比較成功，系統(tǒng)工作穩(wěn)

19、定。 從實(shí)驗(yàn)調(diào)試結(jié)果分析可得，系統(tǒng)基本實(shí)現(xiàn)了熱光源信號(hào)的采集和電動(dòng)車(chē)運(yùn)動(dòng)方向的控制。參考文獻(xiàn)1 國(guó)際賽制機(jī)器人滅火比賽規(guī)則.PDF2 李全利、遲榮強(qiáng). 單片機(jī)原理及接口技術(shù). 北京：高等教育出版社，2004.13 譚浩強(qiáng). C程序設(shè)計(jì)(第二版). 北京：清華大學(xué)出版社，1999.124 童詩(shī)白、華成英. 模擬電子技術(shù)基礎(chǔ)(第三版). 北京：高等教育出版社，2003.12 5 康華光. 電子技術(shù)基礎(chǔ) 數(shù)字部分(第四版). 北京： 高等教育出版社，1900.16 黃智偉. 全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽電路設(shè)計(jì). 北京：北京航空航天大學(xué)出版社，2006.127 黃智偉. 全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽系統(tǒng)設(shè)計(jì).

20、北京：北京航空航天大學(xué)出版社，2006.12 8 文艷、譚鴻. Protel 99 SE電子電路設(shè)計(jì). 北京：機(jī)械工業(yè)出版社，2006.8程序請(qǐng)單附錄1: #include "stm32f10x_lib.h"#include "public.h"u8 time_3ms = 0;u8 depart=0;u8 restart=0;u8 room=0;u8 gangway=0;u8 white=0;u8 room0=0;u8 room1=0;u8 room2=0;u8 gangway3=0;u8 gangway4=0;u8 sensor_temp_ahead=

21、0;u8 sensor_temp_ahead0=0;u8 sensor_temp_back=0;u8 sensor_temp_back0=0;u8 proceed=0;s8 memory=0; int main(void) system_init(); while(1) if(time_3ms >= 3) time_3ms = 0; /smokecheck(); if(depart=0) Encoder_Total=0; handle(-10); motor_speed(AHEAD_TURN,2); memory=2; while(Encoder_Total<1050); memo

22、ry=0; Encoder_Total=0; handle(-17); motor_speed(AHEAD_TURN,2); memory=2; while(Encoder_Total<3600); memory=0; room=1; depart=1; if(room=1) sensor_temp_ahead = sensor_ahead(); if(sensor_temp_ahead!=sensor_temp_ahead0|proceed=1) proceed=0; sensor_temp_ahead0=sensor_temp_ahead;switch(sensor_temp_ahe

23、ad) case 1: handle(-10); motor_speed(AHEAD_TURN,2); break; case 2: handle(-5); motor_speed(AHEAD,1); break; case 3: handle(0); motor_speed(AHEAD,1); break; case 4: handle(5); motor_speed(AHEAD,1); break; case 5: handle(10); motor_speed(AHEAD_TURN,2); break; case 0: if(white=0) white=1; Encoder_Total

24、=0; handle(19); motor_speed(AHEAD_TURN,2); memory=2; while(Encoder_Total<3000); memory=0; Encoder_Total=0; handle(20); motor_speed(AHEAD_TURN,2); memory=2; while(Encoder_Total<2500); memory=0; secure=0; dispose(); proceed=1; else handle(-18); motor_speed(AHEAD_TURN,2); break; case 6: /全黑 handl

25、e(0); motor_speed(0,0); secure=0; dispose(); gangway=1; room=0; break; if(gangway=1) sensor_temp_back = sensor_back(); if(sensor_temp_back!=sensor_temp_back0|proceed=1) proceed=0; sensor_temp_back0=sensor_temp_back;switch(sensor_temp_back) case 1: handle(16); motor_speed(BACK,-1); break; case 2: han

26、dle(13); motor_speed(BACK,-1); break; case 3: handle(10); motor_speed(BACK,-1); break; case 4: handle(7); motor_speed(BACK,-1); break; case 5: handle(0); motor_speed(BACK,-1); break; case 6: handle(-5); motor_speed(BACK,-1); break; case 7: handle(-10); motor_speed(BACK,-1); break; case 8: handle(-15

27、); motor_speed(BACK,-1); break; case 9:/全黑 handle(0); motor_speed(0,0); room=2; gangway=0; Encoder_Total=0; break; case 0: /全白 handle(17); motor_speed(BACK_TURN,-1); break; if(room=2) sensor_temp_ahead = sensor_ahead(); if(sensor_temp_ahead!=sensor_temp_ahead0|proceed=1) proceed=0; sensor_temp_ahead

28、0=sensor_temp_ahead;switch(sensor_temp_ahead) case 1: handle(-15); motor_speed(AHEAD_TURN,2); break; case 2: handle(-8); motor_speed(AHEAD,1); break; case 3: handle(0); motor_speed(AHEAD,1); break; case 4: handle(8); motor_speed(AHEAD,1); break; case 5: handle(15); motor_speed(AHEAD_TURN,2); break;

29、case 0: /全白 if(room1=0&&Encoder_Total<1400) handle(15); motor_speed(AHEAD,1); if(room1=0&&Encoder_Total>=1400) handle(-1); motor_speed(AHEAD,1); room1=1; else handle(18); motor_speed(AHEAD,1); break; case 6: /全黑 secure=0; dispose(); room=0; gangway=2; break; if(gangway=2) senso

30、r_temp_back = sensor_back(); if(sensor_temp_back!=sensor_temp_back0|proceed=1) proceed=0; sensor_temp_back0=sensor_temp_back;switch(sensor_temp_back) case 1: handle(18); motor_speed(BACK_TURN,-2); break; case 2: handle(15); motor_speed(BACK_TURN,-2); break; case 3: handle(10); motor_speed(BACK,-1);

31、break; case 4: handle(8); motor_speed(BACK,-1); break; case 5: handle(0); motor_speed(BACK,-1); break; case 6: handle(-10); motor_speed(BACK,-1); break; case 7: handle(-16); motor_speed(BACK_TURN,-1); break; case 8: handle(-19); motor_speed(BACK_TURN,-2); break; case 9:/全黑 Encoder_Total=0; handle(-2

32、); motor_speed(AHEAD,1); memory=1; while(Encoder_Total<2200); memory=0; gangway=0; room=3; break; case 0: /全白 if(room2=0) Encoder_Total=0; handle(0); motor_speed(BACK,-1); memory=-1; while(Encoder_Total<100); memory=0; Encoder_Total=0; room2=1; else if(Encoder_Total<=8000) handle(16); motor

33、_speed(BACK,-1); if(Encoder_Total>8000&&Encoder_Total<=18000) handle(-20); motor_speed(BACK_TURN,-2); else handle(20); motor_speed(BACK,-1); break; if(room=3) sensor_temp_ahead = sensor_ahead(); if(sensor_temp_ahead!=sensor_temp_ahead0|proceed=1) proceed=0; sensor_temp_ahead0=sensor_te

34、mp_ahead;switch(sensor_temp_ahead) case 1: handle(-10); motor_speed(AHEAD_TURN,2); break; case 2: handle(-3); motor_speed(AHEAD,1); break; case 3: handle(0); motor_speed(AHEAD,1); break; case 4: handle(3); motor_speed(AHEAD,1); break; case 5: handle(10); motor_speed(AHEAD_TURN,2); break; case 0: /全白

35、 handle(18); motor_speed(AHEAD_TURN,2); break; case 6: room=0; gangway=3; break; if(gangway=3) if(gangway3=0) Encoder_Total=0; handle(-2); motor_speed(AHEAD_TURN,2);memory=2; while(Encoder_Total<1800);memory=0;Encoder_Total=0; handle(-20); motor_speed(AHEAD_TURN,2);memory=2; while(Encoder_Total&l

36、t;2200);handle(0); motor_speed(AHEAD_TURN,2);memory=0;gangway3=1;sensor_temp_ahead = sensor_ahead();if(sensor_temp_ahead!=6)handle(0); motor_speed(AHEAD_TURN,2);else room=4; gangway=0; if(room=4) sensor_temp_ahead = sensor_ahead(); if(sensor_temp_ahead!=sensor_temp_ahead0|proceed=1) proceed=0; sensor