三轴数字陀螺仪篇

1、机器人衡态调整三轴数字陀螺仪篇ZXY北京森汉科技有限公司目录一硬件介绍11陀螺仪芯片12陀螺仪的载板23SHR-8S机器人三轴分布34STC12C5A60S芯片接口45载板实物4二陀螺仪操作51访问格式52软件接口5三姿势状态分析51自纠正归位系统介绍52自纠正跳位系统介绍6四姿态调整程序解析81行走姿态调整程序82静止站立姿态调整程序193左右平移姿态调整程序25一硬件介绍1陀螺仪芯片如下图1-1所示，三轴数字陀螺仪L3G4200D芯片X、Y、Z三轴互相正交分布。图1-1 坐标方向及背面图图1-1中明确标出了L3G4200D芯片X、Y、Z轴的坐标方向及背面图。本陀螺仪芯片可供用户选择其尺度量

2、程，量程范围从 ±250 dps ±2000 dps，低量程数值用于高精度慢速运动测量，而高量程则用于测量超快速的手势和运动。这款器件提供一个16位数据输出，以及可配置的低通和高通滤波器等嵌入式数字功能。就算时间推移或温度变化，这款器件仍然保持连续稳定的输出。L3G4200D 特性（ST）：u 三种可选全尺度(±250/500/2000 存保计划）u I2C/SPI 数字输出接口u 16 比特率值的数据输出u 8 比特温度数据输出u 两个数字输出线（中断和 data ready）u 集成低和高通滤波器的用户可选带宽u 在时间和温度上平稳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”认证2陀螺仪的载板（1）本次采用陀螺仪的IIC接口方式，访问陀螺仪内部的三轴数字值。其每一轴的数字值均为有符号的16进制数，以补码的形式存放。采用单片机两个I/O口（P4.1与P3.3）模拟IIC与陀螺仪IIC接口（SCL、SDA）相接，使用100kHz采样速率对陀螺仪内部寄存器配置与

4、三轴数字值的读取。其中三轴X、Y、Z数字值的存放寄存器分别为OUT_X_L与OUT_X_H、OUT_Y_L与OUT_Y_H、OUT_Z_L与OUT_Z_H，只要对它们进行读取即可。其应用载板正面及接线提示图如下图1-2所示。GNDSCL+5VSDA图1-2 载板正面图（2）陀螺仪芯片的载板背面图如图1-3所示。YZX图1-3 载板背面图3SHR-8S机器人三轴分布在此特别的说明一下，这里讲到的三轴与L3G4200D芯片载板的三轴完全重合。根据三轴数字陀螺仪载板在机器人身长的插接方式，如下图1-4所示的坐标。YXZYZXY、Z平面ZXYXYZX、Y平面Y、Z平面图1-4 载板在机器人上的坐标图4

5、STC12C5A60S芯片接口STC12C5A60S芯片是SHR-8S人形机器人的主控板上面的MCU，它负责机器人行走步伐函数的发生，另外还负责红外线接收与识别，与上位机三维图形化软件进行通讯的功能。本次再给它加一个任务就是“三轴数字陀螺仪的数据采样和分析”。所以使用该芯片的IIC采样端口，对L3G4200D进行三轴数字值采样。具体的端口连接参考下面图1-5。图1-5 STC12C5A60S芯片接口图根据上图1-5，使用如下的端口协议：STC12C5A60AD芯片：P4.1 - 载板SCL； P3.3 - 载板SDA；5载板实物载板的实物图，如下图1-6所示。实物暂时未出图1-6 载板实物图二

6、陀螺仪操作本陀螺仪操作接口含有IIC与SPI两种，由于考虑到SPI接口多占用I/O口的问题，所以此次采用IIC接口方式对陀螺仪芯片操作，即一根时钟线SCL和一根数据线SDA连接陀螺仪，实现数据传输。1访问格式基于标准IIC通信格式，对陀螺仪内部寄存器进行访问。采样数率使用100kHz，分别依次从六个寄存器（每一轴的数字值由低8位与高8位组合）读取出三轴X、Y、Z的数字值，其中六个寄存器分别为OUT_X_L与OUT_X_H、OUT_Y_L与OUT_Y_H、OUT_Z_L与OUT_Z_H。2软件接口基于标准IIC的通信协议，编写访问陀螺仪内部数据的接口子程序（void Read_IIC_XYZAx

7、is(void) ）；其依次读取出每一轴的高低8位，再分别组合存放于有符号16位的整型变量当中，共3个变量（L3G4200D_dis_data0、L3G4200D_dis_data1、L3G4200D_dis_data2），分别对应于X、Y、Z三轴的角速度值。由于用于控制机器人相对较快行走，所以降低陀螺仪返回的角速度值，以缩小100倍处理。三姿势状态分析机器人在行走运动时欲维持其各种平衡状态，首先要研究立正姿势下的平衡方程。由于立正姿势本身就是一种静止状态，而且它相对于所有的稳定状态都可以直接过渡，所以要把立正姿势下的各种平衡关系分类研究透彻。首先要探讨一下平衡的概念，所谓的平衡，也就是说系统

8、在一定范围内保持着规律性的运动。比如说钟摆本身就是一种特殊的平衡，还有像弹簧的往复震动，小孩子玩的陀螺不停的旋转等等都是一种稳定状态也就是一种平衡。在人体的研究范畴里面，平衡状态的种类是最多的，它的种类要比天上的战斗机还多上百倍。经常看到的杂技演员，在高空做的各种惊险动作，其实都是在从一个稳定态变化到另外的一个稳定态的过程。在这个过程中，我们发现过渡过程都是运动的，而稳定态都是静止的。不过，我们把各种状态归类总结成一下2种。1自纠正归位系统介绍在日常生活中经常会碰到类似的情况，正常行走的行人一不留神，脚踩到一个西瓜皮后，这个人挣扎了几下又直起身来，差一点就摔到了。这种现象归类为自动纠正归位系统

9、现象，如下面的流程图3-1所示。稳定态（A）过渡态跳段出返回段稳定态可以是静态也可以是动态。过渡态都是动态。“跳出段”是由外界干扰系统而产生。“返回段”是由反馈量进入系统而产生。图3-1 自纠正归位系统图当稳定的系统受到外界干扰的时候，系统就面临着失稳的风险。如果系统的自纠正能力很强，那么他就会很快地进行自我纠正并回到初始的稳定态。上面的例子里，正常行走的行人就是一个稳定态，它本身就是动态平衡的，行走本身就是一种周期性的运动。突然踩到一个西瓜皮代表一个很强的外界干扰因素随机性地进入系统，行人马上会手舞足蹈地翻腾起来，因为他不想摔倒。这个翻腾的过程就是过度态，它相对来说会比较短暂。随后这个人依靠

10、躯干和四肢的运动，保持住了自己的重心，没有摔倒。最后，他又继续向前行走。我们把踩到西瓜皮的瞬间归为跳出段，把四肢翻腾的过程归为返回段。2自纠正跳位系统介绍还是刚才的那个行人，可是这回的情况不同了，这次他最后摔倒了。这个行人，他起初是在正常行走，一不留神，脚踩到一个西瓜皮，这个人挣扎了几下却无济于事，结果就摔到了。这种现象归类为自动跳位系统现象，如下面的流程图3-2所示。稳定态（A）过渡态跳段出跳位段稳定态（A）可以是静态也可以是动态。“跳位段”也是由反馈量进入系统而产生。稳定态（B）稳定态（B）可以是静态也可以是动态。图3-2 自纠正跳位系统图当稳定的系统受到外界干扰的时候，系统就面临着失稳的

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

12、维持它的稳态平衡。也就是，在每一种稳态情况下，在一定较小时间内启动陀螺仪维持该稳态平衡，使其当前稳态的姿势状况不变。比如机器人在行走一步时，走完前半步处于稳态（A），再走完下半步处于稳态（B）。在前半步的稳态（A）时，启动陀螺仪及时修正该稳态的偏移，以维持该状态不变。下半步依然如此。如果机器人处于站立等静止状态时，就等同处于一种稳态的情况，依然启动陀螺仪及时修正偏移，以维持稳态的姿势。欲使机器人所有运动姿势正常化，首先调节好机器人所有运动姿势中每一种稳态正常化。如下面流程图3-3所示。稳定态（A）可以是静态也可以是动态。稳定态（B）可以是静态也可以是动态。稳定态（ B ）“跳位段”也是由反馈量

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

14、器人的姿势。本调整程序工作逻辑方式是，快速反馈，快速回程。主要使用的函数名称列举如下：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_

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

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

17、否？标志前后稳定否是调整舵机从0到5号的值，使机器人往前倾移左摆动角速度超限否？否调整舵机从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;陀螺仪是否插上？该段代码用于保

18、存机器人当前所处某一姿势时相关要调整的舵机各值，其中舵机有从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;保存临时状态该段代码用于调用读取出陀螺仪三轴数字值的处理函数，其如下：Read_IIC_XYZAxis();读取陀螺仪三轴数字值前摆动角速度超限否？该段代码用于判断机器人当前所处某一姿势时是否发生向前倾移，如倾移则调整舵机从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);是调整舵机从0到5号的值，使机器人往后倾移该段代码用于判断机器人当前所处某一姿势时是否发生向后倾移，如倾移则调整舵机从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;后摆动角速度超限否？是调整舵机从0到5号的值，使机器人往前倾移该段代码用于判断机器人当前所处某一姿势时是否发生向左倾移，如倾移则调整舵机从8号到11号的值，使机器人向右倾移。具体代码如下：if(L3G4200D_dis_data0 < LR_Amp0) /左偏position8 += 1;position10 += 1;position11 -= 1; /*position9 -= 1; /*PWM_24();Lowlevel_500u(delays_value); 左摆动角速度超限否？是调整舵机从8到11号的值，使机器人往右倾移右摆动角速度超限否？该段代码用于判断

22、机器人当前所处某一姿势时是否发生向右倾移，如倾移则调整舵机从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;是调整舵机从8到11号的值，使机器人往左倾移该段代码用于恢复机器人调整前所处某一姿势，其中需要恢复的舵机有从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;恢复临时状态以下表为在木地板上不同速度下行走时参考参数：环境速度值S前后摆动旋转角速度幅限值X左右摆动旋转角速度幅限值Y备注在木地板上（稍光滑）临界值较佳值临界值较佳值（1） 当要左右平移或者转动时，建议将速度值增大到5以上，避免机器人因自身产生较大的惯性力而使其平移或转动角度过大导致失常态。（2） 调节速度值越小时，调整参数的临界值也相应变大（各30以上），较佳值亦同。（3） 调节速度值越小时，调整参数的临界值也相应变小（最小到25为止），较佳值亦同。0304530451304530452304

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

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

27、540以下表为在地毯、卵石地板等上不同速度下行走时参考参数：环境速度值S前后摆动旋转角速度幅限值X左右摆动旋转角速度幅限值Y备注在地毯、卵石地板等上临界值较佳值临界值较佳值（10） 当要左右平移或者转动时，建议将速度值增大到5以上，避免机器人因自身产生较大的惯性力而使其平移或转动角度过大导致失常态。（11） 调节速度值越小时，调整参数的临界值也相应变大（40以上），较佳值亦同。（12） 调节速度值越小时，调整参数的临界值也相应变小（最小35），较佳值亦同。（13） 在这些较为复杂一点的地面上行走时，调节的参数均偏大些，至少40以上，否则会出现失常行走。340454045440454045540

28、454045635453545735453545835403540.3035403540参数调节注意事项：（1）当机器人在不如木板那么整平的地面上或者更复杂的地面上行走的时候，速度值参数尽可能加大，陀螺仪X、Y值也尽可能加大。也就是，地面越复杂，越要慢速行走，否则机器人就会失常。（2）机器人在行走时，其中某些舵机有时会出现突然抖动，会造成行走常态突发改变，这种情况下最好先让机器人停一下再接着行走。出现这种现象，在于机器人行走时某一时间某些舵机因供电不足或者线性度不良所导致的。此下，只能调大参数，使机器人慢速行走。（3）当机器人在正常行走时，调节陀螺仪参数X、Y基本上在35到50之间变化。当出现

29、地面复杂时，建议把行走速度S调到3以上。2静止站立姿态调整程序机器人静止站立姿态的调整原理，等同机器人在行走过程中当双腿着地时某一姿态的调整原理，但有其特殊之处。区别是，当静止状态被迫改变时，依旧循环采集陀螺仪数值，及时判断是否超限并且修改相应舵机的值，以至使机器人回归到静止站立姿态，不过使机器人回归站立姿态过程，采用了软回程的方式，而行走时采用快速回程的方式。另外，机器人前后、左右的回程以分开的方式控制。该调整程序的工作逻辑方式是，快速反馈，缓速回程。主要使用的函数名称列举如下：void Read_IIC_XYZAxis(void) /采集三轴数字值函数void ExecuteL3G4200

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

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

32、(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 = position

33、8;position_temp9 = position9;position_temp10 = position10;position_temp11 = position11;该段代码用于调用读取出陀螺仪三轴数字值的处理函数，其如下：Read_IIC_XYZAxis();读取陀螺仪三轴数字值前摆动角速度超限否？该段代码用于检测与调整前后的姿态，需操作的舵机有从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 此处调用前后软回程的代码段调整舵机从0到5号的值，使机器人往后倾移后摆动

35、角速度超限否？调整舵机从0到5号的值，使机器人往前倾移该段代码用于机器人调整姿势之后，需要恢复前后方向的舵机到静止站立时的舵机状态，恢复的舵机有从0号到5号，即恢复事先保存的临时状态值，并置前后回程结束标志。具体代码如下：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);启动前后的软回程该段代码用于机器人调整姿势之后，需要恢复左右方

37、向的舵机到静止站立时的舵机状态，恢复的舵机有从8号到11号，即恢复事先保存的临时状态值，并置左右回程结束标志。具体代码如下：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);启动左右的软回程该段代码用于检测与调整左右的姿态，需操作的舵机有从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 此处调用左右软回程的代码段左摆动角速度超限否？调整舵机从8、9号的值，使机器人往左撇腿右摆动角速度超限否？调整舵机从10、11号的值，使机器人往右撇腿3左右平移姿态调整程序机器人左右平移姿态的调整原理，等同机器人在行走过程中当双腿着地时某一姿态的调整原理，但有其特殊之处。区别是，当每平移完一步处于站立状态时，再做姿态调整。在平移完

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

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

42、标志置位否？是否左摆动角速度超限否？否是调整舵机从8、9号的值，使机器人往左撇腿右摆动角速度超限否？否启动左右的软回程置左右回程结束标志是调整舵机从10、11号的值，使机器人往右撇腿前后左右回程标志置位否？否返回读取陀螺仪数值是退出调整函数图4-7 平移姿态调整流程图该姿态调整函数的各块代表代码如下：该段代码用于判断陀螺仪是否插上，未插上则退出调整函数，其如下：if(L3G_flag_plug1 = 1)if(psx_mode != 0xFF)L3G_Delay10ms();if(flag_acc_enable = 1)adc_adjust();return;陀螺仪是否插上？该段代码用于保存机

43、器人当前所处某一姿势时相关要调整的舵机各值，其中舵机有从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;保存临时状态读取陀螺仪三轴数字值该段代码用于调用读取出陀螺仪三轴数字值的处理函数，其如下：Read_IIC_XYZAxis();该段代码用于机器人调整姿势之后，需要恢复前后方向的舵机到站立时的舵机状态，恢复的舵机有从0号到5