使能中驅(qū)的兩路直流無刷電機(jī)PffM波輸出，調(diào)整滅火機(jī)器人直流無刷電機(jī)X、直流無刷電機(jī)Y的PWM波輸出，使得兩個永磁直流無刷電機(jī)運動方向相反，滅火機(jī)器人原地在加速計傳感器Al的控制下實現(xiàn)精確的原地180度轉(zhuǎn)向，然后滅火機(jī)器人沿著原先相反的方向運動。
[0048]如果在坐標(biāo)(X，Y)時的方向為北，則更新其坐標(biāo)為(X，Y)，新坐標(biāo)方向為南；如果在坐標(biāo)(X，Y)時的方向為東，則更新其坐標(biāo)為(XI，Y)，新坐標(biāo)方向為西；如果在坐標(biāo)(X，Y)時的方向為南，則更新其坐標(biāo)為(X，Y)，新坐標(biāo)方向為北；如果在坐標(biāo)(X，Y)時的方向為西，則更新其坐標(biāo)為(X，Y)，新坐標(biāo)方向為東；
9)在滅火機(jī)器人沿著當(dāng)前方向向前運動過程中如果有擋墻進(jìn)入前方的運動范圍，并且此時左右的傳感器S2和S3判斷左邊有擋墻，而S4、S5判斷右方?jīng)]有擋墻時，滅火機(jī)器人將存儲此時坐標(biāo)(X，Y)，然后滅火機(jī)器人將按照圖7的曲線行走；
在右轉(zhuǎn)彎時，STM32F407首先把行走直線很短的距離R90_Leading按照控制器不同搜尋速度和加速度要求生成速度-時間運動梯形圖，這個梯形包含的面積就是滅火機(jī)器人兩個直流無刷電機(jī)X和直流無刷電機(jī)Y要運動的距離。STM32F407根據(jù)這個梯形圖再結(jié)合光電編碼器和電機(jī)電流反饋生成驅(qū)動兩軸直流無刷電機(jī)的PWM波，然后STM32F407使能驅(qū)動芯片驅(qū)動兩個獨立直流無刷電機(jī)X和直流無刷電機(jī)Y向前運動，在向前運動過程中，傳感器S2、S3、S4、S5實時對左右的擋墻進(jìn)行判斷，并反饋當(dāng)前房間擋墻信息，滅火機(jī)器人進(jìn)入單左墻導(dǎo)航模式，然后再結(jié)合設(shè)定的左擋墻導(dǎo)航閥值，加速度計Al記錄滅火機(jī)器人實時的加速度信號并送給控制器，控制器分別通過積分和二次積分得到瞬時角速度和角度，記錄滅火機(jī)器人的瞬時加速度、速度和位置信息，當(dāng)滅火機(jī)器人快速探索脫離了設(shè)定中心位置時，微處理器根據(jù)離開中心位置的偏差借助加速計傳感器Al開始進(jìn)行實時補(bǔ)償，微調(diào)電機(jī)的PffM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置；如果滅火機(jī)器人在校正位置階段出現(xiàn)失速或者房間地面灰塵較多的情況，STM32F407會調(diào)節(jié)M加大滅火機(jī)器人與地面的摩擦并使能后驅(qū)的兩輪PWM波控制信號，控制器開啟直流無刷電機(jī)Z和直流無刷電機(jī)R，系統(tǒng)進(jìn)入四輪驅(qū)動狀態(tài)，STM32F407根據(jù)時間和速度要求，把剩余的距離D轉(zhuǎn)化為四軸直流無刷電機(jī)的PffM波，使直流無刷電機(jī)X、直流無刷電機(jī)Y、直流無刷電機(jī)Z和直流無刷電機(jī)R以相同的速度前進(jìn)，滅火機(jī)器人在四驅(qū)狀態(tài)下依舊按照原有的左擋墻導(dǎo)航模式前進(jìn)，加速度計Al實時記錄滅火機(jī)器人的瞬時運動加速度，通過積分和二次積分得到其速度和位置信號，當(dāng)滅火機(jī)器人四輪探索脫離了設(shè)定中心位置時，微處理器根據(jù)離開中心位置的偏差借助加速度計Al開始進(jìn)行實時補(bǔ)償，微調(diào)四軸直流無刷電機(jī)的PffM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置；
當(dāng)?shù)竭_(dá)既定目標(biāo)時，傳感器參考值R90_FrOntWallRef開始工作，防止外界干擾開始做誤差補(bǔ)償。誤差補(bǔ)償結(jié)束后開始調(diào)整直流無刷電機(jī)X和直流無刷電機(jī)Y速度使其完成弧度ARC的運動曲線，STM32F407首先把弧度ARC按照控制器不同搜尋速度和加速度要求生成速度-時間運動梯形圖，這個梯形包含的面積就是滅火機(jī)器人兩個直流無刷電機(jī)要運動的距離。然后STM32F407再結(jié)合光電編碼器以及電流傳感器Cl、C2的反饋生成驅(qū)動直流無刷電機(jī)運動的PWM波，PWM波通過驅(qū)動橋放大后推動滅火機(jī)器人完成轉(zhuǎn)彎搜尋。在滅火機(jī)器人轉(zhuǎn)彎搜尋過程中，傳感器S2、S3、S4、S5無法為系統(tǒng)提供位置參考，系統(tǒng)依靠加速度計Al進(jìn)行位置修正。在滅火機(jī)器人快速搜尋轉(zhuǎn)彎過程中加速度計Al實時記錄其瞬時加速度，控制器然后通過積分和二次積分得到其速度和位置信息，并通過與設(shè)定位置的速度和角度對比，當(dāng)滅火機(jī)器人快速搜尋脫離了設(shè)定位置時，在新的采樣周期內(nèi)，系統(tǒng)按照偏差大小微調(diào)直流無刷電機(jī)的PWM波輸入，兩軸伺服系統(tǒng)開始進(jìn)行實時補(bǔ)償來調(diào)整滅火機(jī)器人的姿態(tài)，使其完成弧度ARC ;
當(dāng)?shù)竭_(dá)既定目標(biāo)后，系統(tǒng)依靠傳感器S2、S3開始導(dǎo)航，控制器把直線行走很短的距離R90_Passing按照不同控制器搜尋速度和加速度要求生成速度-時間運動梯形圖，這個梯形包含的面積就是滅火機(jī)器人中置兩個直流無刷電機(jī)X和電機(jī)Y要運動的距離。STM32F407根據(jù)這個梯形圖再結(jié)合光電編碼器和直流無刷電機(jī)電流反饋生成驅(qū)動兩軸直流無刷電機(jī)的PWM波，然后STM32F407使能驅(qū)動芯片驅(qū)動兩個獨立直流無刷電機(jī)X和直流無刷電機(jī)Y向前運動，在向前加速過程中，傳感器S2、S3實時對左擋墻進(jìn)行判斷，并反饋當(dāng)前房間擋墻信息，滅火機(jī)器人進(jìn)入單左墻導(dǎo)航模式，然后再結(jié)合設(shè)定的左擋墻導(dǎo)航閥值，加速度計Al記錄滅火機(jī)器人實時的加速度信號并送給控制器，控制器分別通過積分和二次積分得到瞬時角速度和角度，記錄滅火機(jī)器人的瞬時加速度、速度和位置信息，當(dāng)滅火機(jī)器人快速探索脫離了設(shè)定中心位置時，微處理器根據(jù)離開中心位置的偏差借助加速計傳感器Al開始進(jìn)行實時補(bǔ)償，微調(diào)直流無刷電機(jī)的PWM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置。如果滅火機(jī)器人在轉(zhuǎn)出校正位置階段出現(xiàn)失速或者房間地面灰塵較多的情況，STM32F407會調(diào)節(jié)M加大滅火機(jī)器人與地面的摩擦并使能后驅(qū)的兩輪PffM波控制信號，控制器開啟直流無刷電機(jī)Z和直流無刷電機(jī)R，系統(tǒng)進(jìn)入四輪驅(qū)動狀態(tài)，STM32F407根據(jù)時間和速度要求，把剩余的距離D轉(zhuǎn)化為四軸直流無刷電機(jī)的PffM波，使直流無刷電機(jī)X、直流無刷電機(jī)Y、直流無刷電機(jī)Z和直流無刷電機(jī)R以相同的速度前進(jìn)，滅火機(jī)器人在四驅(qū)狀態(tài)下依舊按照原有的左擋墻導(dǎo)航模式前進(jìn)，加速度計Al實時記錄滅火機(jī)器人的瞬時運動加速度，通過積分和二次積分得到其速度和位置信號，當(dāng)滅火機(jī)器人四輪探索脫離了設(shè)定中心位置時，微處理器根據(jù)離開中心位置的偏差借助加速度計Al開始進(jìn)行實時補(bǔ)償，微調(diào)四軸直流無刷電機(jī)的PWM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置；當(dāng)?shù)竭_(dá)既定目標(biāo)后完成整個右轉(zhuǎn)彎的軌跡曲線運動，然后控制其開始更新其坐標(biāo)和方向。
[0049]如果在坐標(biāo)(X，Y)時的方向為北，則更新其坐標(biāo)為(X+1，Y)，新坐標(biāo)方向為東；如果在坐標(biāo)(X，Y)時的方向為東，則更新其坐標(biāo)為(X，Y-1)，新坐標(biāo)方向為南；如果在坐標(biāo)(X，Y)時的方向為南，則更新其坐標(biāo)為(X-1，Y)，新坐標(biāo)方向為西；如果在坐標(biāo)(X，Y)時的方向為西，則更新其坐標(biāo)為(X，Y+1)，新坐標(biāo)方向為北；
10)在滅火機(jī)器人沿著當(dāng)前方向向前運動過程中如果有擋墻進(jìn)入前方的運動范圍，并且此時左右的傳感器S2和S3判斷左邊無擋墻，而S4、S5判斷右方有擋墻時，滅火機(jī)器人將存儲此時坐標(biāo)(X，Y)，然后滅火機(jī)器人將按照圖8的曲線行走；
在左轉(zhuǎn)彎時，控制器控制電機(jī)行走規(guī)律與右轉(zhuǎn)類似，當(dāng)通過三段法到達(dá)既定目標(biāo)后完成整個左轉(zhuǎn)彎的軌跡曲線運動，然后控制器按照圖6的設(shè)置開始更新其坐標(biāo)和方向。
[0050]如果在坐標(biāo)(X，Y)時的方向為北，則更新其坐標(biāo)為(X-1，Y)，新坐標(biāo)方向為西；如果在坐標(biāo)(X，Y)時的方向為東，則更新其坐標(biāo)為(X，Y+1)，新坐標(biāo)方向為北；如果在坐標(biāo)(X，Y)時的方向為南，則更新其坐標(biāo)為(X+1，Y)，新坐標(biāo)方向為東；如果在坐標(biāo)(X，Y)時的方向為西，則更新其坐標(biāo)為(X，Y-1 )，新坐標(biāo)方向為南； 11)當(dāng)滅火機(jī)器人到達(dá)一個新的房間格后，光電傳感器S7開始工作，對新坐標(biāo)下的光源進(jìn)行判斷，如果光電傳感器捕捉到信號將通知STM32F407發(fā)現(xiàn)目標(biāo)，STM32F407會開啟基于CCD的圖像采集系統(tǒng)進(jìn)行光源分析，經(jīng)確定火源無誤后，圖像采集系統(tǒng)再次工作:首先控制器根據(jù)圖像采集系統(tǒng)確定干冰滅火器噴嘴噴射方向與蠟燭之間的角度，STM32F407首先禁止后驅(qū)的兩路PWM工作，按照旋轉(zhuǎn)角度、角加速度要求，把此角度轉(zhuǎn)化為角度、角速度、角加速度指令值，然后再結(jié)合電機(jī)電流C1、C2和光電編碼器的反饋，自動調(diào)整直流無刷電機(jī)X和電機(jī)Y的PWM波控制信號，使得電機(jī)X正轉(zhuǎn)，電機(jī)Y反轉(zhuǎn)，在加速度計Al控制下滅火機(jī)器人開始原地旋轉(zhuǎn)調(diào)整噴嘴與蠟燭之間的角度，當(dāng)?shù)竭_(dá)設(shè)定角度后，控制器使得滅火機(jī)器人原地自鎖；
STM32F407再根據(jù)圖像采集系統(tǒng)確定噴嘴與火源之間的高度差，STM32F407按照距離、速度、加速度要求，把此高度差轉(zhuǎn)化為位置、速度、加速度指令值，然后再結(jié)合電機(jī)E的電和光電編碼器的反饋，自動調(diào)整直流電機(jī)E的PffM波控制信號，然后驅(qū)動直流電機(jī)E工作，使得滅火器的噴嘴高度與蠟燭火源高度一致，控制器開啟攜帶的干冰滅火器的電磁閥，開始對蠟燭進(jìn)行噴灑干冰直至光源消失，圖像采集系統(tǒng)二次判斷火源，確定滅火完成后，控制器關(guān)閉圖像采集系統(tǒng)；STM32F407自動調(diào)整直流無刷電機(jī)X和電機(jī)Y的PffM波控制信號，使得電機(jī)X反轉(zhuǎn)，電機(jī)Y正轉(zhuǎn)，在加速度計Al控制下滅火機(jī)器人開始原地旋轉(zhuǎn)，并恢復(fù)到剛?cè)雽m的位置；
如果光電傳感器S7沒有捕捉到新坐標(biāo)下的光源，滅火機(jī)器人將離開目前房間格，將繼續(xù)搜尋并更新其坐標(biāo)；
12)當(dāng)滅火機(jī)器人搜到光源，并噴灑干冰完