0063]在右轉彎時，FPGA首先把行走直線很短的距離R90_Leading按照控制器不同搜尋速度和加速度要求生成滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。在滅火機器人沿著當前房間格向前探索過程中，傳感器S2、S3會對左擋墻進行判斷，并記錄儲存當前搜尋房間擋墻信息，滅火機器人根據前進方向左擋墻的房間信息進入單左墻導航模式，然后再結合設定的左擋墻導航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl記錄滅火機器人實時的加速度、速度和角度信號并送給FPGA，FPGA實時記錄儲存滅火機器人的瞬時加速度、速度和位置信息，當滅火機器人快速探索脫離了設定中心位置時，微處理器根據離開中心位置的偏差由FPGA轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器C1、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PWM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。通過此方式可以精確調整滅火機器人的姿態，使其重新回到設定中心位置。
[0064]當到達既定目標時，傳感器參考值R90_FrOntWallRef開始工作，防止外界干擾開始做誤差補償。誤差補償結束后開始調整直流無刷電機X和直流無刷電機Y速度使其完成弧度ARC的運動曲線。FPGA首先把弧度ARCll按照控制器不同搜尋速度和加速度要求轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，PWM波控制信號通過驅動橋放大后推動滅火機器人完成轉彎搜尋。在滅火機器人轉彎搜尋過程中，傳感器S2、S3、S4、S5無法為系統提供位置參考，系統依靠方向傳感器Dl進行位置修正。在滅火機器人快速搜尋轉彎過程中方向傳感器Dl實時記錄其瞬時角度，并與設定位置的角度對比，當滅火機器人快速搜尋脫離了設定位置時，在新的采樣周期內，FPGA把偏差大小轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PffM控制信號，兩軸伺服系統開始進行實時補償來調整滅火機器人的姿態，使其完成弧度 ARCll ;
當完成ARCll轉入前校正后，FPGA把弧度ARC12按照控制器不同搜尋速度和加速度要求轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，PWM波控制信號通過驅動橋放大后推動滅火機器人完成轉彎搜尋。在滅火機器人轉彎搜尋過程中，系統依靠方向傳感器Dl進行位置修正。在滅火機器人快速搜尋轉彎過程中方向傳感器Dl實時記錄其瞬時角度，并與設定位置的角度對比，當滅火機器人快速搜尋脫離了設定位置時，在新的采樣周期內，FPGA把偏差大小轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PWM控制信號，兩軸伺服系統開始進行實時補償來調整滅火機器人的姿態，使其完成弧度ARC12 ; 當完成弧度ARC12轉彎后，滅火機器人為弧度轉出做準備。FPGA把弧度ARC13按照控制器不同搜尋速度和加速度要求轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，PWM波控制信號通過驅動橋放大后推動滅火機器人完成轉彎搜尋。在滅火機器人轉彎轉出過程中，系統依靠方向傳感器Dl進行位置修正。在滅火機器人快速搜尋轉彎過程中方向傳感器Dl實時記錄其瞬時角度，并與設定位置的角度對比，當滅火機器人快速搜尋脫離了設定位置時，在新的采樣周期內，FPGA把偏差大小轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PffM控制信號，兩軸伺服系統開始進行實時補償來調整滅火機器人的姿態，使其完成弧度ARC13 ;
當到達既定目標后，系統依靠傳感器S2、S3開始導航，FPGA把直線行走很短的距離R90_Passing按照控制器不同搜尋速度和加速度要求生成滅火機器人直流無刷電機X和直流無刷電機Y要運行的位置、速度和加速度指令值，FPGA再結合電流傳感器C1、C2和電機光電編碼器的反饋生成控制直流無刷電機X和直流無刷電機Y的PWM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。在滅火機器人沿著當前房間格向前探索過程中，傳感器S2、S3會對左擋墻進行判斷，并記錄儲存當前搜尋房間擋墻信息，滅火機器人根據前進方向左擋墻的房間信息進入單左墻導航模式，然后再結合設定的左擋墻導航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl記錄滅火機器人實時的加速度、速度和角度信號并送給FPGA，FPGA實時記錄儲存滅火機器人的瞬時加速度、速度和位置信息，當滅火機器人快速探索脫離了設定中心位置時，微處理器根據離開中心位置的偏差由FPGA轉化為滅火機器人直流無刷電機X和直流無刷電機Y要運行的新的位置、速度和加速度指令值，FPGA再結合電流傳感器Cl、C2、電機光電編碼器、加速計傳感器Al、陀螺儀Gl和方向傳感器Dl的反饋，微調直流無刷電機X和直流無刷電機Y的PffM控制信號，控制信號經驅動橋放大后驅動直流無刷電機X和直流無刷電機Y向前運動。通過此方式可以精確調整滅火機器人的姿態，使其重新回到設定中心位置。當到達既定目標后完成整個右轉彎的軌跡曲線運動，然后控制其開始更新其坐標和方向。
[0065]如果在坐標(X，Y)時的方向為北，則更新其坐標為(X+1，Y)，新坐標方向為東；如果在坐標(X，Y)時的方向為東，則更新其坐標為(X，Y-1)，新坐標方向為南；如果在坐標(X，Y)時的方向為南，則更新其坐標為(X-1，Y)，新坐標方向為西；如果在坐標(X，Y)時的方向為西，則更新其坐標為(X，Y+1)，新坐標方向為北；
11)在滅火機器人沿著當前方向向前運動過程中如果有擋墻進入前方的運動范圍，并且此時左右的傳感器S2和S3判斷左邊無擋墻，而S4、S5判斷右方有擋墻時，滅火機器人將存儲此時坐標(X，Y)，然后滅火機器人將按照圖8的曲線行走；為了充分發揮FPGA的快速處理數據能力，舍棄了基于LM629的三段法轉入法，為了使轉彎更逼近于實際情況，本發明采用五段法完成轉彎:轉入前直線校正L90_Leading，轉入弧度校正ARC21，轉入弧度ARC22，轉出弧度校正ARC23，轉出直線校正L90_Passing。
[0066]在左轉彎時，控制器控制電機行走規律與右轉類似。當通過五段法到達既定目標后完成整個左轉彎的軌跡曲線運動，然后控制器開始更新其坐標和方向。
[0067]如果在坐標(X，Y)時的方向為北，則更新其坐標為(X-1，Y)，新坐標方向為西；如果在坐標(X，Y)時的方向為東，則更新其坐標為(X，Y+1)，新坐標方向為北；如果在坐標(X，Y)時的方向為南，則更新其坐標為(X+1，Y)，新坐標方向為東；如果在坐標(X，Y)時的方向為西，則更新其坐標為(X，Y-1 )，新坐標方向為南；
12)當滅火機器人到達一個新的房間格后，光電傳感器S7開始工作，對新坐標下的光源進行判斷，如果光電傳感器捕捉到信號將通知STM32F407發現目標，STM32F407會開啟圖像采集系統進行光源分析，經確定火源無誤后，圖像采集系統再次工作:首先控制器根據圖像采集系統確定干冰滅火器噴嘴與蠟燭之間的角度，FPGA按照旋轉角度、角加速度要求，把此角度轉化為角度、角速度、角加速度指令值，FPGA結合電機電流Cl、C2和光電編碼器的反饋，自動調整直流無刷電機X和直流無刷電機Y的PWM波控制信號，使得電機X正轉，電機Y反轉，在加速度計Al、陀螺儀Gl和方向傳感器Dl控制下滅火機器人開始原地旋轉調整噴嘴與蠟燭之間的角度，當到達設定角度后，控制器使得滅火機器人原地自鎖；
控制器根據圖像采集系統確定噴嘴與火源之間的高度差，FPGA按照距離、速度、加速度要求，把此高度差轉化為位置、速度、加速度指令值，FPGA結合電機E的電流和光電編碼器的反饋，自動調整直流電機E的PffM波控制信號，然后驅動直流電機E工作，使得滅火器的噴嘴高度與蠟燭火源高度一致，控制器開啟攜帶的干冰滅火器的電磁閥，開始對蠟燭進行噴灑干冰直至光源消失，圖像采集系統二次判斷火源，確定滅火完成后，