三軸數(shù)字陀螺儀篇

1、機器人衡態(tài)調(diào)整三軸數(shù)字陀螺儀篇ZXY北京森漢科技有限公司目錄一硬件介紹11陀螺儀芯片12陀螺儀的載板23SHR-8S機器人三軸分布34STC12C5A60S芯片接口45載板實物4二陀螺儀操作51訪問格式52軟件接口5三姿勢狀態(tài)分析51自糾正歸位系統(tǒng)介紹52自糾正跳位系統(tǒng)介紹6四姿態(tài)調(diào)整程序解析81行走姿態(tài)調(diào)整程序82靜止站立姿態(tài)調(diào)整程序193左右平移姿態(tài)調(diào)整程序25一硬件介紹1陀螺儀芯片如下圖1-1所示，三軸數(shù)字陀螺儀L3G4200D芯片X、Y、Z三軸互相正交分布。圖1-1 坐標(biāo)方向及背面圖圖1-1中明確標(biāo)出了L3G4200D芯片X、Y、Z軸的坐標(biāo)方向及背面圖。本陀螺儀芯片可供用戶選擇其尺度量

2、程，量程范圍從 ±250 dps ±2000 dps，低量程數(shù)值用于高精度慢速運動測量，而高量程則用于測量超快速的手勢和運動。這款器件提供一個16位數(shù)據(jù)輸出，以及可配置的低通和高通濾波器等嵌入式數(shù)字功能。就算時間推移或溫度變化，這款器件仍然保持連續(xù)穩(wěn)定的輸出。L3G4200D 特性（ST）：u 三種可選全尺度(±250/500/2000 存保計劃）u I2C/SPI 數(shù)字輸出接口u 16 比特率值的數(shù)據(jù)輸出u 8 比特溫度數(shù)據(jù)輸出u 兩個數(shù)字輸出線（中斷和 data ready）u 集成低和高通濾波器的用戶可選帶寬u 在時間和溫度上平穩(wěn)u 嵌入式 self-tes

3、tu 寬電源電壓，2.4 V 到 3.6 Vu 低電壓兼容的 IOS，1.8 Vu 嵌入式 power-down 和睡眠模式u 嵌入式溫度傳感器u 嵌入式 FIFO 緩存u 高抗撞擊能力u 擴展的工作溫度范圍(-40°C到+85°C)u ECOPACK® RoHS 和 “Green”認(rèn)證2陀螺儀的載板（1）本次采用陀螺儀的IIC接口方式，訪問陀螺儀內(nèi)部的三軸數(shù)字值。其每一軸的數(shù)字值均為有符號的16進制數(shù)，以補碼的形式存放。采用單片機兩個I/O口（P4.1與P3.3）模擬IIC與陀螺儀IIC接口（SCL、SDA）相接，使用100kHz采樣速率對陀螺儀內(nèi)部寄存器配置與

4、三軸數(shù)字值的讀取。其中三軸X、Y、Z數(shù)字值的存放寄存器分別為OUT_X_L與OUT_X_H、OUT_Y_L與OUT_Y_H、OUT_Z_L與OUT_Z_H，只要對它們進行讀取即可。其應(yīng)用載板正面及接線提示圖如下圖1-2所示。GNDSCL+5VSDA圖1-2 載板正面圖（2）陀螺儀芯片的載板背面圖如圖1-3所示。YZX圖1-3 載板背面圖3SHR-8S機器人三軸分布在此特別的說明一下，這里講到的三軸與L3G4200D芯片載板的三軸完全重合。根據(jù)三軸數(shù)字陀螺儀載板在機器人身長的插接方式，如下圖1-4所示的坐標(biāo)。YXZYZXY、Z平面ZXYXYZX、Y平面Y、Z平面圖1-4 載板在機器人上的坐標(biāo)圖4

5、STC12C5A60S芯片接口STC12C5A60S芯片是SHR-8S人形機器人的主控板上面的MCU，它負(fù)責(zé)機器人行走步伐函數(shù)的發(fā)生，另外還負(fù)責(zé)紅外線接收與識別，與上位機三維圖形化軟件進行通訊的功能。本次再給它加一個任務(wù)就是“三軸數(shù)字陀螺儀的數(shù)據(jù)采樣和分析”。所以使用該芯片的IIC采樣端口，對L3G4200D進行三軸數(shù)字值采樣。具體的端口連接參考下面圖1-5。圖1-5 STC12C5A60S芯片接口圖根據(jù)上圖1-5，使用如下的端口協(xié)議：STC12C5A60AD芯片：P4.1 - 載板SCL； P3.3 - 載板SDA；5載板實物載板的實物圖，如下圖1-6所示。實物暫時未出圖1-6 載板實物圖二

6、陀螺儀操作本陀螺儀操作接口含有IIC與SPI兩種，由于考慮到SPI接口多占用I/O口的問題，所以此次采用IIC接口方式對陀螺儀芯片操作，即一根時鐘線SCL和一根數(shù)據(jù)線SDA連接陀螺儀，實現(xiàn)數(shù)據(jù)傳輸。1訪問格式基于標(biāo)準(zhǔn)IIC通信格式，對陀螺儀內(nèi)部寄存器進行訪問。采樣數(shù)率使用100kHz，分別依次從六個寄存器（每一軸的數(shù)字值由低8位與高8位組合）讀取出三軸X、Y、Z的數(shù)字值，其中六個寄存器分別為OUT_X_L與OUT_X_H、OUT_Y_L與OUT_Y_H、OUT_Z_L與OUT_Z_H。2軟件接口基于標(biāo)準(zhǔn)IIC的通信協(xié)議，編寫訪問陀螺儀內(nèi)部數(shù)據(jù)的接口子程序（void Read_IIC_XYZAx

7、is(void) ）；其依次讀取出每一軸的高低8位，再分別組合存放于有符號16位的整型變量當(dāng)中，共3個變量（L3G4200D_dis_data0、L3G4200D_dis_data1、L3G4200D_dis_data2），分別對應(yīng)于X、Y、Z三軸的角速度值。由于用于控制機器人相對較快行走，所以降低陀螺儀返回的角速度值，以縮小100倍處理。三姿勢狀態(tài)分析機器人在行走運動時欲維持其各種平衡狀態(tài)，首先要研究立正姿勢下的平衡方程。由于立正姿勢本身就是一種靜止?fàn)顟B(tài)，而且它相對于所有的穩(wěn)定狀態(tài)都可以直接過渡，所以要把立正姿勢下的各種平衡關(guān)系分類研究透徹。首先要探討一下平衡的概念，所謂的平衡，也就是說系統(tǒng)

8、在一定范圍內(nèi)保持著規(guī)律性的運動。比如說鐘擺本身就是一種特殊的平衡，還有像彈簧的往復(fù)震動，小孩子玩的陀螺不停的旋轉(zhuǎn)等等都是一種穩(wěn)定狀態(tài)也就是一種平衡。在人體的研究范疇里面，平衡狀態(tài)的種類是最多的，它的種類要比天上的戰(zhàn)斗機還多上百倍。經(jīng)?？吹降碾s技演員，在高空做的各種驚險動作，其實都是在從一個穩(wěn)定態(tài)變化到另外的一個穩(wěn)定態(tài)的過程。在這個過程中，我們發(fā)現(xiàn)過渡過程都是運動的，而穩(wěn)定態(tài)都是靜止的。不過，我們把各種狀態(tài)歸類總結(jié)成一下2種。1自糾正歸位系統(tǒng)介紹在日常生活中經(jīng)常會碰到類似的情況，正常行走的行人一不留神，腳踩到一個西瓜皮后，這個人掙扎了幾下又直起身來，差一點就摔到了。這種現(xiàn)象歸類為自動糾正歸位系統(tǒng)

9、現(xiàn)象，如下面的流程圖3-1所示。穩(wěn)定態(tài)（A）過渡態(tài)跳段出返回段穩(wěn)定態(tài)可以是靜態(tài)也可以是動態(tài)。過渡態(tài)都是動態(tài)?！疤龆巍笔怯赏饨绺蓴_系統(tǒng)而產(chǎn)生?！胺祷囟巍笔怯煞答伭窟M入系統(tǒng)而產(chǎn)生。圖3-1 自糾正歸位系統(tǒng)圖當(dāng)穩(wěn)定的系統(tǒng)受到外界干擾的時候，系統(tǒng)就面臨著失穩(wěn)的風(fēng)險。如果系統(tǒng)的自糾正能力很強，那么他就會很快地進行自我糾正并回到初始的穩(wěn)定態(tài)。上面的例子里，正常行走的行人就是一個穩(wěn)定態(tài)，它本身就是動態(tài)平衡的，行走本身就是一種周期性的運動。突然踩到一個西瓜皮代表一個很強的外界干擾因素隨機性地進入系統(tǒng)，行人馬上會手舞足蹈地翻騰起來，因為他不想摔倒。這個翻騰的過程就是過度態(tài)，它相對來說會比較短暫。隨后這個人依靠

10、軀干和四肢的運動，保持住了自己的重心，沒有摔倒。最后，他又繼續(xù)向前行走。我們把踩到西瓜皮的瞬間歸為跳出段，把四肢翻騰的過程歸為返回段。2自糾正跳位系統(tǒng)介紹還是剛才的那個行人，可是這回的情況不同了，這次他最后摔倒了。這個行人，他起初是在正常行走，一不留神，腳踩到一個西瓜皮，這個人掙扎了幾下卻無濟于事，結(jié)果就摔到了。這種現(xiàn)象歸類為自動跳位系統(tǒng)現(xiàn)象，如下面的流程圖3-2所示。穩(wěn)定態(tài)（A）過渡態(tài)跳段出跳位段穩(wěn)定態(tài)（A）可以是靜態(tài)也可以是動態(tài)?！疤欢巍币彩怯煞答伭窟M入系統(tǒng)而產(chǎn)生。穩(wěn)定態(tài)（B）穩(wěn)定態(tài)（B）可以是靜態(tài)也可以是動態(tài)。圖3-2 自糾正跳位系統(tǒng)圖當(dāng)穩(wěn)定的系統(tǒng)受到外界干擾的時候，系統(tǒng)就面臨著失穩(wěn)的

11、風(fēng)險。如果系統(tǒng)的自糾正能力在某一方面不是很強，那么它就會被另外的穩(wěn)定態(tài)所代替。在這個例子里，正常行走的行人就是穩(wěn)定態(tài)（A）。突然踩到一個西瓜皮代表一個很強的外界干擾因素隨機性地進入系統(tǒng)，行人馬上會四肢翻騰，這個翻騰的過程就是過度態(tài)。行人在拼命保持自己的重心，但是最后他還是摔倒了。摔倒后，坐在了地上就是穩(wěn)定態(tài)（B）。我們把踩到西瓜皮的瞬間歸類為跳出段，把四肢翻騰但是仍然倒下的過程歸類為跳位段。參考以上的兩個自糾正系統(tǒng)的特點，下面進行研究“人體立正姿勢、雙腿劈叉姿勢、單腿站立姿勢、原地踏步姿勢、雙足行走姿勢等”各種姿勢的穩(wěn)定性和過渡性的特點。本次使用陀螺儀，就是用于控制機器人處于這兩種自糾正系統(tǒng)下

12、維持它的穩(wěn)態(tài)平衡。也就是，在每一種穩(wěn)態(tài)情況下，在一定較小時間內(nèi)啟動陀螺儀維持該穩(wěn)態(tài)平衡，使其當(dāng)前穩(wěn)態(tài)的姿勢狀況不變。比如機器人在行走一步時，走完前半步處于穩(wěn)態(tài)（A），再走完下半步處于穩(wěn)態(tài)（B）。在前半步的穩(wěn)態(tài)（A）時，啟動陀螺儀及時修正該穩(wěn)態(tài)的偏移，以維持該狀態(tài)不變。下半步依然如此。如果機器人處于站立等靜止?fàn)顟B(tài)時，就等同處于一種穩(wěn)態(tài)的情況，依然啟動陀螺儀及時修正偏移，以維持穩(wěn)態(tài)的姿勢。欲使機器人所有運動姿勢正?；?，首先調(diào)節(jié)好機器人所有運動姿勢中每一種穩(wěn)態(tài)正?；?。如下面流程圖3-3所示。穩(wěn)定態(tài)（A）可以是靜態(tài)也可以是動態(tài)。穩(wěn)定態(tài)（B）可以是靜態(tài)也可以是動態(tài)。穩(wěn)定態(tài)（ B ）“跳位段”也是由反饋量

13、進入系統(tǒng)而產(chǎn)生。穩(wěn)定態(tài)（A）過渡態(tài)維持 A穩(wěn)態(tài) 維持 B穩(wěn)態(tài)出位出跳段段跳段跳圖3-3 陀螺儀應(yīng)用系統(tǒng)圖四姿態(tài)調(diào)整程序解析1行走姿態(tài)調(diào)整程序在機器人行走過程中，當(dāng)處在雙腿著地某一姿態(tài)時，循環(huán)啟動陀螺儀周期性地采集三軸數(shù)字陀螺儀的三軸數(shù)字值，緊接著存儲在數(shù)組L3G4200D_dis_data3變量當(dāng)中，并比較數(shù)組L3G4200D_dis_data3與對應(yīng)設(shè)定角速度幅限值，依據(jù)是否超幅限值改變相應(yīng)從0到11號舵機的值，調(diào)整機器人姿勢使其保持當(dāng)前姿態(tài)穩(wěn)定。當(dāng)機器人保持在其當(dāng)前姿態(tài)時，就退出循環(huán)，繼續(xù)行走，同樣也依舊進入下一個姿態(tài)當(dāng)中去。若在某一姿態(tài)的平衡狀態(tài)被迫改變，就會同樣進入循環(huán)當(dāng)中，及時修正機

14、器人的姿勢。本調(diào)整程序工作邏輯方式是，快速反饋，快速回程。主要使用的函數(shù)名稱列舉如下：void Read_IIC_XYZAxis(void) /采集三軸數(shù)字值函數(shù)void ExecuteL3G4200D_TwoSetDown(char FB_Amplitude, char LR_Amplitude, uchar delays_value) /調(diào)整姿勢函數(shù)參數(shù)：FB_Amplitude -前后擺動旋轉(zhuǎn)角速度幅限值(陀螺儀Y軸旋轉(zhuǎn)角速度) LR_Amplitude -左右擺動旋轉(zhuǎn)角速度幅限值(陀螺儀X軸旋轉(zhuǎn)角速度) delays_value -行走速度值(一個值代表500us延時)此三個參數(shù)FB_

15、Amplitude、LR_Amplitude、delays_value分別由無線遙控器調(diào)節(jié)設(shè)定。參數(shù)在遙控器屏幕上顯示位置與相應(yīng)調(diào)整按鈕調(diào)整功能，如下從圖4-1到圖4-4所示。參數(shù)LR_Amplitude參數(shù)FB_Amplitude參數(shù)delays_value圖4-1 參數(shù)顯示位置圖按下13鍵，即進入設(shè)置陀螺儀與加速計的參數(shù)模式。按下14鍵，即退出設(shè)置陀螺儀與加速計的參數(shù)模式。圖4-2 進入與退出參數(shù)模式圖按一下25鍵，就減小速度參數(shù)一個值，但速度就快一個值。按一下26鍵，就增加速度參數(shù)一個值，但速度就慢一個值。圖4-3 速度參數(shù)設(shè)置圖按一下16鍵，就增加一個前后擺動旋轉(zhuǎn)角速度幅限值。按一下1

16、1鍵，就減小一個前后擺動旋轉(zhuǎn)角速度幅限值。按一下12鍵，就減小一個左右擺動旋轉(zhuǎn)角速度幅限值。按一下15鍵，就增加一個左右擺動旋轉(zhuǎn)角速度幅限值。圖4-4 陀螺儀參數(shù)設(shè)定圖注意：機器人在行走當(dāng)中每一種半步穩(wěn)態(tài)，只需調(diào)用該ExecuteL3G4200D_TwoSetDown函數(shù)，即可調(diào)整行走當(dāng)中因慣性導(dǎo)致機器人身體前后左右的偏移，前提是機器人的行走步伐要無誤。其中，該姿態(tài)調(diào)整函數(shù)ExecuteL3G4200D_TwoSetDown的實現(xiàn)流程圖4-5所示。開始陀螺儀是否插上？否退出調(diào)整函數(shù)是保存臨時狀態(tài)讀取陀螺儀三軸數(shù)字值前擺動角速度超限否？否是調(diào)整舵機從0到5號的值，使機器人往后傾移后擺動角速度超限

17、否？標(biāo)志前后穩(wěn)定否是調(diào)整舵機從0到5號的值，使機器人往前傾移左擺動角速度超限否？否調(diào)整舵機從8到11號的值，使機器人往右傾移右擺動角速度超限否？標(biāo)志左右穩(wěn)定否均穩(wěn)定？是調(diào)整舵機從8到11號的值，使機器人往左傾移恢復(fù)臨時狀態(tài)退出調(diào)整函數(shù)圖4-5 行走姿態(tài)調(diào)整流程圖該姿態(tài)調(diào)整函數(shù)中各塊代表代碼如下：該段代碼用于判斷陀螺儀是否插上，未插上則退出調(diào)整函數(shù)，其如下：if(L3G_flag_plug1 = 1)/*if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1)adc_adjust(); */return;陀螺儀是否插上？該段代碼用于保

18、存機器人當(dāng)前所處某一姿勢時相關(guān)要調(diào)整的舵機各值，其中舵機有從0號到11號，除了6號和7號舵機之外。具體代碼如下：position_temp0 = position0;position_temp1 = position1;position_temp2 = position2;position_temp3 = position3;position_temp4 = position4;position_temp5 = position5;position_temp8 = position8;position_temp9 = position9;position_temp10 = position10

19、;position_temp11 = position11;保存臨時狀態(tài)該段代碼用于調(diào)用讀取出陀螺儀三軸數(shù)字值的處理函數(shù)，其如下：Read_IIC_XYZAxis();讀取陀螺儀三軸數(shù)字值前擺動角速度超限否？該段代碼用于判斷機器人當(dāng)前所處某一姿勢時是否發(fā)生向前傾移，如傾移則調(diào)整舵機從0號到5號的值，使機器人向后傾移。具體代碼如下：if(L3G4200D_dis_data1 < FB_Amp0) /向前俯position0 += 1;position1 += 1;position2 -= 1;position3 -= 1;position4 -= 1;position5 += 1;PWM_

20、24();Lowlevel_500u(delays_value);是調(diào)整舵機從0到5號的值，使機器人往后傾移該段代碼用于判斷機器人當(dāng)前所處某一姿勢時是否發(fā)生向后傾移，如傾移則調(diào)整舵機從0號到5號的值，使機器人向前傾移。具體代碼如下：if(L3G4200D_dis_data1 > FB_Amp1) /向后傾position0 -= 1;position1 -= 1;position2 += 1;position3 += 1;position4 += 1;position5 -= 1;PWM_24();Lowlevel_500u(delays_value); else over_leftfl

21、ag |= 0x01;后擺動角速度超限否？是調(diào)整舵機從0到5號的值，使機器人往前傾移該段代碼用于判斷機器人當(dāng)前所處某一姿勢時是否發(fā)生向左傾移，如傾移則調(diào)整舵機從8號到11號的值，使機器人向右傾移。具體代碼如下：if(L3G4200D_dis_data0 < LR_Amp0) /左偏position8 += 1;position10 += 1;position11 -= 1; /*position9 -= 1; /*PWM_24();Lowlevel_500u(delays_value); 左擺動角速度超限否？是調(diào)整舵機從8到11號的值，使機器人往右傾移右擺動角速度超限否？該段代碼用于判斷

22、機器人當(dāng)前所處某一姿勢時是否發(fā)生向右傾移，如傾移則調(diào)整舵機從8號到11號的值，使機器人向左傾移。具體代碼如下：if(L3G4200D_dis_data0 > LR_Amp1) /右偏position8 -= 1;position10 -= 1;position11 += 1; /*position9 += 1; /*PWM_24();Lowlevel_500u(delays_value);else over_leftflag |= 0x02;是調(diào)整舵機從8到11號的值，使機器人往左傾移該段代碼用于恢復(fù)機器人調(diào)整前所處某一姿勢，其中需要恢復(fù)的舵機有從0號到11號，除了6號和7號舵機之外。具

23、體代碼如下：if(over_leftflag = 0x03)position0 = position_temp0;position1 = position_temp1;/position2 = position_temp2;position3 = position_temp3;position4 = position_temp4;/position5 = position_temp5;position8 = position_temp8;position10 = position_temp10;position9 = position_temp9;position11 = position_t

24、emp11;PWM_24();Lowlevel_500u(delays_value);break;恢復(fù)臨時狀態(tài)以下表為在木地板上不同速度下行走時參考參數(shù)：環(huán)境速度值S前后擺動旋轉(zhuǎn)角速度幅限值X左右擺動旋轉(zhuǎn)角速度幅限值Y備注在木地板上（稍光滑）臨界值較佳值臨界值較佳值（1） 當(dāng)要左右平移或者轉(zhuǎn)動時，建議將速度值增大到5以上，避免機器人因自身產(chǎn)生較大的慣性力而使其平移或轉(zhuǎn)動角度過大導(dǎo)致失常態(tài)。（2） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變大（各30以上），較佳值亦同。（3） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變?。ㄗ钚〉?5為止），較佳值亦同。0304530451304530452304

25、53045330453045430453045525402540.3025302530以下表為在水泥地板上不同速度下行走時參考參數(shù)：環(huán)境速度值S前后擺動旋轉(zhuǎn)角速度幅限值X左右擺動旋轉(zhuǎn)角速度幅限值Y備注在水泥地板上臨界值較佳值臨界值較佳值（4） 當(dāng)要左右平移或者轉(zhuǎn)動時，建議將速度值增大到5以上，避免機器人因自身產(chǎn)生較大的慣性力而使其平移或轉(zhuǎn)動角度過大導(dǎo)致失常態(tài)。（5） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變大（35以上），較佳值亦同。（6） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變?。ㄗ钚?0），較佳值亦同。03545354513545354523545354533545354543545

26、3545530403040.3030403040以下表為在木板桌上不同速度下行走時參考參數(shù)：環(huán)境速度值S前后擺動旋轉(zhuǎn)角速度幅限值X左右擺動旋轉(zhuǎn)角速度幅限值Y備注在木板桌上臨界值較佳值臨界值較佳值（7） 當(dāng)要左右平移或者轉(zhuǎn)動時，建議將速度值增大到5以上，避免機器人因自身產(chǎn)生較大的慣性力而使其平移或轉(zhuǎn)動角度過大導(dǎo)致失常態(tài)。（8） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變大（30以上），較佳值亦同。（9） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變?。ㄗ钚?5），較佳值亦同。030453045130453045230453045330453045430453045525402540.3025402

27、540以下表為在地毯、卵石地板等上不同速度下行走時參考參數(shù)：環(huán)境速度值S前后擺動旋轉(zhuǎn)角速度幅限值X左右擺動旋轉(zhuǎn)角速度幅限值Y備注在地毯、卵石地板等上臨界值較佳值臨界值較佳值（10） 當(dāng)要左右平移或者轉(zhuǎn)動時，建議將速度值增大到5以上，避免機器人因自身產(chǎn)生較大的慣性力而使其平移或轉(zhuǎn)動角度過大導(dǎo)致失常態(tài)。（11） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變大（40以上），較佳值亦同。（12） 調(diào)節(jié)速度值越小時，調(diào)整參數(shù)的臨界值也相應(yīng)變?。ㄗ钚?5），較佳值亦同。（13） 在這些較為復(fù)雜一點的地面上行走時，調(diào)節(jié)的參數(shù)均偏大些，至少40以上，否則會出現(xiàn)失常行走。340454045440454045540

28、454045635453545735453545835403540.3035403540參數(shù)調(diào)節(jié)注意事項：（1）當(dāng)機器人在不如木板那么整平的地面上或者更復(fù)雜的地面上行走的時候，速度值參數(shù)盡可能加大，陀螺儀X、Y值也盡可能加大。也就是，地面越復(fù)雜，越要慢速行走，否則機器人就會失常。（2）機器人在行走時，其中某些舵機有時會出現(xiàn)突然抖動，會造成行走常態(tài)突發(fā)改變，這種情況下最好先讓機器人停一下再接著行走。出現(xiàn)這種現(xiàn)象，在于機器人行走時某一時間某些舵機因供電不足或者線性度不良所導(dǎo)致的。此下，只能調(diào)大參數(shù)，使機器人慢速行走。（3）當(dāng)機器人在正常行走時，調(diào)節(jié)陀螺儀參數(shù)X、Y基本上在35到50之間變化。當(dāng)出現(xiàn)

29、地面復(fù)雜時，建議把行走速度S調(diào)到3以上。2靜止站立姿態(tài)調(diào)整程序機器人靜止站立姿態(tài)的調(diào)整原理，等同機器人在行走過程中當(dāng)雙腿著地時某一姿態(tài)的調(diào)整原理，但有其特殊之處。區(qū)別是，當(dāng)靜止?fàn)顟B(tài)被迫改變時，依舊循環(huán)采集陀螺儀數(shù)值，及時判斷是否超限并且修改相應(yīng)舵機的值，以至使機器人回歸到靜止站立姿態(tài)，不過使機器人回歸站立姿態(tài)過程，采用了軟回程的方式，而行走時采用快速回程的方式。另外，機器人前后、左右的回程以分開的方式控制。該調(diào)整程序的工作邏輯方式是，快速反饋，緩速回程。主要使用的函數(shù)名稱列舉如下：void Read_IIC_XYZAxis(void) /采集三軸數(shù)字值函數(shù)void ExecuteL3G4200

30、D_TwoStandUp(char FB_Amplitude, char LR_Amplitude, uchar delays_value) /調(diào)整姿勢函數(shù)在ExecuteL3G4200D_TwoStandUp調(diào)整姿勢函數(shù)中，三個參數(shù)FB_Amplitude、LR_Amplitude、delays_value分別由無線遙控器調(diào)節(jié)設(shè)定。具體調(diào)節(jié)方式，等同上文行走姿態(tài)調(diào)整程序的調(diào)節(jié)方式。該姿態(tài)調(diào)整函數(shù)的實現(xiàn)流程圖4-6所示。開始陀螺儀是否插上？否退出調(diào)整函數(shù)是保存臨時狀態(tài)讀取陀螺儀數(shù)值前后回程結(jié)束標(biāo)志置位否？是否前擺動角速度超限否？否是調(diào)整舵機從0到5號的值，使機器人往后傾移后擺動角速度超限否？否

31、啟動前后的軟回程是置前后回程結(jié)束標(biāo)志調(diào)整舵機從0到5號的值，使機器人往前傾移左右回程結(jié)束標(biāo)志置位否？是否左擺動角速度超限否？否是調(diào)整舵機從8、9號的值，使機器人往左撇腿右擺動角速度超限否？否啟動左右的軟回程置左右回程結(jié)束標(biāo)志是調(diào)整舵機從10、11號的值，使機器人往右撇腿前后左右回程標(biāo)志置位否？否返回讀取陀螺儀數(shù)值是退出調(diào)整函數(shù)圖4-6 靜止站立姿態(tài)調(diào)整流程圖該姿態(tài)調(diào)整函數(shù)中各塊代表代碼具體如下：陀螺儀是否插上？該段代碼用于判斷陀螺儀是否插上，未插上則退出調(diào)整函數(shù)，其如下：if(L3G_flag_plug1 = 1)/*if(psx_mode != 0xFF)L3G_Delay10ms();if

32、(flag_acc_enable = 1)adc_adjust(); */return;保存臨時狀態(tài)該段代碼用于保存機器人當(dāng)前所處某一姿勢時相關(guān)要調(diào)整的舵機各值，其中舵機有從0號到11號，除了6號和7號舵機之外。具體代碼如下：position_temp0 = position0;position_temp1 = position1;position_temp2 = position2;position_temp3 = position3;position_temp4 = position4;position_temp5 = position5;position_temp8 = position

33、8;position_temp9 = position9;position_temp10 = position10;position_temp11 = position11;該段代碼用于調(diào)用讀取出陀螺儀三軸數(shù)字值的處理函數(shù)，其如下：Read_IIC_XYZAxis();讀取陀螺儀三軸數(shù)字值前擺動角速度超限否？該段代碼用于檢測與調(diào)整前后的姿態(tài)，需操作的舵機有從0號到5號。其如下：if(L3G4200D_dis_data1 < FB_Amp0) /前傾position0 += 2;position1 += 1;position2 -= 1;position3 -= 2;position4 -

34、= 1;position5 += 1;PWM_24();Lowlevel_500u(delays_value);position_temp6+;else if(L3G4200D_dis_data1 > FB_Amp1) /后傾position0 -= 2;position1 -= 1;position2 += 1;position3 += 2;position4 += 1;position5 -= 1;PWM_24();Lowlevel_500u(delays_value);position_temp6-;else 此處調(diào)用前后軟回程的代碼段調(diào)整舵機從0到5號的值，使機器人往后傾移后擺動

35、角速度超限否？調(diào)整舵機從0到5號的值，使機器人往前傾移該段代碼用于機器人調(diào)整姿勢之后，需要恢復(fù)前后方向的舵機到靜止站立時的舵機狀態(tài)，恢復(fù)的舵機有從0號到5號，即恢復(fù)事先保存的臨時狀態(tài)值，并置前后回程結(jié)束標(biāo)志。具體代碼如下：over_leftflag |= 0x01;if(position_temp6 != 125)position_change0=position_temp0-position0;position_change1=position_temp1-position1;position_change2=position_temp2-position2;position_change3

36、=position_temp3-position3;position_change4=position_temp4-position4;position_change5=position_temp5-position5;if(position_temp6 > 125)Point_ExecuteFB(delays_value, position_change3+6);else if(position_temp6 < 125)Point_ExecuteFB(delays_value, position_change0+6);啟動前后的軟回程該段代碼用于機器人調(diào)整姿勢之后，需要恢復(fù)左右方

37、向的舵機到靜止站立時的舵機狀態(tài)，恢復(fù)的舵機有從8號到11號，即恢復(fù)事先保存的臨時狀態(tài)值，并置左右回程結(jié)束標(biāo)志。具體代碼如下：over_leftflag |= 0x02;position_change8=position_temp8-position8;position_change9=position_temp9-position9;if(position_change8 < 0)value_change = position_change8;value_change = (value_change) + 1;Point_ExecuteLR(delays_value,value_chan

38、ge);position_change10=position_temp10-position10;position_change11=position_temp11-position11;if(position_change10 > 0)Point_ExecuteLR(delays_value,position_change10);啟動左右的軟回程該段代碼用于檢測與調(diào)整左右的姿態(tài)，需操作的舵機有從8號到11號。其如下：if(L3G4200D_dis_data0<LR_Amp0) /左傾position8 += 3;position9 -= 1;PWM_24();Lowlevel_5

39、00u(delays_value);else if(L3G4200D_dis_data0>LR_Amp1)/右傾position10 -= 3;position11 += 1; PWM_24();Lowlevel_500u(delays_value);else 此處調(diào)用左右軟回程的代碼段左擺動角速度超限否？調(diào)整舵機從8、9號的值，使機器人往左撇腿右擺動角速度超限否？調(diào)整舵機從10、11號的值，使機器人往右撇腿3左右平移姿態(tài)調(diào)整程序機器人左右平移姿態(tài)的調(diào)整原理，等同機器人在行走過程中當(dāng)雙腿著地時某一姿態(tài)的調(diào)整原理，但有其特殊之處。區(qū)別是，當(dāng)每平移完一步處于站立狀態(tài)時，再做姿態(tài)調(diào)整。在平移完

40、一步之后的狀態(tài)被迫改變時，依舊循環(huán)采集陀螺儀數(shù)值，及時判斷是否超限并且修改相應(yīng)舵機的值，以至使機器人回歸到站立姿態(tài)，不過使機器人回歸站立姿態(tài)過程，前后采用快回程的方式，左右采用了軟回程的方式，與靜止站立狀態(tài)的回程方式一樣，而行走時則采用了快速回程的方式。另外，機器人前后、左右的回程以分開的方式控制。該調(diào)整程序的工作邏輯方式是，快速反饋，緩速與快速回程。主要使用的函數(shù)名稱列舉如下：void Read_IIC_XYZAxis(void) /采集三軸數(shù)字值函數(shù)void ExecuteL3G4200D_TwoTranslation(char FB_Amplitude, char LR_Amplitud

41、e, uchar delays_value) /調(diào)整姿勢函數(shù)在ExecuteL3G4200D_TwoTranslation調(diào)整姿勢函數(shù)中，三個參數(shù)FB_Amplitude、LR_Amplitude、delays_value分別由無線遙控器調(diào)節(jié)設(shè)定。具體調(diào)節(jié)方式，等同上文行走姿態(tài)調(diào)整程序的調(diào)節(jié)方式。該姿態(tài)調(diào)整函數(shù)的實現(xiàn)流程圖4-7所示。開始陀螺儀是否插上？否退出調(diào)整函數(shù)是保存臨時狀態(tài)讀取陀螺儀數(shù)值前后回程結(jié)束標(biāo)志置位否？是否前擺動角速度超限否？否是調(diào)整舵機從0到5號的值，使機器人往后傾移后擺動角速度超限否？否啟動前后的快回程是置前后回程結(jié)束標(biāo)志調(diào)整舵機從0到5號的值，使機器人往前傾移左右回程結(jié)束

42、標(biāo)志置位否？是否左擺動角速度超限否？否是調(diào)整舵機從8、9號的值，使機器人往左撇腿右擺動角速度超限否？否啟動左右的軟回程置左右回程結(jié)束標(biāo)志是調(diào)整舵機從10、11號的值，使機器人往右撇腿前后左右回程標(biāo)志置位否？否返回讀取陀螺儀數(shù)值是退出調(diào)整函數(shù)圖4-7 平移姿態(tài)調(diào)整流程圖該姿態(tài)調(diào)整函數(shù)的各塊代表代碼如下：該段代碼用于判斷陀螺儀是否插上，未插上則退出調(diào)整函數(shù)，其如下：if(L3G_flag_plug1 = 1)if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1)adc_adjust();return;陀螺儀是否插上？該段代碼用于保存機

43、器人當(dāng)前所處某一姿勢時相關(guān)要調(diào)整的舵機各值，其中舵機有從0號到11號，除了6號和7號舵機之外。具體代碼如下：position_temp0 = position0;position_temp1 = position1;position_temp2 = position2;position_temp3 = position3;position_temp4 = position4;position_temp5 = position5;position_temp8 = position8;position_temp9 = position9;position_temp10 = position10;position_temp11 = position11;保存臨時狀態(tài)讀取陀螺儀三軸數(shù)字值該段代碼用于調(diào)用讀取出陀螺儀三軸數(shù)字值的處理函數(shù)，其如下：Read_IIC_XYZAxis();該段代碼用于機器人調(diào)整姿勢之后，需要恢復(fù)前后方向的舵機到站立時的舵機狀態(tài)，恢復(fù)的舵機有從0號到5