基于三轴重力加速度传感器的机器人状态检测.docx

《基于三轴重力加速度传感器的机器人状态检测.docx》由会员分享，可在线阅读，更多相关《基于三轴重力加速度传感器的机器人状态检测.docx（37页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、基于三轴重力加速度传感器的机器人状态检测学生姓名：赵彤彤 指导教师姓名：裴东学生届别：2016届 专业：电气工程及其自动化 班级：2016级电气工程及其自动化专业1班 学号：201672050137摘要本设计以STM32单片机为主控制系统，采用ADXL345三轴重力加速度传感器实现机器人不同状态的检测，如上坡、下坡、跌倒等。机器人硬件由主控制器模块、电源模块、传感器模块及其他硬件自主搭建。通过统计三轴重力加速度传感器ADXL345的X轴、Y轴、Z轴的信号，获得机器人状态突变瞬间的三轴加速度数据信息，从而实现机器人的状态检测。机器人技术目前已经成为了全球最热点的研究领域之一，保证机器人运行过程中

2、的可靠性和稳定性尤为重要，而机器人的状态检测对机器人正常工作的可靠性和稳定性具有重要作用。关键词: STM32、ADXL345三轴重力加速度传感器、机器人的状态检测。AbstractIn this design, STM32 microcontroller is used as the main control system, and ADXL345 three-axis weight force acceleration sensor is used to realize the detection of different states of the robot, such as uphi

3、ll, downhill, fall, etc.The robot hardware is independently built by the main controller module, the power module, the sensor module and other hardware.Through the statistics of the X-axis, Y-axis and z-axis signals of the three-axis gravity force acceleration sensor ADXL345, the three-axis accelera

4、tion data information of the moment of sudden state change of the robot can be obtained, so as to realize the state detection of the robot.At present, robotics has become one of the most popular research fields in the world. It is particularly important to ensure the reliability and stability of rob

5、ot operation, and the state detection of robot plays an important role in the reliability and stability of robot operation.Key words: STM32, ADXL345 three axle weight force acceleration sensor, Robot status detection.目录引言11.系统概述及方案选择21.1机器人状态检测概述21.1.1 机器人状态检测背景及意义21.1.2 机器人状态检测研究目标21.2 方案选择及整体设计21.

6、2.1 主控制器的选择21.2.2 传感器的选择31.2.3 整体设计42.硬件设计42.1 主控制器42.1.1 STM32F103系列单片机介绍42.1.2 STM32F103ZET6单片机最小系统52.1.3 STM32F103ZET6原理图及PCB图72.2 舵机模块82.3 传感器模块82.3.1 ADXL345三轴重力加速度传感器介绍82.3.2 ADXL345三轴重力加速度传感器内部结构及原理92.3.3 ADXL345三轴重力加速度传感器引脚配置和功能描述102.3.4 ADXL345三轴重力加速度传感器数据读取112.4 电源稳压模块122.5 机器人机械结构设计123.机器

7、人状态检测143.1跌倒状态检测143.2上坡及下坡状态检测163.3ADXL345与STM32F103ZET6串行通信174.软件设计184.1软件开发平台18图4-1 Keil5软件开发界面图184.2 三轴重力加速度传感器ADXL345数据获取的部分程序184.3 机器人状态调整部分程序224.3.1 机器人正常行走部分程序224.3.2 机器人上坡部分程序244.3.3 机器人跌倒复位部分程序27总结35致谢36参考文献37引言传统机器人综合电子信息工程、计算机科学与技术、传感器技术、仿生学等多门学科，是多领域集合的产物。随着机器人技术的成熟与发展，智能机器人已经进入人们的生活，与机器

8、对话已经不再是天方夜谭。机器人作为科技浪潮革新下的新兴产业的代表，它的发展备受世界各国关注，也成为了各国科技发展制高点的竞争。20世纪50年代，世界上第一台可编程机器人在美国被制造后，机器人产业便表现出很大的生命力。目前机器人已经在工业、农业、服务业、医学等领域有着极其广泛的应用。2015年我国国务院颁布的中国制造2025更是将机器人产业提高到了国家战略层面，这为我国机器人技术的发展奠定了良好的基础。我国机器人的研究从二十世纪七十年代开始，1975年由于我国企业技术改造的需要，在上海的针织九厂出现了插销板机器人，上海同和电机厂也有了压铸用机器人，但这些机器人都是用步进电机驱动。机器人的位置、时

9、间以及顺序信息由插销插孔预设坐标实现对机器人的点位顺序控制。在此期间我国机器人技术的研究处于一种闭塞的状态，我们对国外机器人技术没有办法进行系统的了解，机器人的研发也在无序、零星地进行。1977年嘉兴会议是我国历史上第一个以机器人作为会议主题的大型会议，自此之后，我国机器人技术的研究步入了一个崭新的时代。美国是最早研发和应用机器人的国家，近代第一台机器人由美国人G.戴伏尔研究发明。这台机器人能够实现编程再现并且应用点到点的控制，具有记忆功能。受到美国机器人热潮的影响，六十年代末由于日本经济高速发展，日本将机器人产业作为解决劳动力不足的一项重要措施。日本是农业机器人研究最早并且市场发育最成熟的国

10、家，美国、欧洲、澳洲紧随其后。七十年代末，受日本机器人技术应用成功的影响，世界各国开始重视机器人技术的发展，机器人技术的研究与发展迎来了全球范围内的第二次高潮。在1980至1985年间，法国政府制定了高级自动化及机器人发展规划，英国、意大利、加拿大等国家也制定了相应的机器人发展计划。保加利亚、新加坡等一些发展中国家也相继开始机器人技术的研究，制定自己的机器人发展计划。目前全球机器人产业的市场规模持续增长，我国的机器人市场需要具有很大的潜力，工业机器人、服务机器人、特种机器人、医疗机器人等在各个领域的应用日益普遍，而如何保证机器人在运行过程中的可靠性、稳定性和安全性已成为研究的热点问题，其中机器

11、人的状态检测尤为重要。1.系统概述及方案选择1.1 机器人状态检测概述1.1.1 机器人状态检测背景及意义自20世纪50年代世界第一台机器人问世以来，机器人技术高速发展，机器人产业也日渐繁荣。全球各个国家都对机器人技术进行不断研发和创新。我国机器人产业蓬勃发展，机器人在工业方面的应用已经非常普遍，机器人在农业、服务业和医学领域的应用更是逐渐推广。机器人可以在危险的情况下替代人工作业，完成人工不能完成的任务，保护人们的生命和财产安全，而机器人的状态检测是保障机器人生存和成功完成作业任务的关键。1.1.2 机器人状态检测研究目标本文研究内容是基于三轴重力加速度传感器的机器人状态检测的设计。机器人使

12、用STM32为主控制器，利用舵机让机器人能够正常行走，同时使用三轴重力加速度传感器，实现机器人不同状态下的状态检测。主要研究内容包括以下方面：1. 机器人方案选择及整体设计 2. 三轴重力加速度传感器方案选择 3. 机器人状态检测 1.2 方案选择及整体设计1.2.1 主控制器的选择方案一：选用比较普及的89C51单片机作为主控制器。89C51简单便捷、价格低廉，学习资料比较多，易于自主学习和掌握；但89C51单片机芯片不能定义成内部复位方式，只能用外部微分电路复位，功耗较高，抗干扰能力弱，I/O口定时器资源少。本文设计机器人系统需要较多的I/O口与定时器资源，不太适合采用89C51单片机作为

13、主控制器。方案二：选用STM32F103ZET6作为主控制器，其性价比高，32位的控制器有着接近于16位甚至高端8位控制器的价格，从成本到功能均易于选择。且与89C51单片机相比较，STM32F103ZET6速度快，配置灵活，功能强大，功耗低，I/O口与定时器资源丰富，学习资料易于获取。方案三：选用 FPGA 作为主控制器。FPGA 是现场可编程门阵列，利用 VHDL编程可以实现各种复杂的逻辑功能，体积小、灵活性强、稳定性高、处理速度快、I/O口资源丰富。如果是大型系统FPGA作为主控制器是很好的选择，但是本文设计机器人系统较小，不太适合采用FPGA作为主控制器。综合以上三种方案，选择方案二。

14、1.2.2 传感器的选择方案一：选用行程开关。行程开关是利用物体与行程开关的碰撞使它的触头动作来接通或分断控制电路从而达到一定控制目的的一种位置开关。当行程开关的触头与物体发生碰撞被按下时，行程开关输出低电平；当行程开关的触头未与任何物体接触时，行程开关一直保持高电平。在此设计中也可以用作机器人的状态检测，但是检测机器人状态模式单一，只能检测机器人是否跌倒，不太适合机器人状态检测的要求。方案二：选用ADXL345三轴重力加速度传感器。三轴重力加速度传感器ADXL345能以很高的灵敏度分别读出状态突变时X、Y、Z 三个轴输出的加速度参数，它是同类产品中的第一个单芯片三轴向加速器，其成本低、容易携

15、带、学习资料易于获取，能很好地获得机器人状态发生变化时各种特征的变化。方案三：采用OpenMV 摄像头。OpenMV是一种处理速度快、功能强大的机器视觉传感器。OpenMV集成了 STM32F427CPU 以及Ov7725摄像头芯片，使用 Python 语言进行编程，简单方便。OpenMV 高效、低功耗、误差小、易上手，受环境限制小，可以很好地检测到机器人周围环境的变化，但是对机器人状态变化时的特征难以把握和判断，检测存在着不准确性，不太适合作为机器人状态检测的传感器。综合以上三种方案，选择方案二。1.2.3 整体设计本系统主要由STM32模块、ADXL345传感器模块、电源模块、机械架构等组

16、成。STM32作为机器人控制系统的控制核心，利用ADXL345采集过来的数据判断机器人当时是处于何种状态，从而控制机器人动作的调整和恢复。STM32控制系统机器人ADXL345传感器模块图1-1 系统总体方案框图2. 硬件设计2.1 主控制器2.1.1 STM32F103系列单片机介绍STM32F103系列单片机是意法半导体公司生产设计的基于 ARM 核心的 32 位增强型 MCU，使用高性能的ARMCortex-M332位的RISC内核，工作频率为72MHz，内置高速存储器，片上集成 32-512KB 的 FLASH 存储器，6-64KB 的SRAM存储器，还有 D/A、A/D 转换电路，1

17、1 个定时器，高达 112 个 I/O 端口。STM32具有数据处理能力强、响应速度快、功耗低、实时性强、编程简单、内存大等优点，提供了优越的实时特性。高性能、 低功耗使STM32作为机器人的主控制器为一个不错的选择。图2-1 STM32F103ZET6最小系统板实物图2.1.2 STM32F103ZET6单片机最小系统2.1.2.1 电源电路电源电路主要用于给MCU、时钟芯片及其他电路提供电能。电源模块主要输出为5V和 3.3V 的电压。其中要使机器人舵机正常工作需要5V 电压，要使单片机以及其它外围芯片正常工作需要 3.3V电压，因此机器人主控制器需要有将5V电压转换为3.3V电压的芯片，

18、在此机器人中选择AMS1117-3.3V电源芯片使我们可以在开发板上引出 3.3V与5V两种不同大小的电源。图2-2 STM32F103ZET6最小系统电源电路图2.1.2.2 复位电路复位是对单片机内部所有寄存器进行初始化操作。当按键S1未按下时，电容隔直流通交流使电路呈断路状态；当按键S1按下时，电容开始充放电，当电容充满电时，单片机复位。图2-3 STM32F103ZET6最小系统复位电路图2.1.2.3 晶振电路晶振电路为系统提供基本时钟信号。图2-4 STM32F103ZET6最小系统晶振电路图2.1.2.4 下载电路STM32F103ZET6的程序下载方式分别为SWD 和 JTAG

19、 两种方式，这里我们使用JTAG方式。我们通常使用J-link进行程序下载，其中J-link的VCC、GND、CLK、DIO四根线分别与USB口对应焊接，连接到机器人的主控制器实现机器人的程序下载。图2-5 J-link实物图2.1.3 STM32F103ZET6原理图及PCB图图2-6 STM32F103ZET6原理图图2-7 STM32F103ZET6 PCB图2.2 舵机模块舵机是由直流电机、减速齿轮组、传感器和控制电路组成的一套自动控制系统，通过输出PWM波指定舵机旋转的角度。舵机的控制信号是PWM信号，PWM信号占空比发生改变舵机转动的角度发生改变，舵机有着旋转角度的限制。普通直流电

20、机与舵机的区别在于普通直流电机可以360度一圈一圈转动，而舵机只能在规定的可转动范围内转动。数字舵机可以在舵机和电机这两种模式中任意切换，因此本次设计我们采用数字舵机，用STM32单片机进行编程改变PWM信号的占空比，进而改变舵机上的电压大小，舵机转动并且转动角度不同，从而实现机器人多自由度的控制。在此设计中我们选LDX-218型号数字舵机。LDX-218型号数字舵机脉宽调制信号T是20ms，脉宽范围是0.5ms -2.5ms，对应舵机转动角度是0-180（-90-90）度。 图2-8 LDX-218舵机实物图及PWM波输出图2.3 传感器模块2.3.1 ADXL345三轴重力加速度传感器介绍

21、 ADXL345是美国ADI公司生产的一颗三轴数字输出 iMEMS 加速度传感器产品。ADXL345能在X、Y、Z 三个轴上以非常高的灵敏度读取低重力水平下的坠落、倾斜、移动、放置、震动和摇摆等参数，它是同类产品中的第一个单芯片三轴向加速器，主要以成本低、容易携带而被广泛应用。ADXL345的主要特性如下6：12，4，8，16g可变的测量范围2最高13bit分辨率，固定的4mg/LSB灵敏度33mm*5mm*1mm超小封装423至140uA超低工作功耗，休眠状态功耗低至0.1uA5标准的I2C或SPI数字接口632级FIFO存储7内置多种运动状态检测功能和灵活的中断方式2.3.2 ADXL34

22、5三轴重力加速度传感器内部结构及原理2.3.2.1 ADXL345三轴重力加速度传感器内部结构ADXL345三轴重力加速度传感器对其X、Y、Z三个轴方向上的加速度信息有一种特殊的敏感性，可以瞬间采集到三个轴向的加速度信号。经过振荡器、容压变换器、增益放大、滤波器以及温度补偿后最终以电压信号的形式输出。6图2-9 ADXL345三轴重力加速度传感器内部结构功能图2.3.2.2 ADXL345三轴重力加速度传感器工作原理ADXL345是多晶硅表面微加工结构，置于晶圆顶部。多晶硅弹簧吊挂在晶圆表面的结构上提供阻力；独立固定板和活动质量连接板组成差分电容对结构偏转进行测量。加速度使惯性质量偏转、差分电

23、容失衡，使传感器输出的幅度与加速度成正比。相敏解调用于确定加速度的幅度和极性。6图2-10 ADXL345三轴重力加速度传感器检测轴图2-11 ADXL345三轴重力加速度传感器输出响应与重力方向关系2.3.3 ADXL345三轴重力加速度传感器引脚配置和功能描述图2-12 ADXL345三轴重力加速度传感器引脚配置表2-1 ADXL345三轴重力加速度传感器引脚功能说明表2.3.4 ADXL345三轴重力加速度传感器数据读取ADXL345是16位数据格式，我们必须对从数据寄存器中获取的加速度数据进行重建。DATAX0是X轴加速度的低字节寄存器，DATAX1是X轴加速度的高字节寄存器。在13位

24、模式下，高4位是符号位(如图2-13所示)。DATA_READY中断信号是三轴加速度数据更新的标志，当新的数据来临时它会被变成高电平，通过DATA_FORMAT寄存器中断信号可设置为从低电平变成高电平，利用低-高跃迁来触发中断服务。我们可从 DATAX0、DATAX1、DATAY0、DATAY1、DATAZ0和DATAZ1这六个寄存器中读取数据，为了确保数据的一致性我们一般使用多字节读取从ADXL345获取到的数据5（如图2-14所示为4线式SPI读序列）。图2-13 ADXL345三轴重力加速度传感器数据结构图图2-14 ADXL345三轴重力加速度传感器4线式SPI连接的数据读取时序2.4

25、 电源稳压模块电源稳压模块使用锂离子电池供电，锂离子电池能量密度高、安全性强、高温性能好，具有输出功率高、寿命循环长、环保等优点。本次设计使用锂离子电池，可重复进行充放电，使用方便。图2-15 锂离子电池实物图2.5 机器人机械结构设计本设计通过对人体结构及简单力学模型的分析设计并制作了一个基于STM32单片机控制下的由15个舵机协调工作的仿人形机器人，其中1个舵机作为机器人头部关节；2个舵机作为机器人肩部关节；机器人左右两只机械臂各有2个舵机；左右两条腿各有4个舵机。机器人采用STM32单片机作为机器人的控制核心，机器人系统从硬件上分为主控制器模块、舵机模块、传感器模块和电源稳压模块。 图2

26、-16 机器人舵机支架实物图图2-17 机器人机械结构模型图图2-18 机器人机械结构实物图3. 机器人状态检测3.1 跌倒状态检测三轴重力加速度传感器被固定在被测机器人上。Y轴方向（垂直方向）的加速度曲线为红色，其正常静止状态下为-1g； X轴方向（前后方向）的加速度曲线为黑色，其正常静止状态下为0g；Z轴方向（左右方向）的加速度曲线为黄色，其正常静止状态下为0g；三轴加速度的矢量和为绿色曲线，其正常静止状态下为+1g。机器人跌倒过程中的加速度变化主要有4个特征。图3-1 机器人意外跌倒加速度变化曲线图图3-2 机器人意外跌倒加速度变化曲线图（分析）这4个特征在图3-2中用红色的方框标出，下

27、面是详细介绍:1. 失重: 我们在刚跌倒时一般都会发生一定的失重现象。物体做自由落体运动时，它的失重现象会变得更加明显，加速度的矢量和会降低到接近0g。一般的跌倒情况，失重现象虽然不会像自由落体运动那么明显，但是也可能会发生合加速度小于1g的情况（通常情况下合加速度应大于1g）。因此这可以作为机器人跌倒检测的第一个判断依据，可以用三轴加速度传感器ADXL345的FREE_FALL中断来检测。2. 碰撞:机器人发生跌倒的时候会与地面或着其他物体发生碰撞，表现在加速度曲线中会出现一个很大的冲击。这个冲击在失重现象之后，我们可以通过三轴重力加速度传感器ADXL345的Activity中断来检测。因此

28、在三轴重力加速度传感器ADXL345的FREE_FALL中断之后，紧接着产生的Activity中断是机器人跌倒的第二个依据。3. 静止: 通常情况下机器人在跌倒（撞击发生）之后，不可能立马调整姿势站立起来，会有一个静止的状态，表现在加速度曲线上会有一段时间的平稳。这段时间可以通过三轴重力加速度传感器ADXL345的Inactivity中断来检测。因此在三轴重力加速度传感器ADXL345的Activity中断之后的Inactivity中断是机器人跌倒的第三个依据。4. 与初始状态比较: 跌倒之后机器人会发生翻转，因此机器人的方向会与初状态（静止站立状态）不同。这使得跌倒之后的静止状态下的三轴加速

29、度数值与初始状态下的三轴加速度不同。三轴重力加速度传感器被固定在被测机器人的某个位置，这样初始状态下的三轴加速度数值可以认为是已知的（初始状态为X轴0g、Y轴-1g、Z轴0g）。读取Inactivity Y轴上的-1g变为了Z轴上的1g，说明机器人发生了侧向跌倒。因此，机器人跌倒后的静止状态下加速度值与初始状态加速度值发生变化且矢量变化超过一定的门限值可以作为机器人跌倒检测的第四个依据。这四个判断依据综合在一起可以作为机器人的跌倒检测依据，及时对机器人的跌倒给出提醒，在实际情况中我们还要注意各个中断之间的时间间隔要在合理的范围之内。3.2 上坡及下坡状态检测由于我们自己搭建的机器人的运动相对比

30、较慢，所以在普通的行走过程中机器人加速度不会发生很大的变化。图3-3为机器人上坡状态加速度变化曲线，图3-4为机器人下坡状态加速度变化曲线。 图3-3 机器人爬坡状态加速度变化曲线图图3-4 机器人下坡状态加速度变化曲线图3.3 ADXL345与STM32F103ZET6串行通信三轴重力加速度传感器ADXL345可以采用I2 C和SPI数字通信两种方式。上述两种情况下ADXL345作为从机运行，CS引脚上拉至VDD I/O，I2 C模式使能。CS引脚应始终上拉至VDD I/O或由外部控制器驱动，由于CS引脚无连接时默认模式不存在，因此如果没有采取这些措施可能会导致器件无法通信。在SPI模式下，

31、CS引脚由总线主机控制。在与单片机STM32F103ZET6通信时我们选择三轴重力加速度传感器ADXL345的I2C通信方式。图3-5 ADXL345 I2 C通信方式下端口连接图三轴重力加速度传感器ADXL345使用I2C通信方式时，CS引脚必须连接VDD I/O，ALT ADDRESS引脚必须连接任一VDD I/O或接地。要使I2C方式正确操作需要外接上拉电阻RP。三轴重力加速度传感器ADXL345 SCL、SDA、INT1、INT2端口分别与单片机STM32F103ZET6任一I/O口（PB6、PB7、PB10、PB11）连接。4. 软件设计4.1软件开发平台此次设计程序编写使用Keil

32、5软件来完成，Keil5是基于C语言的STM32系列开发板的编程开发软件。其操作界面简单，使用方便，易于操作和学习。图4-1 Keil5软件开发界面图4.2 机器人状态调整部分程序4.2.1 机器人正常行走部分程序void qianjing(void)frpapostatus1 = 0;while(1)if(frpapostatus1 = 0)&(frpapostatus2 = 0)next_state6 = frpapo6 + 18;next_state7 = frpapo7 + 20;next_state13 = frpapo13-2;update(200);delay_ms(50);if

33、(frpapostatus2 = 0)if(LEFT2=1)&(RIGHT2=0)next_state7 = frpapo7 +10;update(220);delay_ms(5);if(LEFT1=1)&(RIGHT1=0)next_state4 = frpapo4 + 6;next_state5 = frpapo5 + 4;next_state8 = frpapo8 + 10;next_state9 = frpapo9 + 12;next_state10 = frpapo10 + 6;next_state3 = frpapo3;update(210);delay_ms(10);else i

34、f(LEFT1=0)&(RIGHT1=1)next_state4 = frpapo4 + 6;next_state5 = frpapo5 + 4;next_state8 = frpapo8 + 10;next_state9 = frpapo9 + 12;next_state10 = frpapo10 - 3;next_state3 = frpapo3;update(210);delay_ms(10);elsenext_state4 = frpapo4 + 10;next_state5 = frpapo5 + 10;next_state8 = frpapo8 + 16;next_state9 =

35、 frpapo9 + 16;next_state10 = frpapo10;next_state3 = frpapo3;update(210);delay_ms(10);next_state7 = frpapo7 + 20;update(220);delay_ms(5);elsedelay_ms(1000);delay_ms(1000);if(LEFT2=1)&(RIGHT2=0)frpapostatus1 = 1;break;elsefrpapostatus1 = 0;frpapostatus3 = 1;break;4.2.2 机器人上坡部分程序void fifpapo(void)frpap

36、ostatus1 = 0;while(1)if(frpapostatus1 = 0)&(frpapostatus2 = 0)next_state6 = frpapofif6 + 18;next_state7 = frpapofif7 + 20;next_state13 = frpapofif13 - 2;update(200);delay_ms(100);if(frpapostatus2 = 0)if(LEFT2=1)&(RIGHT2=0)if(LEFT1=1)&(RIGHT1=0)next_state4 = frpapofif4 + 5;next_state5 = frpapofif5 +

37、5;next_state8 = frpapofif8 + 11;next_state9 = frpapofif9 + 9;next_state10 = frpapofif10 + 4;next_state3 = frpapo3;update(210);delay_ms(10);else if(LEFT1=0)&(RIGHT1=1)next_state4 = frpapofif4 + 5;next_state5 = frpapofif5 + 5;next_state8 = frpapofif8 + 11;next_state9 = frpapofif9 + 9;next_state10 = fr

38、papofif10 - 4;next_state3 = frpapo3;update(210);delay_ms(10);elsenext_state7 = frpapo7 +10;update(220);delay_ms(5);next_state4 = frpapofif4 + 9;next_state5 = frpapofif5 + 9;next_state8 = frpapofif8 + 18;next_state9 = frpapofif9 + 16;next_state10 = frpapo10;next_state3 = frpapo3;update(210);delay_ms(

39、10);next_state7 = frpapo7 + 23;update(220);delay_ms(5);elsedelay_ms(1000);delay_ms(1000);if(LEFT2=1)&(RIGHT2=0)frpapostatus1 = 1;break;elsefrpapostatus1 = 0;frpapostatus3 = 1;break;4.2.3 机器人跌倒复位部分程序void fif_stage(void)while(1)if(frpapostatus3=1)if(LEFT2=0)&(RIGHT2=0)if(LEFT1=1)&(RIGHT1=0)next_state4

40、 = frpapofif4 + 5;next_state5 = frpapofif5 + 5;next_state8 = frpapofif8 + 11;next_state9 = frpapofif9 + 9;next_state10 = frpapofif10 + 4;next_state3 = frpapo3;update(210);delay_ms(10);else if(LEFT1=0)&(RIGHT1=1)next_state4 = frpapofif4 + 5;next_state5 = frpapofif5 + 5;next_state8 = frpapofif8 + 11;next_state9 = frpapofif9 + 9;next_state10 = frpapofif10 - 4;next_state3 = frpapo3;update(210);delay_ms(10);elsenext_state7 = frpapo7 +10;update(220);delay_ms(5);next_state4 = frpapofif4 + 9;next_state5 = frpapofif5 + 9;next_state8 = frpapofif8 + 18;next_state9 = frpapofif9 + 16;