清洁机器人系统设计

清洁机器人作为为人类服务的特种机器人，它是服务机器人不可或缺的一部分。它能够代替人完成家庭地面的清理，也是目前最有可能进入千家万户的服务机器人。清洁机器人涉及到机构学、运动学导航和路径规划等关键科学问题，在服务机器人方面是近年来国内外学者重点研究的领域之一，同时清洁机器人技术作为近些年新兴起的高新技术，国内外各大智能机器人厂商都对其做了广泛的深入研究。 世界各国尤其是西方发达国家都在致力于研究开发和广泛使用服务机器人。如果清扫机器人的性价比足够高，那么清洁机器人的市场将会被看好。 本文简要介绍了清洁机器人在国内外的发展现状和应用情况，实验前期，对各个模块进行了解，例如电机、STM32、接近开关、蓝牙模块、黑白线识别模块等等，对这些进行学习程序并编写。接着研究了清洁机器人的手机蓝牙控制、红外线避障、红外线黑白线循迹。结合实验室实际情况，设计了机器人样机。其主要工作内容包括：小车机械本体设计、硬件设计、电气原理图、STM32单片机控制系统硬件电路、控制系统软件设计和机器人的手机蓝牙控制测试试验、红外线避障实验、红外线黑白线循迹实验。后期进行对小车的机械结构进行完善，并进行三维图二维图的绘制。通过实验表明所设计的机器人样机能够实现手机蓝牙控制前后左右移动的功能，达到设计要求。机器人能够智能实现避障和循迹的功能。关键词：清洁机器人 手机蓝牙控制 STM32单片机 避障 循迹 目录摘要I第一章 设计综述11.1 智能机器人国内外系统发展现状11.1.1 国外产品研究状况21.1.2 国内产品研究状况21.2 智能机器人系统设计任务概述31.3课程设计主要内容3第二章 方案设计52.1机械结构方案设计52.2驱动方案选择52.3 传感器的选择62.4 结构的合理性8第三章 机械系统设计93.1 机械系统总体方案93.2关键零部件结构设计93.2.1 机器人外形设计93.2.2 机器人的行走机构设计10第四章 控制系统设计114.1 控制系统总体方案114.2 选型114.3驱动电机控制电路设计114.4 传感检测外围电路设计124.5 电源供电电路设计134.6 总的电路图134.7 STM32平台IO分布图14第五章 软件系统设计155.1 软件系统总体方案155.2 速度控制方案155.3 流程图165.3.1 手机蓝牙控制流程图165.3.2 红外线避障流程图175.3.3 红外线循迹流程图185.3.4 超声波循迹流程图19第六章 装配与调试206.1 机器人系统整体装配20III6.2系统调试流程与状况206.3 设计创新点216.4 系统缺陷与改进21第七章 市场应用前景分析227.1 市场调查分析227.2 市场背景227.3 市场的行业分析237.4 市场的预测23项目心得24参考文献25附录一 成本分析26附录二 程序源代码27IV第一章 设计综述1.1 智能机器人国内外系统发展现状1.1.1 国外产品研究状况对于机器人大家可能不会太陌生，工业机器人在很多领域都得到了比较广泛的应用，但是对于家庭机器人我们所了解的却相当的少，而日本，欧美等国家的研究则比较领先，有的都已经投入市场，在实际中投入使用。2002年10月1日，瑞典的拉克斯电子公司与日本东芝公司共同开发的清洁机器人 “特里洛巴伊特”上市销售，“特里洛巴伊特”主要由清扫机器和超声波传感器构成，在工作时可避开室内摆放的各种家具用品。只要家庭主妇领着它搞过一次清扫后，它便可以按行走过的清扫线路进行自动清扫。这种机器人是充电式的，每一次充电可连续工作 小时。瑞典家电制造商伊莱克斯（EIectoIux）研制生产的清洁机器人小“三叶虫”高13，直径35，表面光滑，呈圆形，内置搜索雷达，可以迅速地探测到并避开桌腿、玻璃器皿、宠物或任何其它障碍物。一旦微处理器识别出这些障碍物，它可重新选择路线，并对整个房间做出重新判断与计算，以保证房间的各个角落都被清扫。在楼梯的台阶等一些没有天然障碍物的地方，只要有一条磁铁，小“三叶虫”便不会跨越。小“三叶虫”开始启动后，体内的搜索雷达会探测出距离最近的墙壁，先顺着墙壁把地板四周的灰尘及异物吸尽。这样它便能探测出整个房间的格局，计算出清扫整个房间所需的时间。只要一接近一件障碍物，它便会重新设定行进路线，不会漏掉每一个角落。电线或地毯的边缘不会被认作是障碍物。小 “三叶虫”的吸刷装置中装有一只专利设计滑轮，可以越过电线或地毯边缘，不被绊住。电源不足时，小“三叶虫”会自动回到充电卡座自行充电。如果此时房间还没有清扫完毕，小“三叶虫”还有记忆功能，充好电后自己回到原处继续吸尘，如图1-1所示。图1-1 三叶虫Fig1-1 TRILOBITE1.1.2 国内产品研究状况在国内的一些大学，如哈尔滨工业大学、华南理工大学、上海交通大学等单位也对清洁机器人进行了大量的研究并取得了一些成果，对清洁机器人相关技术如机器感知、机器人导航和定位与路径规划、机器人控制、电源与电源管理、动力驱动等技术的研究则更多，这些都为清洁机器人的研究开发和推广奠定了物质基础和技术基础。哈尔滨工业大学于90年代开始致力于这方面的研究，与香港中文大学合作，联合研制开发出一种全方位移动清扫机器人。该机器人具有如下特点：采用全方位移动技术，使机器人可执行对狭窄区域等死区的清扫任务；采用开放式机器人铰制结构，实现硬件可扩展，软件可移植、可继承，使机器人作为服务载体具有更好的功能适应性；在拥挤环境下的实时避障功能，能更好地适应不断变化的清扫工作环境；遥控操作和自主运动两种运行方式；吸尘机构可实现吸尘腔路的自动转换，提高了吸尘效率。浙江大学于1999年初在浙江大学机械电子研究所开始进行智能吸尘机器人的研究，两年后设计成功国内第一个具有初步智能的自主吸尘机器人。这种智能吸尘机器人工作时，首先进行环境学习：利用超声波传感器测距，与墙保持一定距离行走，在清洁这些角落的同时获得房间的尺寸信息，从而决定清扫时间；之后，利用随机和局部遍历规划相结合的策略产生高效的清扫路径；清扫结束以后，自行回到充电座补充电力。吸尘机器人在5.53.5平方米的实际家庭环境中，工作10分钟可以达到90%以上的覆盖率。更大房间的清扫试验还没有进行。目前，系统正在引入机器视觉和全局图1-2 国内公司生产的机器人KV8Fig1-2 robots domestic production KV8定位功能，力图在多房间环境下，提高自定位能力、智能决策能力以及回归充电效率，最终提高清扫效率。如图1-2所示。KV8保洁机器人是今年在市场以低价位卖得比较火的一款产品，也是国内首个产品化清扫机器人。它广泛用于家庭、办公和娱乐场所，以及其它一些人员不便进入的地方。KV8能够通过自身的碰撞传感器来实现随机的清扫和碰撞处理，需要人工对其电池进行充电，有三种工作模式可以选择，在启动时伴有音乐声。1.2 智能机器人系统设计任务概述a. 具备完整检测系统，机器人能够充分检测外部接触环境（红外线避障）；b. 具备完成控制能力，能够按照任务应用完成相关运动（红外线循迹）；c. 具备足够负载能力，可携带移动电源；d. 具备无线通讯功能，能够实现专用机器人系统的无线远程控制（手机蓝牙控制）。1.3课程设计主要内容（1）资料分析：查阅相关文献资料，对资料进行分析总结。（2）机器人总体设计：确定机器人的具体任务要求，根据任务初步拟定机器人的技术参数、运动形式、机械结构、驱动方案、控制方案等。（3）机器人机械结构设计：将机器人分解为车身结构、运动机构等若干部分，分别对各个结构的关键部件进行详细设计并校核，绘制机器人总装图和关键零部件图。（4）传感和信息检测及信息传输：根据任务要求，完成相关信息检测、处理，并完成信息的正确传输。（5）运动控制方案设计：基于传感信息，采用单片机完成机器人控制系统硬件和软件的设计和系统调试。（6）编制课程设计说明书。第二章 方案设计2.1机械结构方案设计本课题要研制一台灵活，控制简单、易于实现，初步完成手动移动、自动避障和路径规划任务的清洁机器人。整个清洁机器人由机械部分和控制系统两大部分组成。机械部分包括高强度塑料底盘、两个驱动轮和两个万向轮。它们是吸尘电机、清洁刷、电池以及控制系统的载体。机器人的底座大小为32CM35CM，机器人有两个电机对称分布于机器人底座上，机器人清扫部分位于底座下方，底座上方则时控制模块、电池、接近开关等，采用螺栓连接在一起。如图2.1。图2.1 整体设计图2.2驱动方案选择 选用12V直流电机即可。图2.2 直流电机直流电机（direct current machine）是指能将直流电能转换成机械能（直流电动机）或将机械能转换成直流电能（直流发电机）的旋转电机。它是能实现直流电能和机械能互相转换的电机，当它作电动机运行时是直流电动机，将电能转换为机械能；作发电机运行时是直流发电机，将机械能转换为电能。直流发电机的工作原理就是把电枢线圈中感应的交变电动势，靠换向器配合电刷的换向作用，使之从电刷端引出时变为直流电动势的原理。利用PWM原理来控制直流电机的转动。所谓PWM就是脉宽调制器，通过调制器给电机提供一个具有一定频率的脉冲宽度可调的脉冲电。脉冲宽度越大即占空比越大，提供给电机的平均电压越大，电机转速就高。反之脉冲宽度越小，则占空比越越小。提供给电机的平均电压越小，电机转速就低。PWM不管是高电平还是低电平时电机都是转动的，电机的转速取决于平均电压。2.3 传感器的选择1、 选用蓝牙模块来实现手机蓝牙控制图2.3 蓝牙模块2、 选用接近开关来实现避障功能图2.4 接近开关接近开关也叫近接开关，又称无触点行程开关，它除可以完成行程控制和限位保护外，还是一种非接触型的检测装置，用作检测零件尺寸和测速等，也可用于变频计数器、变频脉冲发生器、液面控制和加工程序的自动衔接等。特点有工作可靠、寿命长、功耗低、复定位精度高、操作频率高以及适应恶劣的工作环境等。 3、选用黑白线循迹模块来实现循迹的功能图2.5 循迹模块传感器的红外发射二极管不断发射红外线，当发射出的红外线没有被反射回来但强度不够大时，光敏三极管一直处于关断状态，此时模块的输出端为高电平，指示二极管一直处于熄灭状态被检测物体出现在检测范围内时，红外线背反射回来且强度足够大，光敏三极管饱和，此时模块的输出端为低电平，指示二极管被点亮。2.4 结构的合理性本机构采用两轮驱动，能更好的实现小车的左右移动。如果采用四轮，由于塑胶轮子的摩擦力不足，很难实现转弯的功能。第三章 机械系统设计3.1 机械系统总体方案我们的清洁机器人既能实现手动清洁也能实现智能清扫，车身上装有3个电机，其中2个电机用来带动车轮转动，剩余一个用来带动清扫部分转动，车上上还装有接近开关等，小车车身装有蓝牙模块和黑白线循迹模块。小车电机控制通过单片机来实现控制。(1) 2行走驱动轮及驱动电机。该部分主要保证机器人能够在平面内移动。壳体前端和侧面装有接近开关，作为碰撞检测传感器。底面的3个黑白线检测开关作为红外线检测传感器，可以实现循迹功能。还有蓝牙模块，实现手机蓝牙控制。(2) 清扫机构。用电机带动两个清扫刷，使左面清扫刷顺时针转动，右面逆时针转动，这样就可以在清扫灰尘时将灰尘集中于吸风口处，为吸尘机构的工作做准备；3.2关键零部件结构设计3.2.1 机器人外形设计根据家庭清洁机器人的设计要求，本次设计的机器人应该包括清扫机构、行走机构、吸尘机构机构，其中清扫机构的设计尤为重要。通过在网上搜索一些相关资料以及在图书馆查阅的资料，初步把机器人的外形设计为长宽均为400mm高为100mm的长方体图3.1 主板3.2.2 机器人的行走机构设计为实现机器人的转弯半径为0，在机器人的行走机构中采用两轮驱动，两个驱动轮对称分布在清扫机构的后方，两个轮子和刷子支撑机体工作和运动，这样设计既节省材料，又可以使刷子和地面全面的接触，从而利于更彻底的清扫。在机器人转弯时，通过控制行走机构的两个电机的转速及旋转方向，进而控制两个轮子，通过轮子的前后差速运动实现机体的转弯半径为0。第四章 控制系统设计4.1 控制系统总体方案清洁机器人由多个功能模块共同组成，这几个模块共同工作、相互协调、相互作用，保证了机器人能够顺利的进行清扫。具体的工作原理如下清洁机器人的中心是清洁机器人的STM32，它对其它各个功能模块进行控制。信息采集模块负责采集周围环境以及机器人本身的各种信息。蓝牙遥控接收模块可以接收人们对机器人的控制信息，然后把信息传给STM32进行处理。4.2 选型1、 电机选择直流电机2、 蓝牙模块选择3、 避障选择接近开关4、 黑白线循迹选择4.3驱动电机控制电路设计图3.1 电机模块1图4.2 电机模块24.4 传感检测外围电路设计 图4.3 超声波模块图4.4 舵机4.5 电源供电电路设计图4.5 降压模块图4.6 降压模块24.6 总的电路图图4.7 电路图4.7 STM32平台IO分布图智能小车STM32平台IO分布图模块引脚IO功能电机驱动FRONT_LEFT_F_PIN(AIN1)PA3左前进IOFRONT_LEFT_B_PIN(AIN2)PA4左后退IOFRONT_RIGHT_F_PINPC11右前进IOFRONT_RIGHT_B_PINPA9右后退IOBEHIND_LEFT_F_PINPA10清扫部分IOBEHIND_LEFT_B_PINPA5清扫部分IO电机驱动电机1PWM口PA6速度调控电机2PWM口PA7速度调控电机3PWM口PA8速度调控串口蓝牙RxPB11串口接收IOTxPB10串口发送IO红外循迹SEARCH_M_PIN(DO)PC6中间循迹IOSEARCH_R_PIN(DO)PC5右循迹IOSEARCH_L_PIN(DO)PC4左循迹IO红外避障VOID_R_PINPC7右边红外避障IOVOID_L_PINPC8左边红外避障IO超声波EchoPB14超声波接收IO，可使用外部中断1引脚TrigPB13超声波触发IO舵机DUOJI_PINPB12舵机信号发送IO，发送PWM控制舵机转角第五章 软件系统设计5.1 软件系统总体方案1、 通过蓝牙模块实现手机端的蓝牙遥控，手动实现小车的前后左右和清扫。2、 通过接近开关实现小车的避障功能。3、 通过黑白线循迹模块实现小车的智能循迹功能。5.2 速度控制方案（1） 调节PWM,PWM主要就是控制频率和占空比的：这两个因素分别通过两个寄存器控制：TIMX_ARR和TIMX_CCRX。ARR寄存器就是自动重装寄存器，也就是计数器记到这个数以后清零再开始计，这样PWM的频率就是tim_frequency/（TIMX_ARR-1）。在计数时会不停的和CCRX寄存器中的数据进行比较，如果小于的话是高电平或者低电平，计数值大于CCRX值的话电平极性反相。所以这也就控制了占空比。（2） 调节驱动力例如：/向前void CarGo(void)front_left_speed_duty=SPEED_DUTY;front_right_speed_duty=SPEED_DUTY;behind_left_speed_duty=SPEED_DUTY;behind_right_speed_duty=SPEED_DUTY;调节每个轮的SPEED_DUTY便可进行调速。5.3 流程图5.3.1 手机蓝牙控制流程图5.3.2 红外线避障流程图5.3.3红外线循迹流程图5.3.4 超声波循迹流程图第六章 装配与调试6.1 机器人系统整体装配 图6.1 总体装配图6.2系统调试流程与状况（1）机械机构一开始是使用了四个电机驱动来控制，却发现小车转弯困难，于是修改为两个驱动来控制，再加入两个万向轮，能够很好的实现左右的移动。（2）小车移动过程中，由于电机驱动的电压的差异，导致往前走的时候左右轮的驱动力大小不一样，于是直走存在一定的误差。（3）小车循迹过程中，转弯的驱动力一开始调的太小或者直走的驱动力过大，导致小车还没能转过弯就直走出了规定范围。（4）小车速度的控制，必须设置好小车的速度才能更好的控制小车的移动。（5）黑白线循迹模块与地面的距离不宜过大与过小。（6）接近开关的响应与避障，速度必须足够能使小车转弯。（7）清扫部分由于是用螺栓连接，清扫的方向只有一个，于是电机三一直为一个方向的驱动。 （8）蓝牙模块与手机连接的时候，有时候未能连接上，必须检查好TX与RX的连接，而且检查初始化状态蓝牙是否有灯在闪烁（有没有供电）。6.3 设计创新点清扫模块本身是依靠地面的摩擦力来进行转动，为了更好的控制其转动，设计了一个电机控制其转动。6.4 系统缺陷与改进存在两个万向轮导致了运行过程中的误差更大。第七章 市场应用前景分析7.1 市场调查分析目前家庭清洁机器人是使用数量最多，应用范围最广的一类家务服务机器人，家庭机器人种类繁多，主要包括扫地机器人、拖地机器人、窗户清洁机器人等等，根据场景不同，价格也存在一定差距，基本在几百元到几千元的价格范围内，扫地机器人一直是家庭清洁机器人的最主要组成部分，占比达到96%以上。7.2 市场背景图7.1 2012到2018年全球家务机器人销量（万台）图7.2 2012到2018全球家务机器人销售额(亿美元)7.3 市场的行业分析扫地机器人进入终端用户后产品属性转为大众消费品其目标用户具有两个明显的特质：（1）具备一定经济偿付能力，市场扫地机器人，大约在1000到1500元区间范围内，而市场上普通的吸尘器的价格大约在200到400元左右，对于普通家庭用户来讲，目前的扫地。机器人在智能化的溢价方面仍然较高，所以我们从淘宝指数平台上的数据可以看出，目前国内扫地机器人用户占比中广东，浙江，江苏，山东，上海等东部沿海发达地区的人均占比最高，上海，北京，深圳，广州，杭州等一线城市人群占比较高。（2）用户普遍年轻化，都市繁忙的生活节奏让最简单的扫地成为生活累赘，扫地机器人的需求痛点在于解放繁忙的年轻都市人群，用智能化的方式解放双手，追求更高品质的生活质量。 7.4 市场的预测扫地机器人在国内兴起的时间大概是2010年左右，从百度搜索指数显示2010年后关于扫地机器人的搜索关注度呈现一路上升的趋势，这与国内扫地机器人产业化相关，国内扫地机器人先驱开始涉足这块业务也主要是在2010年前后，随着市场需求培育和产品不断更新迭代，扫地机器人在中专的关注度，逐渐升温，扫地机器人针对的目标用户是所有家庭产品的需求痛点，是为了解放繁忙的现代人，解放双手，用智能化的机器人代替人。目前我们所研发的清洁机器人主要是面对年轻的群体，当清洁机器人技术成熟以后，将会推广至所有人。项目心得本文根据毕业设计开题报告书所布置的任务，制定了详细的设计方案，并按照此方案逐步完成了电路原理图的设计以及软件程序的设计。本文的重点是基于stm32微处理器为核心，添加其他外围电路为辅助，并且加载必要的程序设计，使小车智能化行动。整个智能小车系统以stm32微处理器为核心，外围电路包括避障电路、循迹电路、电机驱动电路等，这些外围电路通过stm32微处理器结合起来，使得各个模块在保证工作准确性的同时，提高了小车的智能化。在这期间，我在所学的基础知识之上，去接触stm32，在接触之初，我不断的遇到问题，而我也知道不经一番寒彻骨，哪的梅香扑鼻来的道理，最终我克服了种种困难，不仅使自己的知识更加牢固，而且也锻炼了自己的心性。 参考文献：1 杜春雷.ARM体系结构与编程M.北京：清华大学出版社，2003-02-01 2 姚文详，宋岩.ARM Cortex-M3权威指南M.北京：北京航空航天大学出版 社，2009-07 3 范书瑞.Cortex-M3嵌入式处理器原理与应用M.北京：电子工业出版社， 2011-01-01 4李宁.基于MDK的STM32处理器开发应用M.北京：北京航空航天大学出版社，2008:1-260. 5 刘军，张洋.原子教你玩STM32M.北京：北京航空航天大学出版社，2013-05-01附录一 成本分析零件价格（元）总计（元）3个直流电机35*3105清扫部分100100接近开关2个20*240蓝牙模块1010黑白线循迹模块3个8*3242个轮胎及联轴器29*258万向轮2个10*220STM32系统板1717其他费用100100合计（元）474附录二 程序源代码1、手机蓝牙控制主要程序：main.c#include stm32f10x.h#include interface.h#include LCD1602.h#include IRCtrol.h#include motor.h#include uart.h#include gpio.h/全局变量定义unsigned int speed_count=0;/占空比计数器 50次一周期char front_left_speed_duty=SPEED_DUTY;char front_right_speed_duty=SPEED_DUTY;char behind_left_speed_duty=SPEED_DUTY;char behind_right_speed_duty=SPEED_DUTY;unsigned char tick_5ms = 0;/5ms计数器，作为主函数的基本周期unsigned char tick_1ms = 0;/1ms计数器，作为电机的基本计数器unsigned char tick_200ms = 0;/刷新显示char ctrl_comm = COMM_STOP;/控制指令unsigned char continue_time=0;unsigned char bt_rec_flag=0;/蓝牙控制标志位int main(void)delay_init();GPIOCLKInit();UserLEDInit();LCD1602Init();/IRCtrolInit();TIM2_Init();MotorInit();ServoInit();USART3Conf(9600);SystemInit();GPIOA_Init();GPIO_SetBits(GPIOA, GPIO_Pin_6);/设置指定的数据端口位GPIO_SetBits(GPIOA, GPIO_Pin_7);GPIO_SetBits(GPIOA, GPIO_Pin_8);GPIO_SetBits(GPIOA, GPIO_Pin_13);while(1) if(tick_5ms = 5)tick_5ms = 0;tick_200ms+;if(tick_200ms = 40)tick_200ms = 0;LEDToggle(LED_PIN);continue_time-;/200ms 无接收指令就停车if(continue_time = 0)continue_time = 1;CarStop();/do somethingif(bt_rec_flag = 1)/接收到蓝牙信号bt_rec_flag = 0;switch(ctrl_comm)case COMM_UP: CarGo();break;case COMM_DOWN: CarBack();break;case COMM_LEFT: CarLeft();break;case COMM_RIGHT: CarRight();break;case COMM_STOP: CarStop();break;default : break;LCD1602WriteCommand(ctrl_comm); motor.c#include motor.h#include interface.h#include stm32f10x.h/GPIO配置函数void MotorGPIO_Configuration(void)GPIO_InitTypeDef GPIO_InitStructure;GPIO_InitStructure.GPIO_Pin = FRONT_LEFT_F_PIN;GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(FRONT_LEFT_F_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = FRONT_LEFT_B_PIN;GPIO_Init(FRONT_LEFT_B_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = FRONT_RIGHT_F_PIN;GPIO_Init(FRONT_RIGHT_F_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = FRONT_RIGHT_B_PIN;GPIO_Init(FRONT_RIGHT_B_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = BEHIND_LEFT_F_PIN;GPIO_Init(BEHIND_LEFT_F_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = BEHIND_LEFT_B_PIN;GPIO_Init(BEHIND_LEFT_B_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = BEHIND_RIGHT_F_PIN;GPIO_Init(BEHIND_RIGHT_F_GPIO, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = BEHIND_RIGHT_B_PIN;GPIO_Init(BEHIND_RIGHT_B_GPIO, &GPIO_InitStructure); /根据占空比驱动电机转动void CarMove(void) /左前轮if(front_left_speed_duty 0)/向前if(speed_count front_left_speed_duty)FRONT_LEFT_GO;elseFRONT_LEFT_STOP;else if(front_left_speed_duty 0)/向后if(speed_count 0)/向前if(speed_count front_right_speed_duty)FRONT_RIGHT_GO;else /停止FRONT_RIGHT_STOP;else if(front_right_speed_duty 0)/向后if(speed_count 0)/向前if(speed_count behind_left_speed_duty)BEHIND_LEFT_GO;else /停止BEHIND_LEFT_STOP;else if(behind_left_speed_duty 0)/向后if(speed_count 0)/向前if(speed_count behind_right_speed_duty)BEHIND_RIGHT_GO;else /停止BEHIND_RIGHT_STOP;else if(behind_right_speed_duty 0)/向后if(speed_count = 5)tick_5ms = 0;tick_200ms+;if(tick_200ms = 40)tick_200ms = 0;LEDToggle(LED_PIN);SearchRun();if(ctrl_comm_last != ctrl_comm)/指令发生变化ctrl_comm_last = ctrl_comm;switch(ctrl_comm)case COMM_UP: CarGo();break;case