2F407+FPGA控制器，在電源打開狀態下，滅火機器人先進入自鎖狀態。控制器首先開啟真空抽吸電機M，通過抽吸裝置先對微型真空吸盤抽吸，使真空吸盤對地面具有一定的吸附能力。滅火機器人通過超聲波傳感器S1、S6判斷前方運動環境，實際導航環境被轉化為控制參數傳輸給FPGA，FPGA把這些環境參數轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PffM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動，加速度計Al、陀螺儀Gl和方向傳感器Dl和光電編碼器實時把測得的信號反饋給FPGA，由FPGA 二次校正滅火機器人的姿態。滅火機器人在運動過程中，FPGA根據滅火機器人運動速度自動通過電機M調節真空裝置對地面的吸附能力，增加有效摩擦，防止滅火機器人快速行走打滑，STM32F407實時儲存房間信息。當控制器發現火源后，圖像采集系統開啟。控制器在圖像采集幫助下，自動調整滅火器噴嘴與火源的角度，同時升降電機E自動調節滅火器的高度，然后自動開啟干冰滅火器電磁閥，通過噴灑干冰滅火，滅火完畢后STM32F407立即調出滅火機器人已經儲存的房間信息，通過洪水算法找出返程最短路徑，并開啟加速模式迅速回到起點等待一下條尋求火源命令。
[0054]基于兩輪高速全自動滅火機器人的雙核伺服控制系統的具體的功能實現如下:
I)為了能夠驅動兩輪滅火機器人運動并解決高速探尋房間打滑問題，本控制系統引入了 FPGA，由其產生四路控制直流無刷電機和直流電機的PWM波控制信號，FPGA通過I/O 口與STM32F407進入實時通訊，由STM32F407控制其開通和關斷。
[0055]2)打開電源瞬間，STM32F407會對電池電壓進行檢測，如果低壓的話，FPGA將封鎖直流無刷電機X和直流無刷電機Y的PffM波控制信號，電機不能啟動，同時電壓傳感器Vl將工作，并發出報警信號。如果系統電壓正常，FPGA將開啟真空抽吸電機M的PffM波控制信號，通過抽吸裝置先對微型真空吸盤抽吸，使真空吸盤對地面具有一定的吸附能力，滿足滅火機器人探尋火源的速度要求。
[0056]3)在滅火機器人未接到探索命令之前，它一般會在起點坐標(0，0)等待控制器發出的探索命令，一旦接到任務后，會沿著起點開始為了尋找火源而進行全宮探索。
[0057]4)滅火機器人放在起點坐標(0，0)，一般情況下，滅火機器人按照圖6中北的方向(計算機編程代碼為O)放置，接到任務后其前方的傳感器S1、S6和會對前方的環境進行判斷，確定有沒有擋墻進入運動范圍，如存在擋墻將向STM32F407發出存儲命令，STM32F407會對中斷做第一時間響應，然后通過調整FPGA的輸出控制直流無刷電機X正轉，直流無刷電機Y反轉，滅火機器人在加速度傳感器Al、陀螺儀Gl和方向傳感器Dl的控制下向右旋轉90度，滅火機器人首先沿著X軸正向(東的方向，計算機編程代碼為2)搜尋火源。
[0058]5)在滅火機器人運動過程中，裝在直流無刷電機X和直流無刷電機Y上的光電編碼器會輸出其位置信號A和位置信號B，光電編碼器的位置信號A脈沖和B脈沖邏輯狀態每變化一次，FPGA的位置寄存器會根據直流無刷電機X和直流無刷電機Y的運行方向加I或者是減I ;
6)在滅火機器人運動過程中，裝在直流無刷電機X和直流無刷電機Y上的光電編碼器的位置信號A脈沖和B脈沖和Z脈沖同時為低電平時，就產生一個INDEX信號給FPGA寄存器，記錄電機的絕對位置，然后換算成滅火機器人在房間中的具體位置。
[0059]7)為了能夠實現滅火機器人準確的坐標計算功能，滅火機器人左右的傳感器S2、S3和S4、S5會時刻對運動方向左右的房間擋墻和柱子進行探測，如果S2、S3或者S4、S5發現傳感器信號發生了躍變，則說明滅火機器人進入了從有房間擋墻到無房間擋墻(或者是從無房間擋墻到有房間擋墻)狀態的變化，STM32F407會根據滅火機器人當前運行狀態精確補償，徹底消除滅火機器人在復雜房間中探尋火源時已經累計的誤差。
[0060]8)在滅火機器人沿著任何一個方向向前運動，在任何一個方格的中心如果確定沒有擋墻進入前方的運動范圍，則滅火機器人將存儲其坐標(X，Y)，并把向前運動一格的位置參數送給FPGA，FPGA把向前一格參數轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器C1、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。在滅火機器人沿著當前房間格向前探索過程中，傳感器S2、S3和S4、S5會對左右的擋墻進行判斷，并記錄儲存當前搜尋房間擋墻信息，滅火機器人根據前進方向左右擋墻的房間信息進入單墻導航模式或者是雙墻導航模式，然后再結合設定的左右擋墻導航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl記錄滅火機器人實時的加速度、速度和角度信號并送給FPGA，FPGA實時記錄儲存滅火機器人的瞬時加速度、速度和位置信息，當滅火機器人快速探索脫離了設定中心位置時，微處理器根據離開中心位置的偏差由FPGA轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PffM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。通過此方式可以精確調整滅火機器人的姿態，使其重新回到設定中心位置。當滅火機器人在加速度計Al的控制下精確運動一格到達新地址時，微處理器將更新其坐標。
[0061]如果在坐標(X，Y)時的方向為北，在更新其坐標為(X，Y+1)，新坐標方向依舊為北；如果在坐標(X，Y)時的方向為東，在更新其坐標為(Χ+1，Y)，新坐標方向依舊為東；如果在坐標(Χ，Υ)時的方向為南，在更新其坐標為(X，Υ-1)，新坐標方向依舊為南；如果在坐標(X，Y)時的方向為西，在更新其坐標為(x-l，Y)，新坐標方向依舊為西；
9)在滅火機器人沿著當前方向向前運動過程中如果傳感器SI和S6判斷前方有擋墻進入運動范圍，并且此時傳感器S2、S3、S4、S5分別判斷左右都有擋墻時，滅火機器人將存儲此時坐標(X，Y)，根據傳感器SI和S6的反饋計算出向前運動停車的位置參數YS1，由FPGA根據探索控制器速度和加速度把向前停車距離參數轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。在滅火機器人沿著當前房間格向前探索過程中，傳感器S2、S3和S4、S5對左右擋墻進行判斷，STM32F407實時記錄儲存當前搜尋房間擋墻信息，滅火機器人根據前進方向左右擋墻的房間信息進入雙墻導航模式，然后再結合設定的左右擋墻導航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl記錄滅火機器人實時的加速度、速度和角度信號并送給FPGA，FPGA實時記錄儲存滅火機器人的瞬時加速度、速度和位置信息，當滅火機器人快速停車脫離了設定中心位置時，微處理器根據離開中心位置的偏差由FPGA轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PffM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。通過此方式可以精確調整滅火機器人的姿態，使其重新回到設定中心位置。滅火機器人實現在設置停車點停車。STM32F407通過FPGA重新調整滅火機器人兩個直流無刷電機的PWM波輸出，使得兩個永磁直流無刷電機運動方向相反，滅火機器人原地在加速計傳感器Al的控制下實現精確的原地180度轉向，然后滅火機器人沿著原先相反的方向運動。
[0062]如果在坐標(X，Y)時的方向為北，則更新其坐標為(X，Y)，新坐標方向為南；如果在坐標(X，Y)時的方向為東，則更新其坐標為(XI，Y)，新坐標方向為西；如果在坐標(X，Y)時的方向為南，則更新其坐標為(X，Y)，新坐標方向為北；如果在坐標(X，Y)時的方向為西，則更新其坐標為(X，Y)，新坐標方向為東；
10)在滅火機器人沿著當前方向向前運動過程中如果有擋墻進入前方的運動范圍，并且此時傳感器S2和S3判斷左邊有擋墻，而S4、S5判斷右方沒有擋墻時，滅火機器人將存儲此時坐標(X，Y)，然后滅火機器人將按照圖7的曲線行走；為了充分發揮FPGA的快速處理數據能力，舍棄了基于MC58113和LM629的三段法轉入法，為了使轉彎更逼近于實際情況，本發明采用五段法完成轉彎:轉入前直線校正R90_Leading，轉入弧度校正ARCl I，轉入弧度ARC12，轉出弧度校正ARC13，轉出直線校正R90_Passing。
[