小蓝xlanll

这个屌丝很懒，什么也没留下！

热门标签

机器人制作开源方案 | 全自动导航分拣机器人_基于plc的机器人视觉分拣系统的国内外研究现状

作者：小蓝xlanll | 2024-05-31 04:05:10

踩

基于plc的机器人视觉分拣系统的国内外研究现状

作者：孙国峰董阳张鑫源

单位：山东科技大学机械电子工程学院

指导老师：张永超贝广霞

1. 研究意义

1.1 研究背景

在工业生产中，机器人在解决企业的劳动力不足，提高企业劳动生产率，提高产品质量和降低生产成本方面有着显著的意义。随着计算机自动化技术和社会的发展，智能机器人的研究和应用成为社会关注的热点，而且目前已在社会各个领域中被广泛使用。在物流产业高速发展的今天,机器人技术的应用程度已成为企业之间竞争和未来发展的重要衡量因素，同时以自动化物流系统作业流程为背景的各种机器人竞赛也在企业和教育部门联合举办下竞相开展，因此设计并制作一台基于视觉系统的智能分拣机器人具有重大的意义。

1.2 研究应用场景

在工业机器人应用中，智能分拣机器人是应用率最高的机器人之一，尤其是在工业制造、港口码头、仓储物流、医药食品等领域应用广泛。从工业生产机器人的工作内容来看，大部分工作内容包括搬运、分拣等基本任务。而且无论机器人用于哪个领域，一般都会涉及搬运和分拣工作，而要进行报搬运、分拣等，就需要借助视觉效果进行判别，也就是机器视觉。

在思考机器视觉时，必须分为两种情况进行分析。第一个是相机静止与整体目标静止，第二个是相机静上与整体目标运动状态。当机器视觉在精确定位上不准确时，会导致搬运和分拣工作受到伤害，甚至无法工作。在机器人的分拣技术中，机器视觉的创造是其顺利完成分拣工作的基础。对机器人机器视觉的科学研究可以辅助机器人顺利、快速地工作。同时提高了工作效率，促进了社会经济各领域的发展。

1.3 国内外研究现状

目前从我国机器人产业的研究来看，我国已经研制出各种类型的机器人，但是对机器人的整体研究还处于起步阶段，对机器人机器视觉技术的研究尚未更深层次开展，关于机器人的分拣系统建设，中国的研究人员也提出了一些可行的算法，例如贝叶斯估计跟踪方法及其目标识别方法等，该算法的明确提出可以在一定程度上辅助机器人的运输和分拣，在精确的算法下机器人分拣工作将更加顺利、准确地进行。

从20世纪50年代初，国外就展开智能循迹移动机器人的研究，并从90年代开始，智能移动机器人的研究就进入了系统化、大规模的研究阶段。尤其突出的是美国卡内基-梅陇大学机器人研究所已经完成Navlab系列的自主移动机器人的研究，这一研究成果代表了国外智能移动机器人的主要研究方向。

几经迭代，国际上的机器人形态也越发多样性，从最初的工业机器人形态衍生出各种形态机器人，或根据运用场景不同，研发侧重点也不一样，比如搬运机器人，消防机器人、配送机器人、医疗机器人、教育机器人等，也可能因为场景不同，最终形态截然不同如：仿人机器人、仿真机器人、AI 机器人，软体机器人等。

2. 总体思路

2.1 总述

本项目主要目的与任务为制作一台可以将运动机构与抓取机构相互分离的智能机器人，两者之间通过无线方式进行通讯，以此种方式将其应用于更多的场景中。

2.2 底盘思路

对于机器人底盘思路，为了提高机器人的处理能力和性能，小车采用四轮驱动，其优点是有足够的动力，可承载车载硬件，使小车在更短的时间内到达目的地。底盘最重要的功能是运动，目前使用的机械运动结构主要有仿生足式、履带式和轮式等。

从实现复杂度和可靠性方面考虑，轮式结构具有结构简单、容易实现、操控方便和成本低等优点。轮式结构又分为四轮驱动、双轮差分驱动+随动轮、全向驱动等，根据实际使用效果和功能分析，我们团队选定四轮全向驱动方式进行移动底盘设计。其优点如下：

① 四轮全向结构通过分别控制4个轮子的转速和方向，可以驱动底盘向任意方向平移和旋转，底盘运动方向和速度调整控制算法简单，容易实现。

② 全向轮受地面影响较小，前后平移距离和旋转角度控制比较准确，运动路线不容易偏移。

③ 全向轮结构简单，对加工精度要求不高，相对于另一种常用的麦克纳姆轮来说，成本较低。

对于智能分拣机器人的驱动，选择4个带有编码器的直流电机，编码器将信号或数据进行编制、转换为可用以通讯、传输和存储的信号形式，在这里编码器就是能够将电机的转动信息（比如转速、转动角度等）转换为脉冲信号的设备。利用具有编码器的直流电机，能让机器人高速运转，采用编码电机控制电机精准转速转向，搭配PID算法，对智能分拣系统的底盘运动就可以更加精确的控制，减少误差运动的积累，提高运动位置的准确性。

为了保证机器人运动的准确性，就不能使运动结构出现歪曲的情况，为此我们选择MPU六轴陀螺仪来实现与电机的微闭环系统，通过对陀螺仪解算的偏航角的读取，并对其进行卡尔曼滤波，可以较好地得到智能车偏离直线的角度，为防止误操作与抖动，我们对其设置了一个较小地活动范围，经过测试，搭配有陀螺仪的底盘行走更加稳定，同时解决了因全向轮打滑而产生的精度误差。

2.3 避障思路

为了使得机器人运动时防止与其他运动或静止的物体相碰撞，以免发生损坏产生损失，我们采用激光雷达搭配超声波模块进行避障。

激光雷达是一种用于获取精确位置信息的传感器，犹如人类的眼睛，可以确定物体的位置、大小等，由发射系统、接收系统及信息处理三部分组成。其工作原理是向目标探测物发送探测信号（激光束），然后将目标发射回来的信号（目标回波）与发射信号进行比较，进行适当处理后，便可获取目标的相关信息，例如，目标距离、方位、高度、速度、姿态、甚至形状等参数，从而对目标进行探测、跟踪和识别。

激光雷达是激光主动探测传感器设备的一种统称。对于测量成像激光雷达，其主要工作原理是通过高频测距和扫描测角实现对目标轮廓的三维扫描测量（成像）。测量成像激光雷达主要包括测量型LiDAR和导航型LiDAR两个大类。测量型LiDAR主要用于高精度测图；导航型LiDAR主要用于智能车、机器人和飞行设备的导航避障。两类产品的测量原理和技术基础相近，主要区别在于作业方式、探测距离和测量精度的差异。激光雷达具有以下的优点：

① 分辨率高

激光雷达工作于光学波段，频率比微波高 2～3 个数量级以上，因此与微波雷达相比，激光雷达具有极高的距离分辨率、角分辨率和速度分辨率。

② 抗干扰能力强

激光波长短，可发射发散角非常小的激光束，多路径效应小（不会形成定向发射，与微波或者毫米波产生多路径效应），可探测低空或超低空目标。

③ 不受光线影响

激光雷达可全天候进行侦测任务。它只需发射自己的激光束，通过探测发射激光束的回波信号来获取目标信息。

④ 体积小、质量轻

通常普通微波雷达的体积庞大，整套系统质量数以吨记，光天线口径就达几米甚至几十米。而激光雷达就要轻便、灵巧得多,外观只有厘米级。而且激光雷达的结构相对简单，维修方便，操纵更容易。

如果让机器人识别物体、行走、避障等，那首先需要为机器人提供激光雷达技术，帮助机器人进行地图绘制确定自身定位及感知周边环境，以及对周边的物体进行定位。譬如扫地机器人，扫地机器人是目前激光雷达应用最广泛的领域，激光雷达配合SLAM算法，可以让扫地机器人在房间里实现智能清扫，清扫的过程中进行地图绘制，实现传输到手机中，就算用户不在家，也可通过手机查看清扫情况，以及安排其它地方清扫。

本项目使用激光雷达主要是利用其精确的测距功能，在此基础之上，结合八个超声波的简单矫正测距，构成机器人运动机构的精确定位与避障系统。

2.4 上、下位机通讯思路

为了严格控制单片机内部运行的时序，我们在机器人的主体运动机构上采用上、下位机通讯的方式，起初采用的是串口通讯，经过一段时间的调试，我们发现，经过串口通讯所传递的数据存在错误较多的现象，这严重的导致了机器人的避障误判，为此我们更换思路，采用 IIC 通讯方式，经过代码撰写与调试，我们发现，这种通讯方式下，数据的正确度较高，同时对数据处理也更加方便。IIC通讯具有以下优点：

① 简单性和有效性

由于接口直接在组件之上，因此IIC总线占用的空间非常小，减少了电路板的空间和芯片管脚的数量，降低了互联成本。总线的长度可高达25英尺，并且能够以10Kbps的最大传输速率支持40个组件。

② 多主控

其中任何能够进行发送和接收的设备都可以成为主总线。一个主控能够控制信号的传输和时钟频率。当然在任何时间点上只能有一个主控。

2.5 机械臂思路

机械臂作为机器人的执行机构，其精度也是非常重要的，机器人的机械臀由6个伺服电机和机械爪组成，舵机由控制信号PWM波形占空比来控制。为了最大限度地控制和提高舵机响应速度，一方面可通过对机械的控制方式进行实现，利用对舵机的控制信号输出转矩余量，使舵机的转角速度和占空比变大，加快舵机的响应速度；另一方面则主要是通过控制对舵机的控制频率信号和增加PWM波形的占空比频率来提高对舵机的响应速度。因为舵机所转的响应角度大小是由PWM波形的占空比直接控制的，且其转动响应速度的大小是由舵机电压和舵机性能所决定的。

舵机构成了机械臂的主要结构，通过修改末端执行机构可以实现不同操作，且所以结构都可以通过3D打印而成，节约成本。

2.6 图像识别思路

随着近几年机器视觉的高速发展，图像识别也成为许多人研究的对象，本团队视觉系统使用OpenMv模块进行图像识别构成机器人的“眼睛”。机器视觉就是用机器代替人眼来做测量和判断。机器视觉系统是指通过机器视觉产品将被摄取目标转换成图像信号，传送给专用的图像处理系统，得到被摄取目标的形态信息，根据像素分布和亮度、颜色等信息，转变成数字化信号；图像系统对这些信号进行各种运算来抽取目标的特征，进而根据判别结果来控制现场的设备动作。

机器视觉系统可以快速获取大量信息，而且易于自动处理，也易于同设计信息以及加工控制信息集成，因此在现代自动化生产过程中，人们将机器视觉系统广泛地用于工况监视、成品检验和质量控制等领域。机器视觉系统的特点是提高生产的柔性和自动化程度。在一些不适合于人工作业的危险工作环境或人工视觉难以满足要求的场合，常用机器视觉来替代人工视觉；同时在大批量工业生产过程中，用人工视觉检查产品质量效率低且精度不高，用机器视觉检测方法可以大大提高生产效率和生产的自动化程度。而且机器视觉易于实现信息集成，是实现计算机集成制造的基础技术。

以Arduino Mega 2560单片机为核心，集成OV7725摄像头芯片，在小巧的硬件模块上，用C语言高效地实现了核心机器视觉算法，提供Python编程接口，可以用Python语言使用OpenMV提供的机器视觉功能，为自己的产品和发明增加有特色的竞争力，使用OpenMV来进行检测并训练模型，将其作为语音播报的基础。

3. 主要创新点

3.1 自主导航性

随着人工成本的不断升高，用机器人代替人力去做一些重复性的高强度的劳动是现代机器人研究的一个重要方向。目前在物流系统和柔性制造系统中，自动导航小车被广泛的应用，但其主要的引导方式是电磁或者惯性引导，电磁引导需要埋设金属线，并加载引导频率，其缺点明显，灵活性差，改变或扩充路径较麻烦，对引导线路附近的铁磁物质有干扰。而惯性引导主要安装陀螺仪，缺点是成本较高维护保养等后续问题较难解决，地面也需要磁性块作辅助定位。

本团队设计的智能分拣机器人与上述作品相比较，通过内部算法设计实现底盘驱动系统的自主导航功能，搭配使用思岚激光雷达传感器，进行精确的同步定位与简单的地图构建，解决分拣机器人在未知环境中运行时定位导航与地图构建的问题，这可以简单的获取场地的二维平面图，从而达到赋予机器人“眼睛”的功能，使分拣机器人在未知场地具有地图模型，结合算法程序判断路迹轨迹，得出自主导航路径。

结合八个超声波的简单测距，使机器人与两侧墙体保持一定距离，避免出现碰撞墙壁的风险。同时为保证机器人运动的准确性，避免运动结构出现歪曲的情况，选择MPU六轴陀螺仪来实现与电机的微闭环系统，通过对陀螺仪解算的偏航角的读取，并对其进行卡尔曼滤波，得到智能车偏离直线的角度，从而进行调正。使用算法设计并搭配激光雷达、超声波传感器、MPU六轴陀螺仪等模块相互配合来实现机器人运动机构的精确自主导航与避障系统。

3.2 基于视觉系统的智能分拣

随着图像识别系统的高速发展，团队采用OpenMV视觉系统模块，对目标物体进行扫描并提取信息，摄取目标转换图像信号，进行图像信息处理，获取目标的形状、尺寸、大小、颜色等信息的视觉特征，利用程序算法进行信息判断，利用ESP8266无线通信模块进行无线信息通讯，控制6舵机机械臂和夹爪抓取相应物体进行智能分拣任务，具有提高智能分拣任务效率，节约时间，解放劳动力的优势。

本项目采取Open MV机器视觉的方法，完成视觉机械臂寻找特定物体并精确抓取的目的, 解决了传统传感器不能识别分辨相似物体的难题，运用Python语言编程帮助摄像头分辨不同形状,颜色的物体,寻找并追踪特定物体,进而实现"寻物，移动，避障，抓取，分拣"一体化的方案。实验结果显示，智能取物小车可实现根据摄像头捕捉到周围环境自动处理，并通过串口发送指令，Arduino板将接收到的指令解析，控制电机和机械臂采取相应动作,即响应信号后小车实现自行移动，到达目标位置并抓取物体，从而实现智能寻物与分拣。

3.3 多种信息交流方式

团队设计的机器人将车体运动平台与机械臂分拣系统相互分离，两者之间通过无线传输模块进行信息交流，机械臂可以根据运输平台的特殊需求分拣不同的货物，两者相互搭配，提高了工作的效率，节约了时间成本，并且在这种设计情况下，可以将此模式应用于更多的场景，更好的完成服务任务。

在车体运输机构中，为了保证时序的要求，采用两个单片机，一个用来进行信息的处理与判断完成大部分任务，另一个用来获取当前的数据信息，两者之间我们进行了不同的实验，首先进行串口通讯，在多次测试的情况下，我们发现通过串口通讯所获取的数据时常存在错值，这经常会导致机器人的误操作，为了解决这一问题，我们改用 IIC 通讯方式，通过以下的模式设置，完成了信息交流的通讯配置。

进行数据传送时，在SCL为高电平期间，SDA线上电平必须保持稳定，只有SCL为低时，才允许SDA线上电平改变状态。且每个字节传送时都是高位在前；对于应答信号，ACK=0时为有效应答位，说明从机已经成功接收到该字节，若为1则说明接受不成功；如果从机需要延迟下一个数据字节开始传送的时间，可以通过把SCL电平拉低并保持来强制主机进入等待状态。

主机完成一次通信后还想继续占用总线在进行一次通信，而又不释放总线，就要利用重启动信号。它既作为前一次数据传输的结束，又作为后一次传输的开始；总线冲突时，按“低电平优先”的仲裁原则，把总线判给在数据线上先发送低电平的主器件；在特殊情况下，若需禁止所有发生在 I2C 总线上的通信，可采用封锁或关闭总线，具体操作为在总线上的任一器件将 SCL 锁定在低电平即可；SDA 仲裁和 SCL 时钟同步处理过程没有先后关系，而是同时进行的。通过测试，我们发现，通过这种方式进行的数据传输，几乎无错值的出现，也不会导致机器人的误操作。

3.4 算法实现机器人自动测距闭环跟墙

机器人采用Dijkstra最短路径算法，自动规划最短路径。具有自主避障能力，遇到障碍物可根据目标路径自主修改局部路径，并更新最短路径。

迪科斯彻算法使用了广度优先搜索解决赋权有向图或者无向图的单源最短路径问题，算法最终得到一个最短路径树。该算法常用于路由算法或者作为其他图算法的一个子模块。Dijkstra算法采用的是一种贪心的策略，声明一个数组dis来保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点的集合：T，初始时刻，原点s的路径权重被赋为0（dis[s] = 0）。若对于顶点s存在能直接到达的边（s,m），则把dis[m]设为w（s, m）,同时把所有其他（s 不能直接到达的）顶点的路径长度设为无穷大。

初始时，集合T只有顶点s。然后从dis 数组选择最小值，则该值就是源点s到该值对应的顶点的最短路径，并且把该点加入到T中，OK，此时完成一个顶点，我们需要看看新加入的顶点是否可以到达其他顶点并且看看通过该顶点到达其他点的路径长度是否比源点直接到达短，如果是，那么就替换这些顶点在dis中的值。最后又从dis中找出最小值，重复上述动作，直到T中包含了图的所有顶点。通过Dijkstra最短路径算法，我们完成了路径中的自动跟墙操作，并借此寻找最短路径来提高效率。

4. 关键技术

4.1 直流电机PID算法闭环控制技术

为了更好的进行机器人的运动，选择了4个霍尔编码器直流电机作为驱动元件，利用霍尔传感器进行检测电机的转速、转动角度等信息，转换为脉冲信号，传输到控制器中，得到转速和转角数据。利用具有编码器的直流电机，转速高、转角精确，能控制电机精准转速转向，同时搭配PID算法，进行负反馈调节，对智能分拣系统的底盘运动就可以更加精确的控制，减少误差运动的积累，提高运动位置的准确性。

4.2 控制器上、下位机通讯技术

为了控制单片机内部运行的时序和提高控制器处理速度、提高运行效率，采用主、从控制器式的上、下位机通讯的方式，对原先采用的是串口通讯，进行改进，采用IIC通讯方式，避免了原先经过串口通讯所传递的数据存在错误较多而导致了机器人的避障误判现象，该通讯方式下，数据的正确度较高，控制较精确，对数据处理也更加方便。

4.3 基于激光雷达的同步定位与地图构建技术

使用激光雷达传感器，进行获取工作场景的环境信息，对场地存在的障碍物进行测距，判断方位，利用激光雷达构建出二维平面地图，对整体环境有了精确的了解，搭配程序算法对可行的路径轨迹进行判断得出轨迹路线，完成自主导航的轨迹规划工作。

4.4 基于视觉系统的智能分拣机械臂

由OpenMV视觉系统模块获得的信息图像进行处理，得出判断结果，通过ESP8266无线通信模块进行无线信息通讯，控制六舵机机械臂和夹爪抓取相应物体进行智能分拣任务。采用这种分拣方式，将机器视觉与机械臂相融合，可以完成更多功能的任务，根据不同用户的不同需求，达到服务大众的目的。

4.5 语音播报

为了更好的体现机器人的各项技术和便于机器人管理，我们在机器人中加入语音播报功能，通过语音播报我们可以知道目前机器人的工作状态，并且可以判断机器人有无发生故障。

5. 发展前景

5.1 项目可行性

在我国的工、农生产和现代服务行业中，物体的智能抓取分拣和自主导航应用广泛，能极大地解放劳动力，降低生产成本，提高效率。在工业生产的智能物流搬运中，采用无人自主导航技术可以减少人员数量，提高工作的安全性和准确性，按照安全的轨迹路线到达指定位置，对不同的物流信息进行视觉识别判断，达到物流分区分拣的目的。同时在餐饮服务业、垃圾智能分拣分类、农业果实分拣采摘等等方向均能广泛应用。根据市场调查及预测的结果，以及有关的产业政策等因素，我们发现这种机器人很受市场的欢迎，它可以解放劳动力，实现自动化的任务，以此论证项目投资建设的必要性。

在项目的技术角度上，通过计算机建模与简单的仿真，我们团队已经完成了此项目的大部分工作，我们将整体项目拆分为多个小型项目，逐步完成，最后进行总调。

从资源配置的角度来衡量此项目的价值，此项目可以有效配置经济资源，具有增加供应、创造就业、提高人民生活等方面的效益。在经济可行性方面，本项目的整体花费低于市场平均水平，可以降低成本，提高经济效益。

从社会可行性方面，国家已经发布了相关的政策法规，鼓励服务机器人行业的发展。《中共中央关于制定国民经济和社会发展第十四个五年规划和二〇三五年远景目标的建议》提出，强化国家战略科技力量。制定科技强国行动纲要，健全社会主义市场经济条件下新型举国体制，打好关键核心技术攻坚战，提高创新链整体效能。

5.2 项目推广前景

5.2.1 物流行业

物流行业是一个备受大家关注和重视的行业，尤其是现在的社会，物流成为人们接触较为普遍的行业，社会对物流服务的需求量也因此增大。面对现在的市场，物流企业要想生存下去，就必须要赢得客户的认可，也要有能力缓解市场需求增加带来的压力。智能化转型成为物流企业势在必行的做法，只有这样，物流企业才能更好的解决现有的难题。物流机器人作为智能转运设备，可有效解决物流企业多种难题。相对传统的人工搬运模式，物流机器人的转运会更加的出色，表现出的优势如下：

① 无人工操作，快速提升搬运效率。物流机器人的运行可以免除人工的参与，让整个的搬运和分配变得更快捷，平时人工一天需要转运的货物，在物流机器人这里只需要一小时就完成，效率大大提升。

② 处理人工无法搬运的工作任务。有些搬运工作并不适合人工参与，比如一些狭小空间的搬运，或者是危险区域的搬运等。选择物流机器人的搬运，可以替代人工。

③ 降低货物转运成本。成本是每个物流企业所关心的话题，如果做不好成本控制，那么物流企业也会被拖垮，因为竞争对手太多了。有了物流机器人的参与，那么人工成本就节省下来了，效率也提升了。

目前社会在不断的发展，人们对物流服务的需求量和品质也在逐步提升。对于物流企业，要改变现状，就必须要转型自身的运营方式，由传统的人工转运转变成为智能化设备的转运。物流机器人的应用也成为众多物流企业转型的工具之一。

综上，我们可以看到物流机器人的发展前景是非常好的。因为随着社会对物流服务需求量的逐渐上升，物流业必须要提升自己的处理货物转运的能力，才能更好的服务大众。物流机器人的应用，恰好解决了物流企业这些难题，让企业有能力去处理更多的货物转运，既赢得了客户的认可，也提升了市场竞争力。

5.2.2 餐饮行业

目前智能服务机器人迎来蓬勃发展，在服务机器人中出现一匹“黑马”，就是餐厅机器人，餐厅机器人中不仅有送餐机器人还有炒菜机器人、迎宾机器人等。这些机器人与餐厅的融合，让餐饮行业另辟新径。虽然目前餐厅机器人形势一片大好，但依然存在很多技术上的不足，除了产品功能上的缺陷，制约餐厅机器人发展的最大原因就是其价格，目前市面上的送餐机器人的零售价格都在6万元左右，而开设一家机器人餐厅至少也需要三台送餐机器人，机器人的费用加上铺设磁轨的费用将近20万元左右，对于一些中小型的餐厅而言，这笔金额仍然是一笔不小的开支。虽然目前餐厅机器人形势一片大好，但业内专家依然认为价格太贵是送餐机器人普及化的“拦路虎”，降低成本则成为大势所趋。当未来有一天餐厅机器人的价格可以从6万元降到6千元，那个时候就会使送餐机器人真正实现大批量量产，才能使送餐机器人得到真正的普及。

6. 示例程序

① newInOne.ino


#include <PID_v1.h>
 
#include <L298N_MotorDriver.h>
 
#include <FlexiTimer2.h>
 
#include<Wire.h>
 
//播放器数据区
 
bool playerWorked;
 
enum playTo{
 
  vegetable,
 
    meat,
 
  fruits,
 
  drinks
 
};
 
//mpu 数据
 
int YPR[3]; //记录三个轴的角度
 
byte acceptableDist = 5;
 
unsigned char Re_buf[8], counter = 0;
 
unsigned char sign = 0;
 
byte mpuDegrees = 5; //代表mpu的阈值，也就是小车进行调整的最小度数 last:5
 
//移动方向控制
 
enum
 
{
 
    go_stop,      //停车
 
    go_Straight,   //前行
 
    go_Back,      //后退
 
    go_Left,      //左行
 
    go_Right,     //右行
 
    right_Around, //右转（原地旋转，一般用不到）
 
    left_Around   //左转（原地旋转，一般用不到）
 
};
 
//标定偏移方向,左旋和右旋为绝对旋转，即：俯视小车，小车的旋转方向
 
enum Offset_direction
 
{
 
    S_Left,    //前运动时，左旋运动
 
    S_Right,   //前运动时，右旋运动
 
    B_Left,    //后运动时，左旋运动
 
    B_Right,   //后运动时，右旋运动
 
    L_Left,    //左运动时，左旋运动
 
    L_Right,   //左运动时，右旋运动
 
    R_Left,    //右运动时，左旋运动
 
    R_Right,   //右运动时，右旋运动
 
    no_Offset, //无偏速度
 
};
 
byte Offset_direction_Max_times = 5; //标记在多少次偏移后更改偏移方向，防止小车原地旋转
 
//移动路线图
 
byte move_Next_To[8][8] = {
 
    {255, 255, 255, 11, 3, 255, 255, 255},
 
    {255, 255, 255, 19, 4, 255, 255, 255},
 
    {255, 18, 17, 18, 12, 255, 255, 255},
 
    {255, 255, 255, 255, 20, 255, 255, 255},
 
    {255, 34, 35, 36, 28, 255, 255, 255},
 
    {255, 33, 255, 255, 255, 255, 255, 255},
 
    {255, 41, 58, 50, 51, 52, 53, 54},
 
    {255, 49, 57, 255, 255, 255, 255, 55}};
 
//记录该节点的四个方向的联通状态
 
byte is_Connected[8][8] = {
 
    {4, 5, 3, 6, 7, 7, 7, 3},
 
    {2, 3, 8, 14, 11, 12, 15, 9},
 
    {14, 7, 7, 15, 11, 6, 15, 3},
 
    {12, 13, 13, 13, 11, 14, 15, 11},
 
    {6, 7, 7, 7, 11, 14, 15, 11},
 
    {14, 11, 12, 13, 9, 8, 8, 8},
 
    {14, 11, 6, 7, 7, 7, 7, 3},
 
    {12, 13, 13, 9, 12, 13, 13, 9}};
 
byte x = 0, y = 0; //当前的小车的位置
 
byte sum = 4;            //传感器的数量
 
double distance[4]={75,25,175,25};      //定义距离变量
 
float distanceWall = 25; //和墙壁保持的标准距离
 
byte move_To[] = {go_Straight, go_Left, go_Back, go_Left, go_Straight, go_Right, go_Straight, go_Left, go_Back, go_Left, go_Right, go_Straight}; //路径选择
 
byte move_Dis[] = {1, 5, 1, 1, 3, 3, 4, 1, 2, 2, 2, 2};                                                                                          //路径所走的路程
 
byte move_Size = sizeof(move_To) / sizeof(byte);                                                                                                 //表示一共有多少个选择
 
//四个多级的输入输出状态
 
// input 输入值录入
 
// pid 计算输出值
 
// setpoint 设定目标
 
double standard_speed = 6;
 
double input_0 = 0, output_0 = 70, setpoint_0 = standard_speed;
 
double input_1 = 0, output_1 = 70, setpoint_1 = standard_speed;
 
double input_2 = 0, output_2 = 70, setpoint_2 = standard_speed;
 
double input_3 = 0, output_3 = 70, setpoint_3 = standard_speed;
 
//pid参数
 
double kp = 9.94, ki = 10.28, kd = 0.00;
 
//记录上一次和这一次的计算值
 
unsigned long last_count_0 = 0, now_count_0 = 0;
 
unsigned long last_count_1 = 0, now_count_1 = 0;
 
unsigned long last_count_2 = 0, now_count_2 = 0;
 
unsigned long last_count_3 = 0, now_count_3 = 0;
 
//四个pid对象
 
PID myPID_0(&input_0, &output_0, &setpoint_0, kp, ki, kd, DIRECT);
 
PID myPID_1(&input_1, &output_1, &setpoint_1, kp, ki, kd, DIRECT);
 
PID myPID_2(&input_2, &output_2, &setpoint_2, kp, ki, kd, DIRECT);
 
PID myPID_3(&input_3, &output_3, &setpoint_3, kp, ki, kd, DIRECT);
 
//四个电机对象
 
L298N_MotorDriver motor_0(5, 28, 29); //左前方电机
 
L298N_MotorDriver motor_1(6, 22, 23); //右前方电机
 
L298N_MotorDriver motor_2(7, 24, 25); //右后方电机
 
L298N_MotorDriver motor_3(4, 26, 27); //左后方电机
 
void setup()
 
{
 
    Wire.begin();                // join i2c bus with address #8
 
    Serial.begin(9600);    //设定波特率
 
    initAudioPlayer(9600);
 
    //Setup the mpu
 
    initMpu6050(); //需要等待两秒钟
 
    myPID_0.SetMode(AUTOMATIC);
 
    myPID_1.SetMode(AUTOMATIC);
 
    myPID_2.SetMode(AUTOMATIC);
 
    myPID_3.SetMode(AUTOMATIC);
 
    pinMode(53,OUTPUT);
 
    digitalWrite(53,LOW);
 
    attachInterrupt(0, count_0, RISING); //pin 2
 
    attachInterrupt(1, count_1, RISING); //pin 3
 
    attachInterrupt(4, count_2, RISING); //pin 19
 
    attachInterrupt(5, count_3, RISING); //pin 18
 
    //电机设定速度
 
    motor_0.setSpeed(125);
 
    motor_1.setSpeed(125);
 
    motor_2.setSpeed(125);
 
    motor_3.setSpeed(125);
 
    //设定方向
 
    setDrection(go_Straight);
 
    //电机使能
 
    motor_0.enable();
 
    motor_1.enable();
 
    motor_2.enable();
 
    motor_3.enable();
 
    FlexiTimer2::set(10, update);
 
    FlexiTimer2::start();
 
}
 
void loop()
 
{
 
    // move_test_Time();
 
    //    move_With_No_Search();
 
    move_With_UpdatePath();
 
    // move_test_Impulse();
 
//    print();
 
    // mpuTest();
 
    // Serial.print("\n");
 
    Serial_Receive();
 
    // Serial_test_disValue();
 
    // Serial.print(" \n");
 
    // delay(40);
 
   speedControlerMpu6050(go_Straight);
 
    speedControlerTOF(go_Straight ,distanceWall);
 
    // Serial_Test_SpeedValue();
 
    // move_Witn_No_Search();
 
    // move_test_Impulse();
 
    // setDrection(go_Left);
 
}
 
/*******************运动速度以及方向控制处理函数*********************/
 
/** @brief 速度速度控制函数，来调节小车的加减速运动，防止小车出现打滑和撞墙现象，此必须成对的使用
* @param speed 正整数，小车速度的改变量，当大于5时直接返回，无任何操作
* @param addOrSubtract 设定小车的加或者减。
*      @arg true 速度加
*      @arg false 速度减
*/
 
void setSpeed(byte speed,bool addOrSubtract){
 
    // if(speed>5)return;
 
    if(addOrSubtract){   //速度加
 
        setpoint_0+=speed;
 
        setpoint_1+=speed;
 
        setpoint_2+=speed;
 
        setpoint_3+=speed;
 
        standard_speed+=speed;
 
    }else{
 
        setpoint_0-=speed;
 
        setpoint_1-=speed;
 
        setpoint_2-=speed;
 
        setpoint_3-=speed;
 
        standard_speed-=speed;
 
    }
 
}
 
/**
* @brief 方向控制函数
* @param dir 目标方向
*/
 
void setDrection(byte dir)
 
{
 
    switch (dir)
 
    {
 
    case go_Straight:
 
        motor_0.setDirection(true);
 
        motor_2.setDirection(false);
 
        motor_1.setDirection(false);
 
        motor_3.setDirection(true);
 
        break;
 
    case go_Back:
 
        motor_0.setDirection(false);
 
        motor_2.setDirection(true);
 
        motor_1.setDirection(true);
 
        motor_3.setDirection(false);
 
        break;
 
    case go_Left:
 
        motor_0.setDirection(false);
 
        motor_2.setDirection(true);
 
        motor_1.setDirection(false);
 
        motor_3.setDirection(true);
 
        break;
 
    case go_Right:
 
        motor_0.setDirection(true);
 
        motor_2.setDirection(false);
 
        motor_1.setDirection(true);
 
        motor_3.setDirection(false);
 
        break;
 
    case right_Around:
 
        motor_0.setDirection(true);
 
        motor_2.setDirection(true);
 
        motor_1.setDirection(true);
 
        motor_3.setDirection(true);
 
        break;
 
    case left_Around:
 
        motor_0.setDirection(false);
 
        motor_2.setDirection(false);
 
        motor_1.setDirection(false);
 
        motor_3.setDirection(false);
 
        break;
 
    default:
 
        break;
 
    }
 
    if (!motor_0.isEnabled())
 
    {
 
        //电机使能
 
        motor_0.enable();
 
        motor_1.enable();
 
        motor_2.enable();
 
        motor_3.enable();
 
    }
 
}
 
/**
* @brief 速度变量值映射方案
* @param value 原始影射输入参数
* @param mpuOrToF 判断是使用哪一种方案
*   @arg true mpuOrTOF方案
*   @arg false mpu方案
*@return 处理过的映射速度变量
*/
 
byte valueMap(byte value,bool mpuOrTOF){
 
    if(mpuOrTOF){
 
         return value/2>4?4:(value/2);
 
    }
 
    else return value/4;
 
}
 
/**
* @brief 此函数用来控制小车在原有的基础上进行自身旋转，也就是速度叠加；
* @param dir 当前小车的方向，当dir=go_Stop指的是小车已经矫正，不必再次自旋。当dir=255时小车是在没有完成矫正的前提下形式方向发生改变引起的。
* @param turnLeftOrRight 小车的自旋方向，向左旋转为true,向右旋转为false
* @return none
*/
 
void selfSpin(byte dir, bool turnLeftOrRight)
 
{
 
    byte value = 2;                          //速度旋转的值
 
    static bool processing = false;          //当其为true时表示正在调整中不必调整
 
    static byte lastDir = go_Straight;       //记录上一次的移动方向
 
    static bool lastTurnLeftOrRight = false; //记录上一次的自旋方向
 
    if (lastDir != dir)
 
    {
 
        lastDir = dir;
 
        dir = 255;
 
    }
 
    if (dir == go_stop && processing == true)
 
    { //恢复原始状态
 
        processing = false;
 
        if ((lastDir == go_Straight && turnLeftOrRight == false) || (lastDir == go_Back && turnLeftOrRight == true))
 
        { //前行右边旋转和后退左旋转的速度变化是一样的
 
            setpoint_0 -= value;
 
            setpoint_1 += value;
 
            setpoint_2 += value;
 
            setpoint_3 -= value;
 
        }
 
        else if ((lastDir == go_Straight && turnLeftOrRight == true) || (lastDir == go_Back && turnLeftOrRight == false))
 
        { //前行左边旋转和后退右旋转的速度变化是一样的
 
            setpoint_0 += value;
 
            setpoint_1 -= value;
 
            setpoint_2 -= value;
 
            setpoint_3 += value;
 
        }
 
        else if ((lastDir == go_Left && turnLeftOrRight == false) || (lastDir == go_Left && turnLeftOrRight == true))
 
        { //左行右边旋转和右退左旋转的速度变化是一样的
 
            setpoint_0 += value;
 
            setpoint_1 += value;
 
            setpoint_2 -= value;
 
            setpoint_3 -= value;
 
        }
 
        else if ((lastDir == go_Left && turnLeftOrRight == true) || (lastDir == go_Left && turnLeftOrRight == false))
 
        { //左行左边旋转和右退右旋转的速度变化是一样的
 
            setpoint_0 -= value;
 
            setpoint_1 -= value;
 
            setpoint_2 += value;
 
            setpoint_3 += value;
 
        }
 
    }
 
    else if (processing == false)
 
    {
 
        if ((dir == go_Straight && turnLeftOrRight == false) || (dir == go_Back && turnLeftOrRight == true))
 
        { //前行右边旋转和后退左旋转的速度变化是一样的
 
            setpoint_0 += value;
 
            setpoint_1 -= value;
 
            setpoint_2 -= value;
 
            setpoint_3 += value;
 
        }
 
        else if ((dir == go_Straight && turnLeftOrRight == true) || (dir == go_Back && turnLeftOrRight == false))
 
        { //前行左边旋转和后退右旋转的速度变化是一样的
 
            setpoint_0 -= value;
 
            setpoint_1 += value;
 
            setpoint_2 += value;
 
            setpoint_3 -= value;
 
        }
 
        else if ((dir == go_Left && turnLeftOrRight == false) || (dir == go_Right && turnLeftOrRight == true))
 
        { //左行右边旋转和右退左旋转的速度变化是一样的
 
            setpoint_0 -= value;
 
            setpoint_1 -= value;
 
            setpoint_2 += value;
 
            setpoint_3 += value;
 
        }
 
        else if ((dir == go_Left && turnLeftOrRight == true) || (dir == go_Right && turnLeftOrRight == false))
 
        { //左行左边旋转和右退右旋转的速度变化是一样的
 
            setpoint_0 += value;
 
            setpoint_1 += value;
 
            setpoint_2 -= value;
 
            setpoint_3 -= value;
 
        }
 
        processing = true;
 
        lastDir = dir;
 
        lastTurnLeftOrRight = turnLeftOrRight;
        }
 
    else if (dir == 255 && processing == true)
 
    {
 
        processing = false; //此类情况适用于在没有调整好的情况下，发生了转弯运动，因此要将速度恢复到标准速度那里
 
        setpoint_0 = standard_speed;
 
        setpoint_1 = standard_speed;
 
        setpoint_2 = standard_speed;
 
        setpoint_3 = standard_speed;
 
    }
 
}
 
/**
* @brief 此函数用来通过mpu来控制小车的航向的矫正，因为小车在移动的过程中会出现小车自身发生旋转，因此引入mpu来进行方向的矫正。
* @param dir 当前小车的方向
* @return none
*/
 
void speedControlerMpu6050(byte dir)
 
{
 
    bool turnLeftOrRight = YPR[0] < 0 ? true : false; //判断小车的倾斜方向，向左边旋转为true,右边为负
 
    if (abs(YPR[0]) < mpuDegrees)
 
        selfSpin(go_stop, turnLeftOrRight);
 
    else
 
        selfSpin(dir, turnLeftOrRight);
 
    return;
 
}
 
/**
*   @brief 小车平移处理函数，小车的矫正分为两个部分，一部分为小车自身矫正航向，一部分来控制小车与墙壁的距离。本部分就是为了解决小车与墙壁的距离不合理而进行平移速度叠加运算。
*   @param dir 小车的当前于运行方向
*   @param left_right 选择以当前方向为参考，向左偏移还是向右边偏移
*       left：false
*       right: true
*   @param value 指定速度偏移量
*   @return none
*/
 
void selTranslation(byte dir, byte left_right, int value)
 
{
 
    static int lastValue=0;   //标记上一次的平移速度偏移量
 
    static byte lastDir = go_Straight;   //上一次的移动方向
 
    static bool lastModel = false;      //判断上一次是左还是右平移
 
    static bool processing = false; //当为true时说明正在平移调整，不需要再次进行调整了。
 
    if (lastDir != dir)     //如果发生了转弯仍然没有完成平移，那么就进行复位
 
    {
 
        lastDir = dir;
 
        dir = 255;          //将方向设置为255，将速度设定为标定速度，连带mpu的速度也进行了复位
 
    }
 
    if (dir == go_stop && processing)   //如果小车目前仍然在平移，并且受到命令stop，进行复位
 
    {
 
        if (lastModel == false)   //上一次是左移
 
        {
 
            setpoint_0 -= lastValue;
 
            setpoint_1 += lastValue;
 
            setpoint_2 -= lastValue;
 
            setpoint_3 += lastValue;
 
        }
 
        else if (lastModel == true) //上次是右移
 
        {
 
            setpoint_0 += lastValue;
 
            setpoint_1 -= lastValue;
 
            setpoint_2 += lastValue;
 
            setpoint_3 -= lastValue;
 
        }
 
        processing = false; //标记当前为非平移调整状态
 
        lastValue=0;
 
        return;
 
    }
 
    else if (dir != go_stop && dir != 255)
 
    {
 
        if(processing&&lastModel!=left_right)   //如果直接跳过了中键的非平移状态，进入到另一个平移状态，直接将此次作用后的状态标记为非平移处理状态
 
        {   lastModel=left_right;
 
            selTranslation(go_stop,false,value);
 
            selTranslation(dir,left_right,value);
 
            // Serial.print("mmmm:   ");
 
            // Serial.print(value);
 
            // Serial.print("   ");
 
            return;
 
        }
 
        else {                  //当前为处理状态并且当前的平移方向和上一次的相同
 
            processing = true;   //标记正在处理中
 
            byte temp=value;
 
            value-=lastValue;   //计算需要达到新的设定值的速度差
 
            lastValue=temp;    //记录新的lastValue值
 
            // Serial.print("value:   ");
 
            // Serial.print(value);
 
            // Serial.print("   ");
 
        }
 
        if ((dir == go_Straight && left_right == true) || (dir == go_Back && left_right == true) || (dir == go_Left && left_right == false) || (dir == go_Right && left_right == false))
 
        {
 
            setpoint_0 += value;
 
            setpoint_1 -= value;
 
            setpoint_2 += value;
 
            setpoint_3 -= value;
 
            lastModel = left_right;
 
        }
 
        else
 
        {
 
            setpoint_0 -= value;
 
            setpoint_1 += value;
 
            setpoint_2 -= value;
 
            setpoint_3 += value;
 
            lastModel = left_right;
 
        }
 
    }
 
    else if (dir == 255)
 
    {
 
        processing = false;
 
        lastValue=0;
 
        setpoint_0 = standard_speed;
 
        setpoint_1 = standard_speed;
 
        setpoint_2 = standard_speed;
 
        setpoint_3 = standard_speed;
 
    }
 
}
 
/**
*   @brief 速度控制函数，用来微调小车矫正位置。函数首先判断当前跟进的时哪一个墙壁，选择方式为当前运动方向的两侧最靠近墙壁的一边。由于本张地图中只有最后的听程序会出现小车左右都大于50cm的情况因此暂时不考虑两侧都大于50的情况
*    @param dir 当前小车的运动方向
*    @param dis 标准距离，即：小车与墙壁最终希望达到的距离
*/
 
void speedControlerTOF(byte dir, float dis)
 
{
 
    byte value; //速度偏移量
 
    float temp; //保存距离
 
    bool Left_or_Straight; //设定跟随的墙壁的方向，当为左侧墙壁或者前侧的墙壁的时候为true,否则为false
 
    if (distance[dir - 1] <11) //消除下位机没能及时传输入数据的问题
 
        return;
 
    if (dir <= 2)   //运动方向为前后运动
 
    {
 
        temp = min(distance[go_Left - 1], distance[go_Right - 1]);
 
        Left_or_Straight = temp == distance[go_Left - 1];
 
    }
 
    else if (dir <= 4)   //运动方向为左右运动
 
    {
 
        temp = min(distance[go_Straight - 1], distance[go_Back - 1]);
 
        Left_or_Straight = temp == distance[go_Straight - 1];
 
    }
 
    if (temp > 45) //为了解决小车如果当前运动方向的左右两侧的墙壁距离大于40的情况
 
        return;
 
    value=valueMap((byte)abs(temp-dis),true);   //获得速度偏移量
 
    // Serial.print(value);
 
    // Serial.print("   ");
 
    if (temp <= dis - 2)
 
    {
 
        if (Left_or_Straight)
 
        {
 
            if (dir == go_Back || dir == go_Left)
 
                selTranslation(dir, false,value);
 
            else
 
                selTranslation(dir, true,value);
 
            // Serial.print(11);
 
            // Serial.print("   ");
 
        }
 
        else
 
        {
 
            if (dir == go_Back || dir == go_Left)
 
                selTranslation(dir, true,value);
 
            else
 
                selTranslation(dir, false,value);
 
            // Serial.print(22);
 
            // Serial.print("   ");
 
        }
 
    }
 
    else if (temp >= dis + 2)
 
    {
 
        if (Left_or_Straight)
 
        {
 
            if (dir == go_Back || dir == go_Left)
 
                selTranslation(dir, true,value);
 
            else
 
                selTranslation(dir, false,value);
 
            // Serial.print(33);
 
            // Serial.print("   ");
 
        }
 
        else
 
        {
 
            if (dir == go_Back || dir == go_Left)
 
                selTranslation(dir, false,value);
 
            else
 
                selTranslation(dir, true,value);
 
            // Serial.print(44);
 
            // Serial.print("   ");
 
        }
 
    }
 
    else
 
        selTranslation(dir,true,0); //车子复位
 
}
 
/**
* @brief 急停函数,使用之后电机将取消使能。
* @param none
* @return none
* */
 
void urgency_Stop()
 
{
 
    motor_0.reverseDirection();
 
    motor_1.reverseDirection();
 
    motor_2.reverseDirection();
 
    motor_3.reverseDirection();
 
    unsigned long mil = millis();
 
    while (millis() - mil < 100)
 
        ;
 
    setDrection(go_stop);
 
}
 
void stopForOpenmv(){
 
    byte temp = standard_speed;
 
    setSpeed(temp, false);    //开始阻塞
 
    Serial2.println("ok!"); //发送请求信息
 
    bool ok = false;    //表明是否受到信息
 
    // while (1){          //验证是否接收到正确的信息
 
    //     long mil = millis();
 
    //     while(millis()-mil<200){
 
    //         if(Serial2.available()!=0){
 
    //             String str = Serial2.readStringUntil('\n');
 
    //             if(str=="ok"){
 
    //                 ok = true;
 
    //                 break;
 
    //             }
 
    //         }
 
    //     }
 
    //     if(ok)
 
    //         break;
 
    // }
 
    // ok = false;
 
    long delaytimes = millis();
 
    while(millis()-delaytimes<3000)
 
        Serial.println("Waiting...");
 
    // while(!playerWorked)    //接听消息
 
        // SerialListener();
 
    // setPlay(meat);   //饮料
 
    // setPlay(vegetable); //肉
 
    setPlay(fruits);    //蔬菜
 
    setPlay(drinks);    //水果
 
    setPlay(drinks);    //水果
 
    setSpeed(temp, true); //取消阻塞
 
    Serial.println("Start...");
 
}
 
/********************************************************/
 
/*******************中断处理处理函数*********************/
 
//外部中断计数函数
 
void count_0() { now_count_0++; }
 
void count_1() { now_count_1++; }
 
void count_2() { now_count_2++; }
 
void count_3() { now_count_3++; }
 
/**
* @brief 使用定时器进行更新当前的状态，然后进行计算控制器的输出值并设定当前的速度，另外还要对当前的串口状态进行监听，不断更新当前的距离状态
* */
 
void update()
 
{
 
    input_0 = now_count_0 - last_count_0;
 
    last_count_0 = now_count_0;
 
    input_1 = now_count_1 - last_count_1;
 
    last_count_1 = now_count_1;
 
    input_2 = now_count_2 - last_count_2;
 
    last_count_2 = now_count_2;
 
    input_3 = now_count_3 - last_count_3;
 
    last_count_3 = now_count_3;
 
    //计算pid的输出量
 
    myPID_0.Compute();
 
    myPID_1.Compute();
 
    myPID_2.Compute();
 
    myPID_3.Compute();
 
    //更新速度
 
    motor_0.setSpeed((byte)output_0);
 
    motor_1.setSpeed((byte)output_1);
 
    motor_2.setSpeed((byte)output_2);
 
    motor_3.setSpeed((byte)output_3);
 
    //更新航向角偏向
 
    mpuUpload();
 
}
 
/**************************************************************/
 
/*********************路径感知以及更新规划函数*******************/
 
/**
* @brief 判断下一步的移动方向
* @param my_X 小车当前的X坐标（矩阵的纵坐标）
* @param my_Y 小车当前的Y坐标（矩阵的横坐标）
* @return dir 返回下一步的移动方向
*/
 
byte nextDir(byte my_X, byte my_Y)
 
{
 
    byte next_X = (byte)(move_Next_To[my_X][my_Y] / 8);
 
    byte next_Y = (byte)(move_Next_To[my_X][my_Y] % 8);
 
    if (next_X == my_X)
 
    {
 
        if (next_Y < my_Y)
 
            return go_Left;
 
        else if (next_Y > my_Y)
 
            return go_Right;
 
        else
 
            return go_stop;
 
    }
 
    else if (next_Y == my_Y)
 
    {
 
        if (next_X < my_X)
 
            return go_Straight;
 
        else if (next_X > my_X)
 
            return go_Back;
 
        else
 
            return go_stop;
 
    }
 
    else
 
        return go_stop;
 
}
 
/**
* @brief 判断指定单元的指定方向是否是相通的
* @param my_X 当前的X坐标（矩阵的纵坐标）
* @param my_Y 当前的Y坐标（矩阵的横坐标）
* @param dir 需要判断的方向
*/
 
bool is_Connected_inDir(byte my_X, byte my_Y, byte dir)
 
{
 
    byte temp = is_Connected[my_X][my_Y];
      byte value = 0;
 
    switch (dir)
 
    {
 
    case go_Straight:
 
        value = ((temp & (1 << 3)) >> 3);
 
        break;
 
    case go_Back:
 
        value = ((temp & (1 << 1)) >> 1);
 
        break;
 
    case go_Left:
 
        value = ((temp & (1 << 0)) >> 0);
 
        break;
 
    case go_Right:
 
        value = ((temp & (1 << 2)) >> 2);
 
        break;
 
    default:
 
        break;
 
    }
 
    if (value == 1)
 
        return true;
 
    return false;
 
}
 
void update_Path(byte my_X, byte my_Y)
 
{
 
    byte next_X = (byte)(move_Next_To[my_X][my_Y] / 8);
 
    byte next_Y = (byte)(move_Next_To[my_X][my_Y] % 8);
 
    byte next_Value = move_Next_To[next_X][next_Y]; //随机障碍物的下一个目标的值
 
    int now = move_Next_To[my_X][my_Y] - my_X * 8 - my_Y;
 
    int next = next_Value - move_Next_To[my_X][my_Y];
 
    if (now == next)
 
    { //如果当前的方向和障碍物所在处应有的方向是一样的
 
        if (now == 1 || now == -1)
 
        {
 
            if (is_Connected_inDir(my_X, my_Y, go_Straight) && is_Connected_inDir(next_X, next_Y, go_Straight))
 
            {
 
                move_Next_To[next_X - 1][next_Y] = move_Next_To[my_X][my_Y];
 
                move_Next_To[my_X][my_Y] -= (8 + now);
 
                move_Next_To[my_X - 1][my_Y] = move_Next_To[my_X][my_Y] + now;
 
            }
 
            else if (is_Connected_inDir(my_X, my_Y, go_Back) && is_Connected_inDir(next_X, next_Y, go_Back))
 
            {
 
                move_Next_To[next_X + 1][next_Y] = move_Next_To[my_X][my_Y];
 
                move_Next_To[my_X][my_Y] += (8 - now);
 
                move_Next_To[my_X + 1][my_Y] = move_Next_To[my_X][my_Y] + now;
 
            }
 
        }
 
        else if (now == 8 || now == -8)
 
        {
 
            if (is_Connected_inDir(my_X, my_Y, go_Left) && is_Connected_inDir(next_X, next_Y, go_Left))
 
            {
 
                move_Next_To[next_X][next_Y - 1] = move_Next_To[my_X][my_Y];
 
                move_Next_To[my_X][my_Y] -= (1 + now);
 
                move_Next_To[my_X][my_Y - 1] = move_Next_To[my_X][my_Y] + now;
 
            }
 
            else if (is_Connected_inDir(my_X, my_Y, go_Right) && is_Connected_inDir(next_X, next_Y, go_Right))
 
            {
 
                move_Next_To[next_X][next_Y + 1] = move_Next_To[my_X][my_Y];
 
                move_Next_To[my_X][my_Y] += (1 - now);
 
                move_Next_To[my_X][my_Y + 1] = move_Next_To[my_X][my_Y] + now;
 
            }
 
        }
 
    }
 
    else
 
    { //如果不一样
 
        byte my_Dir = nextDir(my_X, my_Y);
 
        byte next_Dir = nextDir(next_X, next_Y);
 
        if (my_Dir == go_Straight || my_Dir == go_Back)
 
        {
 
            if (next_Dir == go_Right)
 
            {
 
                move_Next_To[my_X][my_Y + 1] = next_Value;
 
                move_Next_To[my_X][my_Y] = my_X * 8 + my_Y + 1;
 
            }
 
            else if (next_Dir == go_Left)
 
            {
 
                move_Next_To[my_X][my_Y - 1] = next_Value;
 
                move_Next_To[my_X][my_Y] = my_X * 8 + my_Y - 1;
 
            }
 
        }
 
        if (my_Dir == go_Left || my_Dir == go_Right)
 
        {
 
            if (next_Dir == go_Straight)
 
            {
 
                move_Next_To[my_X - 1][my_Y] = next_Value;
 
                move_Next_To[my_X][my_Y] = my_X * 8 + my_Y - 8;
 
            }
 
            else if (next_Dir == go_Back)
 
            {
 
                move_Next_To[my_X + 1][my_Y] = next_Value;
 
                move_Next_To[my_X][my_Y] = my_X * 8 + my_Y + 8;
 
            }
 
        }
 
    }
 
}
 
/**
* @brief 用来调整小车在拐弯的时候与墙壁的距离，如果没有达到标准距离便进行调整。
* @param dir 当前的运动方向
*/
 
void repair(byte dir)
 
{
 
    digitalWrite(53,HIGH);
 
    Serial_Receive();
 
    setDrection(dir);
 
    setSpeed(2,false);
 
    while (distance[dir - 1] > 25){
 
        Serial_Receive();
 
        serialEvent2();
 
        speedControlerTOF(dir, distanceWall);
 
        speedControlerMpu6050(dir);
 
    }
 
    setSpeed(2,true);
 
    urgency_Stop();
 
    now_count_0 = now_count_1 = now_count_2 = now_count_3 = 0;
 
    last_count_0 = last_count_1 = last_count_2 = last_count_3 = 0;
 
    digitalWrite(53,LOW);
 
}
 
/**
* @brief 更新路径，当机械臂完成任务之后返回停止区
* @param Dx 停车的位置,可选值1，2，3表示停到D1，D2，D3
*/
 
void reWrite_Path(byte Dx)
 
{
 
    move_Next_To[2][1] = 18;
 
    move_Next_To[2][2] = 19;
 
    move_Next_To[2][3] = 20;
 
    move_Next_To[2][4] = 12;
 
    move_Next_To[1][4] = 4;
 
    move_Next_To[0][4] = 5;
 
    move_Next_To[0][5] = 13;
 
    move_Next_To[1][5] = 14;
 
    move_Next_To[1][6] = 22;
 
    switch (Dx)
 
    {
 
    case 1:
 
        move_Next_To[2][6] = 21;
 
        move_Next_To[2][5] = 29;
 
        move_Next_To[3][5] = 37;
 
        move_Next_To[4][5] = 45;
 
        move_Next_To[5][5] = 45;
 
        break;
 
    case 2:
 
        move_Next_To[2][6] = 30;
 
        move_Next_To[3][6] = 38;
 
        move_Next_To[4][6] = 46;
 
        move_Next_To[5][6] = 46;
 
        break;
 
    case 3:
 
        move_Next_To[2][6] = 23;
 
        move_Next_To[2][7] = 31;
 
        move_Next_To[3][7] = 39;
 
        move_Next_To[4][7] = 47;
 
        move_Next_To[5][7] = 47;
 
        break;
 
    default:
 
        break;
 
    }
 
}
 
/**************************************************************/
 
/**********************测试函数********************************/
 
/**
* @brief 移动过程中不断更新局部路径，具有避障的功能
* */
 
void move_With_UpdatePath()
 
{
 
    byte my_X = 7, my_Y = 7;
 
    // byte my_X = 0, my_Y = 3;
 
    byte dir, temp=255;
 
    while (1)
 
    {
 
        dir = nextDir(my_X, my_Y);
 
        Serial.println(dir);
 
        Serial_Receive();
 
        if (distance[dir - 1] < 50)
 
        {
 
            repair(dir);
 
            urgency_Stop();
 
            update_Path(my_X, my_Y);
 
            dir = nextDir(my_X, my_Y);
 
        }
 
        // Serial.println(dir);
 
        setDrection(dir);
 
        digitalWrite(53,LOW);
 
        while (is_Move_To_StandardDis() == 0)
 
        {
 
            serialEvent2();
 
            Serial_Receive();
 
            speedControlerTOF(dir, distanceWall);
 
            speedControlerMpu6050(dir);
 
        }
 
        // digitalWrite(53,HIGH);
 
        while(is_Move_To_StandardDis() == 1){
 
            serialEvent2();
 
            Serial_Receive();
 
            speedControlerTOF(dir, distanceWall);
 
            speedControlerMpu6050(dir);
 
        }
 
        byte last_X=my_X;
 
        byte last_Y=my_Y;
 
         
 
        temp = move_Next_To[my_X][my_Y];    //获取下一次的目标地址
 
        my_X = temp / 8;
 
        my_Y = temp % 8;
 
        if(my_X==0&&my_Y==3)
 
            stopForOpenmv();
 
        Serial.print("X:   ");
 
        Serial.print(my_X);
 
        Serial.print("\tY:   ");
 
        Serial.print(my_Y);
 
        Serial.print("\tvalue:\t");
 
        Serial.println(!is_Connected_inDir(last_X, last_Y, dir));
 
        if(!is_Connected_inDir(my_X,my_Y,dir))
 
            repair(dir);
 
        if(my_X==2&&my_Y==1){
 
            digitalWrite(53,HIGH);
 
            urgency_Stop();
 
            urgency_Stop();
 
            byte sv=standard_speed;
 
            setSpeed(sv, false);
 
            long mil = millis();
 
            while (millis()-mil<9000)
 
            {
 
               
 
            }
 
            setSpeed(sv, true);
 
            reWrite_Path(1);
 
            digitalWrite(53,LOW);
 
        }
 
           
 
    }
 
}
 
/**
* @brief 直接移动不需要进行搜索
*/
 
void move_With_No_Search()
 
{
 
    byte move_ways = 0; //目前已经移动的距离
 
    for (int i = 0; i < move_Size; i++)
 
    {
 
        move_ways = 0;
 
        setDrection(move_To[i]);
 
        while (move_ways < move_Dis[i])
 
        {
 
            speedControlerTOF(move_To[i], distanceWall);
 
            speedControlerMpu6050(move_To[i]);
 
            serialEvent2();
 
            Serial_Receive();
 
            move_ways += is_Move_To_StandardDis();
 
        }
 
        repair(move_To[i]);
 
    }
 
}
 
/**
* @brief 是否移动到标准距离（50cm）
* @param none
* @return
*      @arg 0 小于25厘米
*      @arg 1 大于50厘米
*      @arg 2 到达50厘米
*
*/
 
byte is_Move_To_StandardDis()
 
{
 
    int ave;
 
    ave = (int)(now_count_0 + now_count_1 + now_count_2 + now_count_3) / 4;
 
    if (ave >= 756)
 
    { //757是指行走50cm产生的脉冲数
 
        now_count_0 = now_count_1 = now_count_2 = now_count_3 = 0;
 
        last_count_0 = last_count_1 = last_count_2 = last_count_3 = 0;
 
        return 2;
 
    }else if(ave >=378) return 1;
 
    return 0;
 
}
 
/**
* @brief 移动测试函数(定时运动)
*/
 
void move_test_Time()
 
{
 
    unsigned long now_millis;
 
    for (byte i = 1; i <= 4; i++)
 
    {
 
        now_millis = millis();
 
        setDrection(i);
 
        while (millis() - now_millis < 5000)
 
        {
 
            serialEvent2();
 
            mpuTest();
 
            Serial.print("\n");
 
            speedControlerMpu6050(i);
 
        }
 
    }
 
}
 
/**
* @brief 移动测试函数(定脉冲运动)
*/
 
void move_test_Impulse()
 
{
 
    int ave;
 
    for (byte i = 1; i <= 4; i++)
 
    {
 
        while (1)
 
        {
 
            ave = (int)(now_count_0 + now_count_1 + now_count_2 + now_count_3) / 4;
 
            if (ave >= 757)
 
            {
 
                now_count_0 = now_count_1 = now_count_2 = now_count_3 = 0;
 
                last_count_0 = last_count_1 = last_count_2 = last_count_3 = 0;
 
                break;
 
            }
 
            print();
 
        }
 
        urgency_Stop();
 
        setDrection(i);
 
    }
 
}
 
/**
* @brief 传感器测试函数,输出的是当前四个方向的距离
*/
 
void Serial_test_disValue()
 
{
 
    String str = "";
 
    for (int i = 0; i < sum; i++)
 
    {
 
        str += distance[i];
 
        str += ";";
 
    }
 
    Serial.print(str);
 
}
 
/**
* @brief 传感器测试函数，输出的是当前四个方向的距离
*/
 
void Serial_Test_SpeedValue()
 
{
 
    String str = "";
 
    str += setpoint_0;
 
    str += " ";
 
    str += setpoint_1;
 
    str += " ";
 
    str += setpoint_2;
 
    str += " ";
 
    str += setpoint_3;
 
    Serial.println(str);
 
}
 
/**
* @brief 打印速度的输入量
*/
 
void print()
 
{
 
    Serial.print("$");
 
    Serial.print(input_0);
 
    Serial.print(" ");
 
    Serial.print(input_1);
 
    Serial.print(" ");
 
    Serial.print(input_2);
 
    Serial.print(" ");
 
    Serial.print(input_3);
 
    Serial.print(";");
 
}
 
/**
* @brief mpu输入数据测试程序
* @param none
* @return none
*/
 
void mpuTest()
 
{
 
    Serial.print("mpu:\t");
 
    Serial.print(YPR[0]);     
  }
 
/*******************字符串处理函数*********************/
 
/**
* @brief 处理下位机的输入字符串信息
* @param str 要处理的字符串
* @return none
*/
 
void String_Dispose(String str)
 
{
 
    int index = 0;
 
    int Start = 0;
 
    for (int i = 0; i < sum; i++)
 
    {
 
        index = str.indexOf(';', Start);
 
        if (index <= 0)
 
            break;
 
        distance[i] = str.substring(Start, index).toFloat();
 
        Start = index + 1;
 
    }
 
}
 
/**
*   @brief 上位机字符串接收函数
* */
 
void Serial_Receive()
 
{
 
    Wire.requestFrom(8, 17);
 
    // Serial.println(Serial3.available());
 
    String temp="";
 
    char c;
 
    int i=0;
 
    while(Wire.available())
 
    {
 
        // String str = "";
 
        // str = Serial3.readStringUntil('\n');
 
        //         Serial.println(str);
 
        // if (str.length() > 5)
 
        // {
 
        //     String_Dispose(str);
 
        //     Serial3.flush();
 
        // }
 
        c=Wire.read();
 
        if(c==';'){
 
            distance[i++]=temp.toInt();
 
            temp="";
 
        }else if(c=='\n'){
 
            distance[i++]=temp.toInt();
 
            temp="";
 
            i=0;
 
        }else if(c<='9'&&c>='0')
 
            temp+=c;
 
    }
 
}
 
/****************************************************/
 
/*******************mpu控制模块*********************/
 
/**
*   @brief 初始化mpu,使用Serial3，延时两秒钟
*   @return none
*/
 
void initMpu6050()
 
{
 
    Serial2.begin(115200);
 
    unsigned long mil = millis();
 
    while (millis() - mil < 2000) //两秒钟
 
        ;
 
    Serial2.write(0XA5);
 
    Serial2.write(0X52); //初始化GY25,连续输出模式
 
}
 
/**
*   @brief mpu数据处理函数，每10ms处理一次,程序在中断中调用
*   @return none
*/
 
void mpuUpload()
 
{
 
    static int k = 360;
 
    sign = 0;
 
    if (Re_buf[0] == 0xAA && Re_buf[7] == 0x55) //检查帧头，帧尾
 
    {
 
        YPR[0] = (Re_buf[1] << 8 | Re_buf[2]) / 100; //合成数据，去掉小数点后2位
 
        if (k == 360)
 
            k = YPR[0];
 
        else
 
        {
 
            YPR[0] -= k;
 
            if (YPR[0] <= -180)
 
                YPR[0] += 360;
 
            else if (YPR[0] >= 180)
 
                YPR[0] -= 360;
 
        }
 
        //        Serial.println(YPR[0]);
 
    }
 
}
 
/**
*   @brief 串口3中断处理函数
*   @return none
*/
 
void serialEvent2()
 
{
 
    while (Serial2.available())
 
    {
 
        Re_buf[counter] = (unsigned char)Serial2.read();
 
        if (counter == 0 && Re_buf[0] != 0xAA)
 
            return; // 检查帧头
 
        counter++;
 
        if (counter == 8) //接收到数据
 
        {
 
            counter = 0; //重新赋值，准备下一帧数据的接收
 
            sign = 1;
 
        }
 
    }
 
}
 
/*****************************************************/
 
/*****************播放器设置******************/
 
/**
* @brief 初始化播放引脚
* @param null
* @return null
*/
 
void initAudioPlayer(int bo)
 
{
 
  Serial2.begin(bo);
 
  pinMode(41, OUTPUT);
 
  pinMode(42, OUTPUT);
 
  pinMode(43, OUTPUT);
 
  pinMode(44, OUTPUT);
 
   digitalWrite(41, HIGH);
 
   digitalWrite(42, HIGH);
 
   digitalWrite(43, HIGH);
 
   digitalWrite(44, HIGH);
 
  playerWorked=false;
 
}
 
/**
* @brief 播放设置，选择播放的音频
* @param order 任务类型的编号
* @return null
*/
 
void setPlay(int order){
 
  switch (order)
 
  {
 
  case meat:
 
    digitalWrite(41,LOW);
 
    break;
 
  case vegetable:
 
    digitalWrite(42,LOW);
 
    break;
 
  case fruits:
 
    digitalWrite(43,LOW);
 
    break;
 
  case drinks:
 
    digitalWrite(44,LOW);
 
    break;
 
 
 
  default:
 
    break;
 
  }
 
//   playerWorked=true;
 
}
 
/**
* @brief 串口监听器，监听来自openmv的数据，并完成播放
* @param null
* @return null
*/
 
void SerialListener(){
 
//   if(Serial2.available()){
 
//     String str=Serial2.readStringUntil('\n');
 
//     Serial.println(str);
 
//     int num=str.toInt();
 
//     if(num<4&&num>=0){
 
// //       playerWorked=true;
 
//       setPlay(num);
 
// //      delay(300);
 
// //       AudioPlayerReset();
 
//     }
 
//   }
 
setPlay(meat);
 
}

② UNO_Servo.ino


/*
Name:    UNO_从机控制程序.ino
Created: 2021/4/3 9:21:17
Author:   BuLaduo
*/
 
#include <Servo.h>
 
Servo m_Servo[5];
 
//Servo
 
char angle_return[100];
 
const int frequency = 25;
 
const int speedd = 35;
 
const int actionTimes = 2000;
 
int servo_port[5] = { 22,23,24,25,26 }; //定义机械臂各舵机引脚，基座至机械爪
 
float servo_angle[5] = { 167, 105, 0, 0, 64}; //定义机械臂各舵机初始角度
 
const int servo_num = sizeof(servo_port) / sizeof(servo_port[0]); //定义舵机数量
 
String data = "123";    //任务数据
 
String Color = "312"; //颜色顺序数据
 
int temp[3]={0,2,1 };        //临时指令
 
int flag_all = 0;   //全局标志位
 
void setup() {
 
    Serial.begin(9600);
 
    pinMode(5, OUTPUT);
 
     pinMode(6, OUTPUT);
 
     pinMode(9, OUTPUT);
 
     pinMode(10, OUTPUT);
 
     digitalWrite(5, LOW);
 
     digitalWrite(6, LOW);
 
     digitalWrite(9, LOW);
 
     digitalWrite(10, LOW);
 
    ServoInit();
 
    delay(1000);
 
}
 
void Serialexam() {
 
    Serial.print("OK\n");
 
    delay(20);
 
    while(1){
 
    delay(500);
 
    while (!Serial.available());
 
    String temp=Serial.readStringUntil('\n');
 
    if(temp=="OK")break;
 
    }  
 
}
 
/*********************+舵机控制函数+*******************/
 
/*
  舵机初始化函数
*/
 
void ServoInit() {
 
    for (int i = 0; i < servo_num; i++) {
 
        m_Servo[i].attach(servo_port[i]);
 
        ServoGo(i, servo_angle[i]);
 
    }
 
}
 
/*
    将字符串转化为可执行的数组
*/
 
void OrderSet(String str) {
 
    int n = str.toInt();
 
    temp[0] = n / 100 % 10-1;
 
    temp[1] = n / 10 % 10-1;
 
    temp[2] = n % 10-1;
 
}
 
/*
  舵机角度最大值及最小值检测函数
  小于最小值, 返回最小值
  大于最大值, 返回最大值
*/
 
float ValueCompare(float value) {
 
    if (value <= 0)
 
        return 0;
 
    else if (value >= 180)
 
        return 180;
 
    else
 
        return value;
 
}
 
void ServoGo(int which, float angle) {
 
    m_Servo[which].write(ValueCompare(angle));
 
}
 
void ServoMove(float* pCurrent, float* pTarget) {
 
    float diff[servo_num];
 
    for (int i = 0; i < servo_num; i++)
 
        diff[i] = (*(pTarget + i) - *(pCurrent + i)) == 0 ? 0 : (*(pTarget + i) - *(pCurrent + i)) / frequency;
 
    for (int i = 0; i < frequency; i++) {
 
        for (int j = 0; j < servo_num; j++) {
 
            *(pCurrent + j) += diff[j];
 
            ServoGo(j, *(pCurrent + j));
 
        }
 
        delay(speedd);
 
    }
 
}
 
void ServoSet(int count, ...) {
 
    float targetAngle[count];
 
    va_list ap;
 
    va_start(ap, count);
 
    for (int i = 0; i < count; i++)
 
        targetAngle[i] = va_arg(ap, int);
 
    va_end(ap);
 
    ServoMove(servo_angle, targetAngle);
 
    delay(1000);
 
}
 
/*********************+舵机控制函数+*******************/
 
/*********************+拿放控制函数+*******************/
 
void GetFn(int* pd) {
 
    switch (*pd) {
 
    case 0:
 
        //put 0
 
       // ServoSet(servo_num, 100,123,123,60,64);
 
       // ServoSet(servo_num, 76,123,123,60,64);
 
        ServoSet(servo_num, 167,100,32,15,64);
 
       
 
        ServoSet(servo_num, 167,160,70,15,64);
 
        digitalWrite(9, HIGH);
 
        digitalWrite(10, LOW);
 
        digitalWrite(5, LOW);
 
        digitalWrite(6, LOW);
 
        ServoSet(servo_num, 167,160,70,41,64);
 
        ServoSet(servo_num, 167,160,70,41,90);
 
        ServoSet(servo_num, 167, 105, 0, 0, 64);
 
//        Serial.println("aaaaaa");
 
        digitalWrite(9, HIGH);
 
        digitalWrite(10, LOW);
 
        while(1);
 
        Serialexam();
 
       
 
//        ServoSet(servo_num, 167,116,167,58,64);
 
//        ServoSet(servo_num, 167,116,167,58,90);
 
//        ServoSet(servo_num, 167,123,123,60,90);
 
        break;
 
    case 1:
 
        //put 1
 
       // ServoSet(servo_num, 100,123,123,60,64);
 
        ServoSet(servo_num, 95,116,168,60,64);
 
        ServoSet(servo_num, 95,116,168,60,90);
 
        ServoSet(servo_num, 95,123,123,60,90);
 
        break;
 
    case 2:
 
        //put 2
 
           
 
       // ServoSet(servo_num, 100,123,123,60,64);
 
       // ServoSet(servo_num, 126,123,123,60,64);
 
        ServoSet(servo_num, 120,116,167,58,64);
 
        ServoSet(servo_num, 120,116,167,58,90);
 
        ServoSet(servo_num, 120,123,123,60,90);
 
       // ServoSet(servo_num, 100,123,123,60,90);
 
        break;
 
    default : break;
 
    }
 
}
 
//   二维码检测舵机控制
 
void QRDetect() {
 
    ServoSet(servo_num, 167, 105, 0, 0, 64);//移动摄像头至二维码屏幕
 
    ServoSet(servo_num, 108, 105, 0, 0, 64);
 
    Serialexam();
 
    ServoSet(servo_num, 167, 105, 0, 0, 64); //机械臂复位
 
}
 
/*
    进行转换，
    得到二维码检测的值，
    和颜色识别得到的颜色后，
    获得舵机指令操作集
*/
 
void get_orders(String serialData) {
 
    for (int i = 0; i < 3; i++)
 
        for (int j = 0; j < 3; j++)
 
            if (data[i] == serialData[j])
 
                temp[i] = j;
 
}
 
void Get_1() {
 
    /*********************************第一次拿取*******************************/
 
    //根据抓取顺序进行物料抓取
 
    for (int i = 0; i < 3; i++) {
 
        delay(1000);
 
       // Serial.print(temp[i]);
 
        switch (temp[i]) {
 
        case 0:
 
            //get 0
 
            //ServoSet(servo_num, 108,86,32,15,99);
 
            ServoSet(servo_num, 78,100,32,15,99);
 
            ServoSet(servo_num, 78,160,70,15,99);
 
            ServoSet(servo_num, 78,160,70,41,99);
 
            ServoSet(servo_num, 78,160,70,41,64);
 
            ServoSet(servo_num, 78,100,70,41,64);
 
            break;
 
        case 1:
 
            //get 1 红
 
            ServoSet(servo_num, 103,100,32,15,99);
 
            ServoSet(servo_num, 103,162,46,5,99);
 
            ServoSet(servo_num, 103,162,46,18,99);
 
            ServoSet(servo_num, 103,162,46,18,64);
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
            break;
 
        case 2:
 
            //get 2
 
            ServoSet(servo_num, 125,100,32,15,99);
 
            ServoSet(servo_num, 125,165,70,15,99);
 
            ServoSet(servo_num, 125,165,70,41,99);
 
            ServoSet(servo_num, 125,165,70,41,64);
 
            //ServoSet(servo_num, 127,86,32,15,64);
 
           // ServoSet(servo_num, 108,86,32,15,64);
 
            break;
 
            default : break;
 
        }
 
//        Serialexam();
 
        GetFn(&temp[i]);
 
        delay(1000);
 
    }
 
    Serialexam();
 
}
 
//物料放置时夹取物块机械臂移动函数
 
void putt(int i) {
 
  Serial.print(i);
 
    if (flag_all == 0) {    //放置到粗加工区
 
        switch (i)
 
        {
 
        case 0:
 
            //放到最左边
 
           // ServoSet(servo_num, 108,86,32,15,64);
 
            ServoSet(servo_num, 103,162,48,15,64);
 
            ServoSet(servo_num, 103,162,48,15,99);
 
            ServoSet(servo_num, 103,86,32,8,99);
 
           // ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        case 1:
 
            //放到最中间
 
           // ServoSet(servo_num, 108,86,32,15,64);
 
           // ServoSet(servo_num, 84,86,32,15,64);
 
            ServoSet(servo_num, 80,166,79,43,64);
 
            ServoSet(servo_num, 80,166,79,43,99);
 
            ServoSet(servo_num, 80,86,32,15,99);
 
            //ServoSet(servo_num, 84,86,32,15,99);
 
           // ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        case 2:
 
            //放到最前
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
            //ServoSet(servo_num, 127,86,32,15,64);
 
            ServoSet(servo_num, 124,166,79,45,64);
 
            ServoSet(servo_num, 124,166,79,45,99);
 
            ServoSet(servo_num, 124,86,32,15,99);
 
            //ServoSet(servo_num, 125,86,32,15,99);
 
           // ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        }
 
    }
 
    else if (flag_all == 1) {   //放置到半成品区
 
        switch (i)
 
        {
 
        case 0:
 
            //放到红
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
            ServoSet(servo_num, 103,144,27,12,64);
 
            ServoSet(servo_num, 103,144,27,12,99);
 
            ServoSet(servo_num, 103,86,32,0,99);
 
            //ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        case 1:
 
            //放到最中间
 
           // ServoSet(servo_num, 108,86,32,15,64);
 
            //ServoSet(servo_num, 85,86,32,15,64);
 
            ServoSet(servo_num, 80,153,56,31,64);
 
            ServoSet(servo_num, 80,153,56,31,99);
 
            ServoSet(servo_num, 80,86,32,15,99);
 
            //ServoSet(servo_num, 85,86,32,15,99);
 
           // ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        case 2:
 
            //放到蓝
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
           // ServoSet(servo_num, 127,86,32,15,64);
 
            ServoSet(servo_num, 124,158,65,32,64);
 
            ServoSet(servo_num, 124,158,65,32,99);
 
            ServoSet(servo_num, 124,86,32,15,99);
 
            //ServoSet(servo_num, 127,86,32,15,99);
 
            //ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        }
 
    }
 
    else if (flag_all == 2) {   //码垛
 
        switch (i)
 
        {
 
       case 0:
 
            //放到最左边
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
            ServoSet(servo_num, 102,123,14,20,64);
 
            ServoSet(servo_num, 102,123,14,20,99);
 
            ServoSet(servo_num, 102,86,32,12,99);
 
            break;
 
        case 1:
 
            //放到最中间
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
            //ServoSet(servo_num, 85,86,32,15,64);
 
            ServoSet(servo_num, 80,130,37,33,64);
 
            ServoSet(servo_num, 80,130,37,33,99);
 
            ServoSet(servo_num, 80,86,32,15,99);
 
            //ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        case 2:
 
            //放到最右边
 
            //ServoSet(servo_num, 108,86,32,15,64);
 
           // ServoSet(servo_num, 126,86,32,15,64);
 
            ServoSet(servo_num, 123,130,37,32,64);
 
            ServoSet(servo_num, 123,130,37,32,99);
 
            ServoSet(servo_num, 123,86,32,15,99);
 
            //ServoSet(servo_num, 108,86,32,15,99);
 
            break;
 
        }
 
    }
 
}
 
//从车上往下夹取
 
void Put_1() {
 
    OrderSet(data);
 
    delay(1000);
 
    for (int i = 0; i < 3; i++) {
 
        switch (temp[i]) {
 
        case 0:
 
              //车中中间
 
            ServoSet(servo_num, 95,123,123,60,92);
 
            ServoSet(servo_num, 95,115,173,60,92);
 
            ServoSet(servo_num, 95,115,173,60,64);
 
           // ServoSet(servo_num, 100,123,123,60,64);
 
            break;
 
        case 1:
                       //车头的
 
            //ServoSet(servo_num, 100,123,123,60,90);
 
            ServoSet(servo_num, 71,123,123,60,93);
 
            ServoSet(servo_num, 71,114,173,60,93);
 
            ServoSet(servo_num, 71,114,173,60,64);
 
            //ServoSet(servo_num, 76,123,123,60,64);
 
            //ServoSet(servo_num, 100,123,123,60,64);
 
            break;
 
        case 2:
 
            //最后面的
 
           // ServoSet(servo_num, 100,123,123,60,90);
 
            ServoSet(servo_num, 120,123,123,60,93);
 
            ServoSet(servo_num, 120,114,173,61,93);
 
            ServoSet(servo_num, 120,114,173,61,64);
 
           // ServoSet(servo_num, 127,123,123,60,64);
 
           // ServoSet(servo_num, 100,123,123,60,64);
 
            break;   
 
        }
 
        if (i == 0)Serialexam();
 
        putt(temp[i]);
 
        delay(1000);
 
    }
 
}
 
/*********************+拿放控制函数+*******************/
 
/*
串口发送指令集：
1.舵机控制：
    IN   机械臂复位
    G1   在原料区夹取物块并且放置到车上
    SC   二维码检测
    P1   放置到粗加工区
    P2   放置到半成品区
    P3   在半成品区码垛
    R1   检测第一个物料颜色
    R2   检测第二个物料颜色
    R3   检测第三个物料颜色
2.任务信息
    QR### 二维码信息
    CO### 颜色信息
*/
 
/*********************+拿放控制函数+*******************/
 
void loop() {
 
  digitalWrite(9, HIGH);
 
  digitalWrite(10, LOW);
 
  delay(50000);
 
  digitalWrite(5, HIGH);
 
  digitalWrite(6, LOW);
 
  digitalWrite(9, LOW);
 
  digitalWrite(10, LOW);
 
  Get_1();
 
    while (Serial.available() > 0) {
 
        String order = Serial.readStringUntil('\n');
 
        Serial.println(order);
 
        if (order.length() < 2)break;
 
        if (order == "IN") ServoInit();   //初始化舵机
 
        else if (order == "SC")QRDetect();   //二维码检测
 
        else if (order == "G1") Get_1();   //采用第一个方式进行夹取
 
        else if (order[0] == 'P') {         //P1/2/3分别对应第一次、第二次、第三次放置
 
            if (order == "P1")flag_all = 0;
 
            else if (order == "P2")flag_all = 1;
 
            else if (order == "P3")flag_all = 2;
 
            Put_1(); Serialexam();
 
        }
 
        else if (order[0] == 'R') {
 
            if (order == "R1") {      //读取第一个物块的颜色
 
                ServoSet(servo_num, 108, 105, 0, 0, 64);
 
                ServoSet(servo_num, 167, 153, 65, 15, 64);
 
                ServoSet(servo_num, 120, 153, 65, 15, 64);
 
//                Serialexam();
 
            }
 
            else if (order == "R2") {   //读取第二个物块的颜色
 
                ServoSet(servo_num, 97, 153, 61, 15, 64);
 
            }
 
            else if (order == "R3") {   //读取第三个物块的颜色
 
                ServoSet(servo_num, 75, 154, 66, 15, 64);
 
            }
 
            Serialexam();
 
        }
 
        else {
 
            if (order[0] == 'Q' && order[1] == 'R') {
 
                data[0] = order[2];
 
                data[1] = order[3];
 
                data[2] = order[4];
 
            }
 
            else if (order[0] == 'C' && order[1] == 'O') {
 
                Color[0] = order[2];
 
                Color[1] = order[3];
 
                Color[2] = order[4];
 
                get_orders(Color);           //获得对应的拿取顺序
 
                Serial.print(temp[0]);
 
                Serial.print(temp[1]);
 
                Serial.print(temp[2]);
 
            }
 
            Serialexam();
 
        }
 
    }
 
    delay(200);
 
}

更多详情请见：【S061】全自动导航分拣机器人

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/650230