三轴数字陀螺仪篇

1、三轴数字陀螺仪手册-目录机器人衡态调整三轴数字陀螺仪篇北京森汉科技有限公司目录1 .硬件介绍 11 .陀螺仪芯片 12 .陀螺仪的载板 23 . SHR-8s机器人三轴分布 34 . STC12C5A60卷片接口 45 .载板实物 42 .陀螺仪操作 51 .访问格式 52 .软件接口 53 .姿势状态分析 51 .自纠正归位系统介绍 52 .自纠正跳位系统介绍 64 .姿态调整程序解析 81 .行走姿态调整程序 82 .静止站立姿态调整程序 193 .左右平移姿态调整程序 25第1页三轴数字陀螺仪手册一.硬件介绍1.陀螺仪芯片如下图1-1所示，三轴数字陀螺仪 L3G4200D芯片X、Y、Z三

2、轴互相正交分布。第1页ddRE9PLLFFTGND(TOP VIEW) DIRECTIONS OF THE DETECTABLE ANGULAR RATES亘-BOTTOM VIEWOISVdd IOSCL/SPCSDZVSDI/SDOSDO/SAO图1-1坐标方向及背面图图1-1中明确标出了 L3G4200D芯片X、Y、Z轴的坐标方向及背面图。本陀螺仪芯片可 供用户选择其尺度量程，量程范围从 250 dps2000 dps,低量程数值用于高精度慢 速运动测量，而高量程则用于测量超快速的手势和运动。这款器件提供一个 16位数据输出,以及可配置的低通和高通滤波器等嵌入式数字功能。就算时间推移或温

3、度变化，这款器件仍然保持连续稳定的输出。L3G4200D 特性（ST）:三种可选全尺度（250/500/2000存保计划）I2C/SPI数字输出接口16比特率值的数据输出8比特温度数据输出两个数字输出线（中断和 data ready ）集成低和高通滤波器的用户可选带宽在时间和温度上平稳嵌入式self-test宽电源电压，2.4 V至ij 3.6 V低电压兼容的IOS, 1.8 V嵌入式power-down和睡眠模式嵌入式温度传感器嵌入式FIFO缓存三轴数字陀螺仪手册高抗撞击能力扩展的工作温度范围（-40 C到+85 C）ECOPACK? RoHS 和 “Green” 认证2 .陀螺仪的载板（1

4、）本次采用陀螺仪的iic接口方式，访问陀螺仪内部的三轴数字值。其每一轴的数字值均为有符号的16进制数，以补码的形式存放。采用单片机两个I/O 口（P4.1与P3.3）模IIC与陀螺仪IIC接口（ SCL、SDA相接，使用100kHz采样速率对陀螺仪内部寄存器 配置与三轴数字值的读取。 其中三轴X、Y、Z数字值的存放寄存器分别为 OUT_X_L OUT_X_H OUT Y Uf OUT Y H OUT Z Uf OUT Z H只要对它们进行读取即可。其应用载板正面及接线提示图如下图1-2所示。图1-2载板正面图（2）陀螺仪芯片的载板背面图如图1-3所示。图1-3载板背面图3 . SHR-8s机器

5、人三轴分布在此特别的说明一下，这里讲到的三轴与L3G4200D芯片载板的三轴完全重合。根据三轴数字陀螺仪载板在机器人身长的插接方式，如下图1-4所示的坐标。Y、Z平成Y、Z平面图1-4载板在机器人上的坐标图4 . STC12C5A60S 芯片接口STC12C5A60卷片是SHR-8s人形机器人的主控板上面的 MCU它负责机器人行走步伐函 数的发生，另外还负责红外线接收与识别，与上位机三维图形化软件进行通讯的功能。本次再给它加一个任务就是“三轴数字陀螺仪的数据采样和分析”。所以使用该芯片的IIC采样端口，对L3G4200DS行三轴数字值采样。具体白端口连接参考下面图1-5。su 胃一 SRXDI

6、E3IH EG T d K班.科再 HEJF3.7 X1AL5 XIAL1 fD S?F4 0 /P20 户3用工 AS(I/P1.2 和小， 育吧格用 ”口73 2FE 41A匚4仁口巴反等F) 3MDC 3 /CCro/TSMFl LE口 1.Pl 0J(ftDC0CLKDUT2P4 OE/RW mg。*后舱空C641骷QT如FIT+5SOFT44一VOr43XQVT4WTUI 35钠TOTTUIrv j ew iH.L2 幽口二eFnatTi?237TK图1-5 STC12C5A60S芯片接口图根据上图1-5,使用如下的端口协议：STC12C5A60A宓片：P4.1 载板 SCL;P3.

7、3 -载板 SDA5 .载板实物载板的实物图，如下图1-6所示。实物暂时未出图1-6载板实物图陀螺仪操作本陀螺仪操作接口含有 IIC与SPI两种，由于考虑到SPI接口多占用I/O 口的问题，所 以此次采用IIC接口方式对陀螺仪芯片操作， 即一根时钟线SCL和一根数据线SDA连接陀螺 仪，实现数据传输。基于标准IIC通信格式，对陀螺仪内部寄存器进行访问。采样数率使用100kHz,分别依次从六个寄存器(每一轴的数字值由低 8位与高8位组合)读取出三轴X、Y、Z的数字值, 其中六个寄存器分别为 OUT X L_W OUT X H OUT Y 与 OUT Y H OUT Z Uf OUT Z H2.软

8、件接口基于标准IIC 的通信协议，编写访问陀螺仪内部数据的接口子程序(void Read_IIC_XYZAxis(void);其依次读取出每一轴的高低8位，再分别组合存放于有符号16 位的整型变量当中，共 3 个变量(L3G4200D_dis_data0 、L3G4200D_dis_data1、 L3G4200D_dis_data2),分别对应于 X、Y、Z三轴的角速度值。由于用于控制机器人相对 较快行走，所以降低陀螺仪返回的角速度值，以缩小 100倍处理。三.姿势状态分析机器人在行走运动时欲维持其各种平衡状态， 首先要研究立正姿势下的平衡方程。 由于 立正姿势本身就是一种静止状态， 而且它相

9、对于所有的稳定状态都可以直接过渡， 所以要把 立正姿势下的各种平衡关系分类研究透彻。首先要探讨一下平衡的概念， 所谓的平衡，也就是说系统在一定范围内保持着规律性的 运动。比如说钟摆本身就是一种特殊的平衡，还有像弹簧的往复震动， 小孩子玩的陀螺不停的旋转等等都是一种稳定状态也就是一种平衡。在人体的研究范畴里面，平衡状态的种类是最多的，它的种类要比天上的战斗机还多上百倍。经常看到的杂技演员，在高空做的各种惊险动作，其实都是在从一个稳定态变化到另外的一个稳定态的过程。在这个过程中，我们发现过渡过程都是运动的，而稳定态都是静止的。不过，我们把各种状态归类总结成一下2种。1 .自纠正归位系统介绍在日常生

10、活中经常会碰到类似的情况，正常行走的行人一不留神， 脚踩到一个西瓜皮后,这个人挣扎了几下又直起身来，差一点就摔到了。这种现象归类为自动纠正归位系统现象， 如下面的流程图3-1所示。出段稳定态 以是静 也可以 动态。“返回段”是由 反馈量进入系 统而产生。k态 是“跳出段”是由 外界干扰系统 而产生。加急定态(A)过渡态过渡态都是动态。图3-1当稳定的系统受到外界干扰的时候，自纠正归位系统图系统就面临着失稳的风险。如果系统的自纠正能力上面的例子里，正常行走的行很强，那么他就会很快地进行自我纠正并回到初始的稳定态。人就是一个稳定态，它本身就是动态平衡的，行走本身就是一种周期性的运动。突然踩到一 个

11、西瓜皮代表一个很强的外界干扰因素随机性地进入系统，行人马上会手舞足蹈地翻腾起 来，因为他不想摔倒。这个翻腾的过程就是过度态，它相对来说会比较短暂。随后这个人依 靠躯干和四肢的运动，保持住了自己的重心，没有摔倒。最后，他又继续向前行走。我们把 踩到西瓜皮的瞬间归为跳出段，把四肢翻腾的过程归为返回段。2 .自纠正跳位系统介绍还是刚才的那个行人，可是这回的情况不同了，这次他最后摔倒了。这个行人，他起初 是在正常行走，一不留神，脚踩到一个西瓜皮，这个人挣扎了几下却无济于事，结果就摔到了。这种现象归类为自动跳位系统现象，如下面的流程图3-2所示。当稳定的系统受到外界干扰的时候,稳定态( 可以是 态也可

12、是动态。一B)静以系统就面临着失稳的风险。如果系统的自纠正能力在某一方面不是很强，那么它就会被另外的稳定态所代替。在这个例子里，正常行走的行人就是稳定态（A）。突然踩到一个西瓜皮代表一个很强的外界干扰因素随机性地进入系统，行人马上会四肢翻腾，这个翻腾的过程就是过度态。行人在拼命保持自己的重心，但是最后他 还是摔倒了。摔倒后，坐在了地上就是稳定态 （B）。我们把踩到西瓜皮的瞬间归类为跳出段， 把四肢翻腾但是仍然倒下的过程归类为跳位段。参考以上的两个自纠正系统的特点，下面进行研究“人体立正姿势、双腿劈叉姿势、单腿站立姿势、原地踏步姿势、双足行走姿势等”各种姿势的稳定性和过渡性的特点。本次使用陀螺仪

13、，就是用于控制机器人处于这两种自纠正系统下维持它的稳态平衡。也就是，在每一种稳态情况下，在一定较小时间内启动陀螺仪维持该稳态平衡，使其当前稳态的姿势状况不变。比如机器人在行走一步时，走完前半步处于稳态 （A）,再走完下半步处于稳态（B）。在前半步的稳态（A时，启动陀螺仪及时修正该稳态的偏移，以维持该状态不 变。下半步依然如此。如果机器人处于站立等静止状态时，就等同处于一种稳态的情况，依 然启动陀螺仪及时修正偏移，以维持稳态的姿势。 欲使机器人所有运动姿势正常化，首先调节好机器人所有运动姿势中每一种稳态正常化。如下面流程图3-3所示。BAA是静态也 可以是动稳定态（B 可以是静态 也可以是动加急

14、定态（B“跳位段”也是由反馈量 进入系统而产生。段图3-3陀螺仪应用系统图四.姿态调整程序解析1 .行走姿态调整程序在机器人行走过程中，当处在双腿着地某一姿态时，循环启动陀螺仪周期性地采集三轴 数字陀螺仪的三轴数字值，紧接着存储在数组L3G4200D_dis_data3变量当中，并比较数组L3G4200D_dis_data3与对应设定角速度幅限值，依据是否超幅限值改变相应从0至U11号舵机的值，调整机器人姿势使其保持当前姿态稳定。当机器人保持在其当前姿态时，就退出循环，继续行走，同样也依旧进入下一个姿态当中去。若在某一姿态的平衡状态被迫改变，就会同样进入循环当中，及时修正机器人的姿势。本调整程

15、序工作逻辑方式是，快速反馈， 快速回程。主要使用的函数名称列举如下：void Read_IIC_XYZAxis（void） /采集三轴数字值函数void ExecuteL3G4200D_TwoSetDown（char FB_Amplitude, char LR_Amplitude, uchar delays_value）/调整姿势函数参数：FB_Amplitude -前后摆动旋转角速度幅限值（陀螺仪Y轴旋转角速度）LR_Amplitude -左右摆动旋转角速度幅限值（陀螺仪X轴旋转角速度） delays_value 行走速度值（一个值代表 500us延时）此三个参数 FB_Amplitude、

16、LR_Amplitude、delays_value 分别由无线遥控器调节设定。参数在遥控器屏幕上显示位置与相应调整按钮调整功能，如下从图4-1到图4-4所示。第28页参：FB_Amplitude参学LR_Amplitude图4-1参数显示位置图图4-2进入与退出参数模式图按一下25键, 就减小速度参 数一个值，但 速度就快一个 值。按一下 26 键，就增加速 度参数一个 值，但速度就 慢一个值。图4-3速度参数设置图按一下12 键，就减小 一个左右摆 动旋转角速 度幅限值。按一下11 键，就减小 一个前后摆 动旋转角速 度幅限值。按一下 16 键，就增加 一个前后摆 动旋转角速 度幅限值。陀螺

17、仪是 否插上？退出调整函数按一下 15 键，就增加 一个左右摆 动旋转角速 度幅限值。图4-4陀螺仪参数设定图注意：机器人在行走当中每一种半步稳态，只需调用该ExecuteL3G4200D_TwoSetDown函数，即可调整行走当中因惯性导致机器人身体前后左右的偏移，前提是机器人的行走步伐要无误。其中，该姿态调整函数 ExecuteL3G4200D_TwoSetDown的实现流程图4-5所示。开始保存临时状态读取陀螺仪三轴数字值否前摆动角速度超限否？J是调整舵机从 0到5 号的值，使机器人往 后倾移后摆动角速 度超限否？标志前后稳定调整舵机从 0到5 号的值，使机器人往 前倾移否左摆动角速度超

18、限否？调整舵机从8到11 号的值，使机器人往 右倾移标志左右稳定右摆动角速度超限否？均稳定?调整舵机从8到11 号的值，使机器人往 左倾移图4-5行走姿态调整流程图该姿态调整函数中各块代表代码如下：陀螺仪是 否插上？该段代码用于判断陀螺仪是否插上, 调整函数，其如下：if(L3G_flag_plug1 = 1)/*if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1) adc_adjust(); */return;未插上则退出保存临时状态该段代码用于保存机器人当前所处某一姿势时相关要调整的舵机各值，其中舵机有从0号到11号, 除了

19、 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;读取陀螺仪三轴数

20、 字值该段代码用于调用读取出陀螺仪三轴数字值的处 理函数，其如下：Read_IIC_XYZAxis();前摆动角速 度超限否？I是调整舵机从0到5 号的值，使机器人往 后倾移后摆动角速度超限否？是调整舵机从 0到5 号的值，使机器人往 前倾移该段代码用于判断机器人当前所处某一姿势时 是否发生向前倾移，如倾移则调整舵机从0号到5号的值，使机器人向后倾移。具体代码如 下：if(L3G4200D_dis_data1FB_Amp1) / 向后倾 position0 -= 1; position1 -= 1; position2 += 1; position3 += 1; position4 += 1;

21、 position5 -= 1; PWM_24();Lowlevel_500u(delays_value); else over_leftflag |= 0x01;左摆动角速 度超限否？丫是调整舵机从8到11 号的值，使机器人往 右倾移右摆动角速 度超限否？是调整舵机从8到11 号的值，使机器人往 左倾移该段代码用于判断机器人当前所处某一姿势时 是否发生向左倾移，如倾移则调整舵机从 8号 到11号的值，使机器人向右倾移。具体代码如 下：if(L3G4200D_dis_data0 LR_Amp1)/右偏position8 -= 1;position10 -= 1;position11 += 1;

22、 /*position9 += 1; /*PWM_24();Lowlevel_500u(delays_value);else over_leftflag |= 0x02;恢复临时状态该段代码用于恢复机器人调整前所处某一姿势，其中需要恢复的舵机有从0号到11号，除了 6号和7号舵机之外。 具体代码如下：if(over_leftflag = 0x03) position0 = position_temp0; position1 = position_temp1;/ position2 = position_temp2; position3 = position_temp3; position4 =

23、 position_temp4;/ position5 = position_temp5; position8 = position_temp8; position10 = position_temp10;position9 = position_temp9;positional = position_temp11;PWM_24();Lowlevel_500u(delays_value); break;以下表为在木地板上不同速度下行走时参考参数:环 境速度 值S前后摆动旋转角速度幅限值X左右摆动旋转角速度幅限值Y备注在 木 地 板 上( 稍 光滑)临界 值较佳 值临界 值较佳 值(1)当要左右

24、平移或者转动时， 建议将速度 值增大到5以上，避免机器人因自身产生较 大的惯性力而使其平移或转动角度过大导致 失常态。(2)调节速度值越小时，调整参数的临界值 也相应变大(各30以上)，较佳值亦同。(3)调节速度值越小时，调整参数的临界值 也相应变小(最小到25为止)，较佳值亦同。030453045130453045230453045330453045430453045525402540.3025302530以下表为在水泥地板上不同速度下行走时参考参数:环 境速度 值S前后摆动旋 转角速度幅限值X左右摆动旋转角速度幅限值Y备注在 水 泥 地 板 上临界 值较佳 值临界 值较佳 值(4)当要左右

25、平移或者转动时，建议将速度值增大到5以上，避免机器人因自身产生较 大的惯性力而使其平移或转动角度过大导致 失常态。(5)调节速度值越小时，调整参数的临界值 也相应变大(35以上)，较佳值亦同。(6)调节速度值越小时，调整参数的临界值 也相应变小(最小 30),较佳值亦同。035453545135453545235453545335453545435453545530403040.3030403040以下表为在木板桌上不同速度下行走时参考参数:环 境速度 值S前后摆动旋转角速度幅限值X左右摆动旋转角速度幅限值Y备注在 木 板 桌 上临界 值较佳 值临界 值较佳 值(7)当要左右平移或者转动时，

26、建议将速度 值增大到5以上，避免机器人因自身产生较 大的惯性力而使其平移或转动角度过大导致 失常态。(8)调节速度值越小时，调整参数的临界值 也相应变大(30以上)，较佳值亦同。(9)调节速度值越小时，调整参数的临界值 也相应变小(最小 25),较佳值亦同。030453045130453045230453045330453045430453045525402540.3025402540以下表为在地毯、卵石地板等上不同速度下行走时参考参数:环 境速度 值S前后摆动旋转角速度幅限值X左右摆动旋转角速度幅限值Y备注在 地毯、 卵 石 地 板 等 上临界 值较佳 值临界 值较佳 值(10)当要左右平移

27、或者转动时，建议将速 度值增大到5以上，避免机器人因自身产生 较大的惯性力而使其平移或转动角度过大导 致失常态。(11)调节速度值越小时，调整参数的临界 值也相应变大(40以上)，较佳值亦同。(12)调节速度值越小时，调整参数的临界 值也相应变小(最小 35),较佳值亦同。(13)在这些较为复杂一点的地面上行走时，调节的参数均偏大些，至少 40以上，否 则会出现失常行走。340454045440454045540454045635453545735453545835403540.3035403540参数调节注意事项：(1)当机器人在不如木板那么整平的地面上或者更复杂的地面上行走的时候，速 度值

28、参数尽可能加大，陀螺仪 X、Y值也尽可能加大。也就是，地面越复杂，越要慢速 行走，否则机器人就会失常。(2)机器人在行走时，其中某些舵机有时会出现突然抖动，会造成行走常态突发 改变，这种情况下最好先让机器人停一下再接着行走。出现这种现象，在于机器人行 走时某一时间某些舵机因供电不足或者线性度不良所导致的。此下，只能调大参数， 使机器人慢速行走。(3)当机器人在正常行走时，调节陀螺仪参数头Y基本上在35到50之间变化。当出现地面复杂时，建议把行走速度S调到3以上。2 .静止站立姿态调整程序机器人静止站立姿态的调整原理，等同机器人在行走过程中当双腿着地时某一姿态的调 整原理，但有其特殊之处。区别是

29、，当静止状态被迫改变时，依旧循环采集陀螺仪数值，及 时判断是否超限并且修改相应舵机的值，以至使机器人回归到静止站立姿态，不过使机器人回归站立姿态过程，采用了软回程的方式，而行走时采用快速回程的方式。另外，机器人前后、左右的回程以分开的方式控制。该调整程序的工作逻辑方式是，快速反馈，缓速回程。主要使用的函数名称列举如下：void Read_IIC_XYZAxis(void) /采集三轴数字值函数void ExecuteL3G4200D_TwoStandUp(char FB_Amplitude, char LR_Amplitude, uchar delays_value)/调整姿势函数在 Exec

30、uteL3G4200D_TwoStandUp 调整姿势函数中，三个参数 FB_Amplitude、 LR_Amplitude、delays_value分别由无线遥控器调节设定。具体调节方式，等同上文行走姿态调整程序的调节方式。该姿态调整函数白实现流程图4-6所示。否 一一M退出调整函数开始陀螺仪是否插上？是111al1111HB11Mli11nli保存临时状态读取陀螺仪数值是前后回程结束标志置位否？否v否前摆动角速I度超限否？是调整舵机从 0到5 号的值，使机器人往 后倾移否 启动前后的软回程置前后回程结束标志后摆动角速 度超限否？是调整舵机从 0到5 号的值，使机器人往 前倾移是 jk 左右

31、回程结束标志置位否?否左摆动角速 度超限否？调整舵机从8、9号 的值，使机器人往左 撇腿启动左右的软回程右摆动角速度超限否？置左右回程结束标志调整舵机从 10、11 号的值，使机器人往 右撇腿前后左右回程标志置位否？否 一、返回读取陀螺仪数值是退出调整函数图4-6静止站立姿态调整流程图该姿态调整函数中各块代表代码具体如下：陀螺仪是 否插上？该段代码用于判断陀螺仪是否插上, 调整函数，其如下：if(L3G_flag_plug1 = 1)/*if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1) adc_adjust(); */ret

32、urn;未插上则退出保存临时状态该段代码用于保存机器人当前所处某一姿势时相关要调整的舵机各值，其中舵机有从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;pos

33、ition_temp10 = position10;position_temp11 = position11;该段代码用于调用读取出陀螺仪三轴数字值的处理函数，其如下：Read_IIC_XYZAxis();该段代码用于检测与调整前后的姿态，需操作的舵机有从0号到5号。其如下：if(L3G4200D_dis_data1 FB_Amp1)/后倾position0 -= 2;position1 -= 1;position2 += 1;position3 += 2;position4 += 1;position5 -= 1;PWM_24();Lowlevel_500u(delays_value);po

34、sition_temp6-; else此处调用前后软回程的代码段三轴数字陀螺仪手册该段代码用于机器人调整姿势之后，需要恢复前后方向的舵机到静止站立时的舵机状态，恢复的舵机有从0号启动前后的软回程到5号，即恢复事先保存的临时状态值，并置前后回程结束标志。具体代码如下：over_leftflag |= 0x01;if(position_temp6 != 125)position_change0=position_temp0-position0;position_change1=position_temp1-position1;position_change2=position_temp2-posi

35、tion2;position_change3=position_temp3-position3;position_change4=position_temp4-position4;position_change5=position_temp5-position5;if(position_temp6 125) Point_ExecuteFB(delays_value, position_change3+6);elseif(position_temp6125)Point_ExecuteFB(delays_value, position_change0+6);该段代码用于机器人调整姿势之后，需要恢复

36、左右方向启动左右的软回程的舵机到静止站立时的舵机状态，恢复的舵机有从8号到11号，即恢复事先保存的临时状态值，并置左右回程结束标志。具体代码如下：over_leftflag |= 0x02;position_change8=position_temp8-position8;position_change9=position_temp9-position9;if(position_change8 0)Point_ExecuteLR(delays_value,position_change10);第29页三轴数字陀螺仪手册左摆动角速度超限否？调整舵机从8、9号 的值，使机器人往左 撇腿右摆动角速度

37、超限否？调整舵机从 10、11 号的值，使机器人往 右撇腿该段代码用于检测与调整左右的姿态，需操作的舵机有从8号到11号。其如下：if(L3G4200D_dis_data0卜LR_Amp0) 左倾position8 += 3;position9 -= 1;PWM_24();Lowlevel_500u(delays_value); else if(L3G4200D_dis_data0LR_Amp1)右倾position10 -= 3;position11 += 1;PWM_24();Lowlevel_500u(delays_value); else 此处调用左右软回程的代码段 3 .左右平移姿态

38、调整程序机器人左右平移姿态的调整原理，等同机器人在行走过程中当双腿着地时某一姿态的调 整原理，但有其特殊之处。区别是，当每平移完一步处于站立状态时，再做姿态调整。在平 移完一步之后的状态被迫改变时，依旧循环采集陀螺仪数值，及时判断是否超限并且修改相应舵机的值，以至使机器人回归到站立姿态，不过使机器人回归站立姿态过程，前后采用快回程的方式，左右采用了软回程的方式，与静止站立状态的回程方式一样，而行走时则采用了快速回程的方式。另外，机器人前后、左右的回程以分开的方式控制。该调整程序的工作逻辑方式是，快速反馈，缓速与快速回程。主要使用的函数名称列举如下：void Read_IIC_XYZAxis(v

39、oid) /采集三轴数字值函数void ExecuteL3G4200D_TwoTranslation(charFB_Amplitude, char LR_Amplitude,uchar delays_value)/调整姿势函数在 ExecuteL3G4200D_TwoTranslation调整姿势函数中，三个参数FB_Amplitude、LR_Amplitude、delays_value 分别由无线遥控器调节设定。具体调节方式，等同上文行走 姿态调整程序的调节方式。该姿态调整函数白实现流程图4-7所示。第38页是左右回程结束 标志置位否？否否/g/左摆动角速一度超限否？是*调整舵机从8、9号

40、的值，使机器人往左 撇腿右摆动角速、否 度超限否？启动左右的软回程是 调整舵机从 10、11 号的值，使机器人往 右撇腿置左右回程结束标志前后左右回程标志置位否？返回读取陀螺仪数值退出调整函数图4-7平移姿态调整流程图该姿态调整函数的各块代表代码如下:陀螺仪是 否插上？该段代码用于判断陀螺仪是否插上，未插上则退出 调整函数，其如下：if(L3G_flag_plug1 = 1)if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1)adc_adjust();return;保存临时状态该段代码用于保存机器人当前所处某一姿势时相关 要调整的舵机各值，其中舵机有从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_temp