基于AVR单片机码垛机器人设计

基于AVR单片机码垛机器人设计目录TOC\o"1-3"\h\u7155第一章绪论 461871.1课程设计主要目的与任务 4281371.2课程设计要求 4189281.3课程设计的意义 410435第二章硬件设计 6213802.1硬件电路的系统概述 6240022.2主芯片 7161172.3多自由度机械手 9320701.认识角度舵机 9142142.零点定义 1062403.姿态调试 10147094.机械手应用 12296945.角度舵机与Atmega328p的接线 13289192.4伺服电机 1388281.认识伺服电机 13189622.伺服电机的调零 13160183.伺服电机与Atmega328p的接线 14572.5QTI传感器 14314921.认识QTI传感器 14132692.QTI传感器的引脚定义 1452433.QTI传感器的性能参数 15320404.QTI传感器与Atmega328p的接线 15244292.6颜色传感器 15284581.认识TCS230颜色传感器 15210622.TCS230传感器的引脚定义 16108283.TCS230传感器的特点 16241084.TCS230颜色传感器的工作原理 16295425.白平衡 17130786.TCS230颜色传感器与Atmega328p的接线 18327、TCS230颜色传感器测试 1856672.7超声波传感器 19144331.认识超声波传感器 19209332.超声波传感器的工作原理 19159863.超声波传感器与Atmega328p的接线 2025163第三章软件设计 21153103.1项目的主流程图 2161593.2Arduino语言及它专门定义的函数 22248653.3颜色传感器的检测程序 24255603.4QTI传感器的检测程序 2570753.5超声波传感器的检测程序 2626465附录1实物图 2720983附录2程序 294941、主程序 28197502、#include"MsTimer2.h" 10192753、颜色传感器的应用程序 105221434、QTI传感器的应用 11198775、机械手的应用程序 11264936、伺服电机的应用程序 12043137、QTI传感器的应用程序 120第一章绪论1.1课程设计主要目的与任务本课题是基于AVR单片机码垛机器人设计，主要是硬件电路的设计和软件电路的设计。主要的目的就是掌握ATmega328p处理器为核心的Openduino控制板中硬件电路的设计方法；单片机软件程序的编写和调试方法；软件Eclipse、串口调试助手、OpenMachineHand的使用方法。本课题的主要任务就是通过传感器的使用，设计一个智能的搬运机器人。主要的任务是通过各种传感器的使用使得机器人更加智能化。划分为这几个模块：1、能够沿规定的路线或者寻迹直行；2、机械手臂能够抓去物块；3、能够识别出物块的颜色；4、能够测量物块与自身的距离，并作出相应的判断；5、能够把9个色块，搬运到目标区域；搬运完成能回到起始区域。1.2课程设计要求1、掌握ATmega328p处理器为核心的Openduino控制板等硬件电路设计方法。2、熟练掌握单片机软件程序的编写和调试方法。3、掌握软件Eclipse、串口调试助手、OpenMachineHand的使用。1.3课程设计的意义本课程设计是培养学生综合运用所学理论知识、发现问题、提出问题、分析问题、解决实际问题、锻炼实践能力的重要环节,是对学生实际工作能力的具体训练和考察过程。随着科学技术发展的日新日异，单片机已经成为当今计算机应用中空前活跃的领域，在生活中可以说得是无处不在。因此作为二十一世纪的大学来说掌握单片机的开发技术是十分重要的。回顾起此次课程设计，我仍感慨颇多，从选题到定稿，从理论到实践，这几个星期，可以说得是苦多于甜，但是可以学到很多很多的的东西，同时不仅可以巩固了以前所学过的知识，而且学到了很多在书本上所没有学到过的知识。通过这次课程设计使我懂得了理论与实际相结合是很重要的，只有理论知识是远远不够的，只有把所学的理论知识与实践相结合起来，从理论中得出结论，才能真正为社会服务，从而提高自己的实际动手能力和独立思考的能力。在设计的过程中遇到问题，可以说得是困难重重，这毕竟第一次做的，难免会遇到过各种各样的问题，同时在设计的过程中发现了自己的不足之处，对以前所学过的知识理解得不够深刻，掌握得不够牢固，比如说不懂一些元器件的使用方法，对单片机汇编语言掌握得不好……通过这次课程设计之后，一定把以前所学过的知识重新温故。通过这次单片机课程设计，我不仅加深了对单片机理论的理解，将理论很好地应用到实际当中去，而且我还学会了如何去培养我们的创新精神，从而不断地战胜自己，超越自己。创新可以是在原有的基础上进行改进，使之功能不断完善，成为真己的东西。硬件设计2.1硬件电路的系统概述相对于智能搬运机器人来说，码垛机器人系统地用到多种传感器来完成码垛色块任务，有更好的稳定性。Openduino控制器的核心是ATmega328p处理器，可以满足系统的复杂性要求。系统中用到的传感器有用来循迹的QTI传感器，用来测距的超声波传感器，用来识别色块颜色的颜色传感器。码垛机器人控制平台的架构如下图所示。图2-1码垛机器人系统架构码垛机器人需要执行循线，测距和抓取色块等动作，循线过程中需要测距，抓取色块后才进行颜色识别，并把物体搬运到目标区域。特色描述：▲支持USB连接线在集成开发环境下调试和下载程序；▲既可用USB接口供电，不需外接电源，也可以使用外部6VDC输入；▲独立的电机驱动电源设计；▲各种传感器和电子元器件标准接口（例如：温湿度、红外线、超声波、热敏电阻、光敏电阻、GPS等）；▲CMUcam摄像头和液晶显示屏接口；▲16路数字舵机接口；▲两路直流电机和两路微型步进电机接口（无需额外驱动）；▲8路专用数字IO扩展口、6路PWM输出、8路10位的ADC扩展接口（电机占用资源包括在内）；▲可以独立于PC机运行等。技术参数：▲主控芯片Atmega328▲晶振：16M▲控制电源：4~6VDC，电机驱动电源：9~24VDC▲通过开关转换的USB编程接口▲主板外形尺寸：130mmx120mm▲适用环境：-40~85°C2.2主芯片图2-2ATmega328p特征：·高性能，低功耗AVR8位微处理器·32K字节Flash，1K字节EEPROM，2K字节RAM，中断向量大小为2个指令字。·高级精简指令集体系结构▲131条强大指令大多数是单一时钟周期执行▲32个（8bit）通用工作寄存器▲完全静态操作▲在20MHz下，数据吞吐率达到每秒20百万条指令▲循环乘法器·高耐力非易失性内存片段▲系统内置4/8/16/32K字节自行Flash程序存储器▲56/512/512/1K字节电可擦只读存储器▲512/1K/1K/2K字节内部静态随机存储器▲写入/擦除周期：10000Flash/100000EEPROM▲数据保持：20年·外围设备特征▲2个独立预分频器和比较模式的8位定时器/计数器▲1个独立预分频器，比较模式和捕获模式的16位定时器/计数器▲独立振荡器的实时计数器▲6个PWM通道▲8通道的10位ADC在MLF封装温度测量▲可编程串行通用同步异步收发器▲主/从设备串行外部接口▲按字节2线串行接口▲可编程的独立片上振荡器的看门狗定时器▲片上模拟比较器▲引脚改变的中端与唤醒·特殊微处理器功能▲电源复位和可编程低功率欠压检测▲内部校准振荡器▲外部/内部中断源▲6种睡眠模式2.3多自由度机械手1.认识角度舵机本设计是利用5个角度舵机相互协作、共同作用来夹取物块。简单地说，通过机械手姿态调试助手改变舵机的脉冲值，使机械手的所在的位置发生改变，从而实现抓取物块，放置物块的能力。如图3-2所示：图2-3多自由度机械手1号舵机接SVO1接口，2号舵机接SVO2接口，3号舵机接SVO3接口，4号舵机接SVO4接口，5号舵机接SVO5接口。如下图所示：图2-4舵机接口2.零点定义手爪舵机的零点：当两个手爪旋转部位成180度时，舵机对应的角度就是手爪的零点位置。关节舵机的零点：当机械臂与安装底盘面成90度夹角时，每个舵机的角度就是对应舵机的零点位置。每个舵机均可从0度位置旋转到180度，从图上看，舵机偏左转（相对机器人是后仰）是从90度向0度转，舵机偏右转（相对机器人是前俯）是从90度向180度转。注意：上述的90度是视觉上相对的90度，不一定是每个舵机90度脉冲对应的位置。图2-5机械手臂零点位置姿态调试为了方便机械手姿态控制，编写调试机械手姿态对应的上位机和下位机软件，软件代码开放，控制板使用的是OpenduinoBoardVer3.0，上位机软件是使用在WindowsXP上运行的VC++6.0开发环境编写的机械手姿势调试助手，下位机软件是使用配置成可开发AVR程序的Eclipse开发环境编写的，上、下位机必须配合使用才可完成调试。将机械手上各个舵机正确连接到控制板（SVO1~SVO8接口），给控制板接上外部电源（锂电池或电源适配器），按一下系统复位键，再将电源开关拨到ON2位置，这时候可以发现机械手被控制到一个基本处于零点的姿态。机械手姿势调试助手的使用步骤：1）点击“连接串口”按钮，若成功打开串口，则按钮会显示“断开串口”2）串口打开后就可以尝试点击“握手设备”按钮，若握手成功，如否则检查串口是否正确，是否下载对应下位机程序，再次握手。3）首先测试调节两个并联的舵机角度，它们的角度必须互补，否则容易损坏舵机，因此上位机软件只用一个调节阀来控制，只给出其中一个舵机的脉宽值，另一个舵机的脉宽值自然就可以得到了。4)鼠标左键点击并按住SVO1&2对应的调节阀，可以左右调节，单位增量为27，右侧的编辑框中的数字也相应改变，数字表示控制通道SVOn（n=1~8）的PWM信号的正脉宽值，单位是0.5us，如图2-6中调节并联舵机的脉宽为3135，那么就是3135*0.5us=1567.5us≈1.6ms，然后点击“刷新舵机”按钮，脉宽数据3135才会被发送到控制板，机械手会立即转动到脉宽对应的角度。其他通道的舵机控制方法一样。当点击“复位机械手”按钮，所有脉宽值将变为默认值3000。图2-6机械手姿态调试助手机械手应用思路：每个机械手的初始化位置都是通过上位机软件调节到零点位置，之后的每个姿态都是以零点位置为参考位置，偏离零点位置的角度值也是通过上位机软件调节而来，脉宽值以27为单位增减。例如某个三自由度机械手的零点位置对应脉宽值为：3135（SVO1&2）、3162（SVO3）、3081（SVO4）、2919（SVO5）。通过软件调节到某个目的姿态，其对应脉宽值为2703（SVO1&2）、2270（SVO3）、3757（SVO4）、3622（SVO5）。如图2-7所示：图2-7则：变化量=（零点值-目标值）/27即：SVO1&2的变化量=（3135-2703）/27=16；SVO3的变化量=（3162-2270）/27=33；SVO4的变化量=（3081-3757）/27=-25；SVO5的变化量=（2919-2703）/27=-26；角度舵机与Atmega328p的接线表1角度舵机与Atmega328p的接线角度舵机Atmega328p引脚角度舵机Atmega328p引脚1号舵机SVO14号舵机SVO42号舵机SVO25号舵机SVO53号舵机SVO32.4伺服电机认识伺服电机伺服电机(servomotor)又称连续旋转电机，是指在伺服系统中控制机械元件运转的发动机，是一种补助马达间接变速装置。伺服电机可使控制速度，位置精度非常准确，可以将电压信号转化为转矩和转速以驱动控制对象。伺服电机转子转速受输入信号控制，并能快速反应，在自动控制系统中，用作执行元件，且具有机电时间常数小、线性度高、始动电压等特性，可把所收到的电信号转换成电动机轴上的角位移或角速度输出。伺服电机的调零所谓伺服电机的调零是指发送一个特定的控制信号（零点标定信号）给伺服电机时，让电机处于静止的状态。由于伺服电机在工厂没有预先调整，它们在接收零点标定信号时可能会转动，因此要用螺丝刀调节伺服电机模块内的调节电阻让伺服电机保持静止，这就是伺服电机的调零过程。图2-8、图2-9就是伺服电机电位器的零点调节以及零点标定信号的时序图。图2-8伺服电机电位器的零点调节 图2-9零点标定信号的时序图如果电机没有进行零点标定，它的连接喉就会转动，而且你会听到里面马达的响声，否则就不会转动且没有响声。当电机没有进行零点标定时，将螺丝刀插入伺服电机的电位器的调节孔，轻轻地转动螺丝刀调节电位器进行零点标定。3.伺服电机与Atmega328p的接线表2伺服电机与Atmega328p的接线序号伺服电机Atmega328p的引脚1左电机2｜PD22右电机19｜A52.5QTI传感器1.认识QTI传感器本设计使用的QTI(QuickTrackInfrared)传感器是一种使用光电接收管来探测物体表面反射光强度的传感器，当QTI传感器面对暗淡的物体表面时，反射光强度很低。当QTI传感器面对明亮的物体表面时，反射光强度很高，不同程度反射光对应传感器不同的输出值，即对应不同亮度的物体时QTI的信号引脚会输出不同的电平信号。本设计中应用到的QTI传感器探测到黑色物体时输出为高电平（+5V），探测到白色物体时输出为低电平（0V），如图2-10所示。图2-10QTI传感器2.QTI传感器的引脚定义1）SIG：信号输出；2）VCC：5V直流电源；3）GND：电源地线；3.QTI传感器的性能参数▲工作温度：-40℃-85℃；▲工作电压：5V；▲连续电流：50mA；▲功耗：100mW；▲佳探测距离：5~10mm；▲佳距离下的大散射角度：65º；▲响应时间：上升沿时间10us,下降沿时间50us。QTI传感器与Atmega328p的接线表3QTI传感器与Atmega328p的接线QTI的个数QTI传感器Atmega328p引脚1靠近左电机14｜A02靠近超声波左侧15｜A13靠近超声波右侧16｜A24靠近右电机17｜A35靠近电池盒18｜A42.6颜色传感器1.认识TCS230颜色传感器颜色传感器就是通过检测某个颜色中三原色的比例来识别该颜色的，本设计中使用的是TCS230颜色传感器，颜色传感器的外观如图2-11所示；颜色传感器的输出信号（由OUT引脚输出）是数字量，可以实现每个颜色信道10位（5V/210）以上的转换精度，可以驱动标准的TTL或者CMOS逻辑输入，因此可直接与微控制器的I/O引脚连接。颜色传感器实物颜色传感器上自带了两个高亮白色LED，有助于提高颜色识别的准确性。图2-11TCS230颜色传感器2.TCS230传感器的引脚定义1）S0、S1：用于选择输出比例因子或者电源关断模式，不同的输出比例因子可调整输出频率，以适应不同的需求；2）S2、S3：用于选择滤波器的类型；3）LED：用于点亮两个LED灯；4）OUT：频率输出引脚，该颜色传感器的典型输出频率范围为2Hz（0.5s）～500kHz（2us）；5）GND：芯片接地引脚；6）+5V、VDD：5V电源。3.TCS230传感器的特点▲可完成高分辨率的光照度/频率转换；▲色彩和输出频率可编程调整；▲单电源工作，工作电压范围为2.7V-5.5V；▲具备掉电恢复功能；▲50KHZ时非线性误差的典型值为0.2%。4.TCS230颜色传感器的工作原理颜色传感器就是通过检测某个颜色中三原色的比例来识别该颜色的，首先打开LED灯，选择输出比例因子，然后依次选定不同的颜色滤波器，每选通一个滤波器就检测其输出引脚输出的不同频率（即光强）的方波脉冲数，最后根据得到的三原色的脉冲数比例判断颜色。S0、S1及S2、S3的不同组合如表4所示：表4S0、S1、S2、S3组合选项S0S1输出比例因子S2S3颜色滤波器类型00关闭电源00红色（R）011:5001蓝色（B）101:510无111:111绿色（G）从理论上来讲，白色是等量的红色、绿色和蓝色混合而成的，但TCS230传感器对这三种颜色的敏感度是不同的，导致TCS230传感器的RGB输出并不相等，因此在使用前必须进行白平衡调整。5.白平衡在本设计中所谓的白平衡就是告诉机器人什么是白色，使得颜色传感器对“白色”中的三原色RGB输出是相等的。通过白平衡调整可以得到在颜色识别时需要用到的通信道的时间基准。白平衡的过程为：在颜色传感器前适当位置放置一个白色物块，打开LED灯，选择输出比例因子，依次通过红色、绿色和蓝色滤波器，对每个通道的脉冲计数到255后关闭通道，最后分别得到每个通道所用的时间，这个时间就是识别颜色时要用的基准时间。白平衡的控制流程如图2-12所示。图2-12白平衡的控制流程TCS230颜色传感器与Atmega328p的接线表5TCS230颜色传感器与Atmega328p的接线TCS230引脚Atmega328P引脚TCS230引脚Atmega328P引脚S07｜PD7OUT3｜PD3S16｜PD6VDD+5（R）S25｜PD5GNDGND（B）S34｜PD4+5+5（R）LED8｜PB0OUT3｜PD3TCS230颜色传感器测试根据颜色传感器的特性，在传感器的使用过程中主要需要知道时间基准和脉冲数，所以在控制程序中可以使用定时器0来统计时间，外部中断1来判断一次脉冲。定时器0是8位定时器，而定时的基本单位不能过大，在统计时间的过程中可以采用定时器0多次累计的方式计算时长。在代码中，对定时器0进行8分频设置，从而定时的基本单位为0.5us。2.7超声波传感器认识超声波传感器本设计中使用的是DM-S28018-B,与普通的超声波一样都是利用声波反射原理。DM-S28018-B提供了精确的、非接触式的距离测量，测量的范围从2cm到3.3m,它非常方便与微控制器连接，只需要两个I/O口进行控制。左边是发射头右边是接收头，其工作频率为12M，从左到右，VCC接5V电源，Trig是接收启动信号引脚，Echo是信号输出引脚，Out是空脚（防盗），GND接地。DM-S28018-B超声波测距传感器的实物图如图2-13所示。图2-13DM-S28018-B超声波传感器的工作原理超声波模块上电后，首先在Trig引脚给超声波模块发送一启动信号（10us），超声波模块接收到启动信号后，就在发射头发出超声波信号，同时将Echo引脚置为高电平，这时控制器就必须开始计时，超声波信号在空气中遇到障碍物后反射回来，当接收头检测到反射回来的超声波信号时，马上将Echo引脚置为低电平，Echo引脚高电平持续的时间t用到转换为距离的计算中。若超过18.5ms还没检测到超声波信号，也将Echo引脚置为低电平，准备下一次测量。超声波测距原理如图2-14所示。图2-14超声波测距原理超声波测距原理超声波在空气中传播速度为340m/s，则测量距离为（t*340）/2。超声波传感器与Atmega328p的接线表６超声波传感器与Atmega328p的接线超声波模块引脚连接Atmega328P引脚ＶＣＣＶＣＣＧＮＤＧＮＤTRIGＰＢ２ECHOＰＢ０ＯＵＴ空软件设计3.1项目的主流程图由码垛机器人的场地图可以看出码垛机器人执行的任务复杂，需要控制的传感器较多，软件编程自然就会有点难度，所以软件架构设计非常重要。在硬件工作正常和连线正确的前提下，软件编程决定任务能否被更稳定、更快速地完成。码垛机器人需要沿着直线走，转弯，抓取色块，放下色块等等多种动作，如果软件结构是使用顺序结构，显然行不通。码垛机器人软件系统架构是采用分支结构来执行，使得系统程序编程变得简单，有条理。码垛机器人的场地图、软件架构流程图如下图所示。图3-1场地图图3-2软件架构流程图3.2Arduino语言及它专门定义的函数Arduino语言建立在C/C++基础上，并把相关的一些参数设置函数化，不用我们去了解硬件的底层，让不了解AVR单片机（微控制器）的朋友也能轻松上手。下面简单的注释一下Arduino语言以及它专门定义的函数。数据类型：1boolean布尔类型7char字符类型2byte字节类型8int整数类型3array数组9float浮点实数类型4long长整型10double双浮点实数5string字符串11unsignedint无符号整型6void空12unsignedlong无符号长整型常量：▲HIGH和LOW：表示数字IO口的电平，HIGH表示高电平（1），LOW表示低电平（0）。▲INPUT和OUTPUT：表示数字IO口的方向，INPUT表示输入（高阻态）,OUTPU表示输出（AVR能提供5V电压40mA电流）。▲true和false：true表示真（1），false表示假（0）。▲以上为基础c语言的关键字和符号，大家可以了解，具体使用可以结合实验的程序。结构：▲voidsetup()初始化变量、管脚模式，调用库函数等▲voidloop()主程序，连续执行函数内的语句▲IO功能操作函数▲数字I/O设定和操作函数▲pinMode(pin,mode)：数字IO口输入输出模式定义函数，pin表示为0～13，mode为INPUT或OUTPUT。▲digitalWrite(pin,value)：数字IO口输出电平函数，pin表示为0～13，value表示为HIGH或LOW。比如定义HIGH可以驱动LED。▲intdigitalRead(pin)：数字IO口读输入电平函数，pin表示为0～13，value表示为HIGH或LOW。比如可以读数字传感器。▲模拟I/O设定和操作函数▲intanalogRead(pin)：模拟IO口读函数，pin表示为0～5（ArduinoDiecimila为0～5，Arduinonano为0～7）。比如可以读模拟传感器（10位AD，0～5V表示为0～1023）。▲analogWrite(pin,value)：PWM数字IO口，PWM输出函数，Arduino数字IO口标注了PWM的IO口可使用该函数，pin表示3,5,6,9,10,11，value表示为0～255。可用于电机PWM调速或音乐播放。时间函数▲delay(ms)：延时函数（单位ms）。▲delayMicroseconds(us)：延时函数（单位us）。数学函数：▲min(x,y)：求最小值▲max(x,y)：求最大值▲abs(x)：计算绝对值▲constrain(x,a,b)：约束函数，下限a，上限b，x必须在ab之间才能返回。▲map(value,fromLow,fromHigh,toLow,toHigh)：约束函数，value必须在fromLow与toLow之间和fromHigh与toHigh之间。▲pow(base,exponent)：指数函数，base的exponent次方。▲sq(x)：平方▲sqrt(x)：开根号3.3颜色传感器的检测程序图识别颜色的流程图intmain(void){init();setup();delay(10);RecogColor();for(i=0;i<3;i++){WhiteBalance(refer_time);//白平衡delay(500);}delay(20);while(1){CurrentColor();Serial.print("R:");Serial.print(clrpulses[0]);Serial.print("");Serial.print("B:");Serial.print(clrpulses[1]);Serial.print("");Serial.print("G:");Serial.println(clrpulses[2]);Serial.println(currentcolor);delay(2000);}}3.4QTI传感器的检测程序intmain(void){init();setup();delay(200);while(1){Serial.print(GetQtiStatus(qtipin[0]));Serial.print(GetQtiStatus(qtipin[1]));Serial.print(GetQtiStatus(qtipin[2]));Serial.print(GetQtiStatus(qtipin[3]));Serial.println(GetQtiStatus(qtipin[4]));}}3.5超声波传感器的检测程序intmain(void){init();setup();delay(200);while(1){dis=DistanceDetection();Serial.println((float)dis,1);delay(2000);}}附录1实物图附录2程序主程序#include<WProgram.h>#include<main.h>#include"OpenMultiServo.h"#include"OpenContinMotor.h"#include"OpenColorSensor.h"#include"OpenUltrasonic.h"#include"OpenQti.h"#defineleftservo2#definerightservo19#defineDownDis150#defineYellow1//黄#defineWhite 2//白#defineRed 3//红#defineBlack 4//黑#defineBlue5//蓝#defineUint27uint8_tyellownum=0;uint8_twhitenum=0;//白区当前色块个数uint8_trednum=0;uint8_tblacknum=0;uint8_tbluenum=0;uint8_tAPnum=3;//A点叠放的色块个数uint8_tBPnum=3;uint8_tCPnum=3;uint8_tDPnum=3;uint8_tEPnum=3;uint8_tfangsum=0;uint8_tremainsum=0;intdot;//传感器引脚定义constintcolorpin[6]={7,6,5,4,8,3};//颜色传感器引脚连接constintqtipin[6]={14,15,16,17,18};//前4个是前方QTI，后面是最后紧挨一起的QTIconstinttrig=12;//超声波Trig引脚constintecho=10;//超声波Echo引脚charfunc=1;//颜色传感器变量intrefer_time[3]={0,0,0};//白平衡得到的基准时间intclrpulses[3]={0,0,0};intcurrentcolor;//超声波变量unsignedlongdis=0,mindis=65535;charcnts1=0,cnts2=0;chari=0;charrecog_times=0;uint16_tInit_servoangle[7]={3000,3054,3378,3000,3000,3000,3000};//存放用户设置的舵机角度，servoangle[0]存放并联舵机角度，单位增量为27uint16_tservoangle[7]={3000,3000,3000,3000,3000,3000,3000};//存放用户设置的舵机角度，servoangle[0]存放并联舵机角度voidCurrentColor(void){ ColorreCognt(refer_time,clrpulses); if(clrpulses[0]>100){//红色滤波器大于150 if(clrpulses[1]>200&&clrpulses[2]>200){//蓝色和绿色滤波器都大于200 currentcolor=White;//白色 } elseif(abs(clrpulses[2]-clrpulses[1]>75)){ currentcolor=Yellow; } else{ currentcolor=Red; } } else{ if(abs(clrpulses[0]-clrpulses[1]>0)){ currentcolor=Black; } else{ currentcolor=Blue; } } delay(1000);}voidFastGoForward(){ PulseOut(leftservo,1700); PulseOut(rightservo,1300); delay(20);}voidSlowGoForward(){ PulseOut(leftservo,1550); PulseOut(rightservo,1450); delay(15);}voidGoBack(void){ PulseOut(leftservo,1480); PulseOut(rightservo,1520); delay(15);}/**左旋转*/voidSpinLeft(void){ PulseOut(leftservo,1400); PulseOut(rightservo,1400); delay(20);}/**右旋转*/voidSpinRight(void){ PulseOut(leftservo,1600);//1540 PulseOut(rightservo,1600);//右旋转（两个轮子一起） delay(20);}voidBlackBack(intdot){ while(dot--){ PulseOut(leftservo,1480); PulseOut(rightservo,1520); delay(15); }}voidTurnLeft(void){ PulseOut(leftservo,1500); PulseOut(rightservo,1350); delay(10);}voidTurnRight(void){ PulseOut(leftservo,1650); PulseOut(rightservo,1500); delay(10);}voidStop(void){ PulseOut(leftservo,1500); PulseOut(rightservo,1500); delay(20);}voidTurnLeftAnyPulse(intpulses){ while(pulses--) { SpinLeft(); delay(2); }}voidTurnRightAnyPulse(intpulses){ while(pulses--) { SpinRight(); delay(2); }}voidCrossblk(intpulses){ while(pulses--) { FastGoForward(); delay(2); }}booleanIsTailQtiBlack(void){ if(GetQtiStatus(qtipin[4])==false) { delay(2); if(GetQtiStatus(qtipin[4])==false) { returnfalse; } else { returntrue; } } else { returntrue; }}/**//检测中间左QTI是否在黑线内*/booleanIsMLeftQtiBlack(void){ if(GetQtiStatus(qtipin[1])==true) { delay(2); if(GetQtiStatus(qtipin[1])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//检测中间右QTI是否在黑线内*/booleanIsMRightQtiBlack(void){ if(GetQtiStatus(qtipin[2])==true) { delay(2); if(GetQtiStatus(qtipin[2])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//检测轮子左QTI是否在黑线内*/booleanIsELeftQtiBlack(void){ if(GetQtiStatus(qtipin[0])==true) { delay(2); if(GetQtiStatus(qtipin[0])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**//检测轮子右QTI是否在黑线内*/booleanIsERightQtiBlack(void){ if(GetQtiStatus(qtipin[3])==true) { delay(2); if(GetQtiStatus(qtipin[3])==true) { returntrue; } else { returnfalse; } } else { returnfalse; }}/**前方中间QTI循线*/booleanMoveMiddleQti(void){ if(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==false) { FastGoForward(); } elseif(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==true) { TurnRight(); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==false) { TurnLeft(); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==true) { FastGoForward(); returntrue; } returnfalse;}/**前方中间QTI循线*/booleanMoveSlowQti(void){ if(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==false) { PulseOut(leftservo,1540); PulseOut(rightservo,1460); delay(15); } elseif(GetQtiStatus(qtipin[1])==false&&GetQtiStatus(qtipin[2])==true) { PulseOut(leftservo,1520); PulseOut(rightservo,1500); delay(10); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==false) { PulseOut(leftservo,1500); PulseOut(rightservo,1480); delay(10); } elseif(GetQtiStatus(qtipin[1])==true&&GetQtiStatus(qtipin[2])==true) { FastGoForward(); returntrue; } returnfalse;}/**轮子附近QTI循线*/booleanMoveEdgeQti(void){ if(GetQtiStatus(qtipin[0])==false&&GetQtiStatus(qtipin[3])==false) { FastGoForward(); } elseif(GetQtiStatus(qtipin[0])==false&&GetQtiStatus(qtipin[3])==true) { TurnRight(); } elseif(GetQtiStatus(qtipin[0])==true&&GetQtiStatus(qtipin[3])==false) { TurnLeft(); } elseif(GetQtiStatus(qtipin[0])==true&&GetQtiStatus(qtipin[3])==true) { FastGoForward(); returntrue; } returnfalse;}/**固定超声波与物件的距离（140-150）*/voidSetDistance(void){ while(1){ dis=DistanceDetection(); if(dis>150){ MoveSlowQti(); delay(20); } elseif(dis<140){ GoBack(); delay(20); } else{ Stop(); break;} }}voidSetDistanceCTop(void){//超声波在放c,e底部物体的距离 while(1){ dis=DistanceDetection(); Serial.println((float)dis,1); if(dis>260){ MoveSlowQti(); delay(20); } elseif(dis<180){ GoBack(); delay(20); } else{ Stop(); break;} }}/**纠正车头方位*/voidCorrectPosition(void){ if(IsMLeftQtiBlack()) { while(IsMLeftQtiBlack())//超声波左侧qti在黑色区域 { SpinLeft();//左转 }} elseif(IsMRightQtiBlack()) { while(IsMRightQtiBlack())//超声波左侧qti在黑色区域 { SpinRight();//右转 }} Stop();}voidOAtoOB(void){ TurnRightAnyPulse(10); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOBtoOC(void){ TurnRightAnyPulse(10); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOCtoOD(void){ TurnRightAnyPulse(10); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOEtoOD(void){ TurnLeftAnyPulse(10);//左转角度15 while(!IsMLeftQtiBlack()){//超声波左侧qti在白色区域 SpinLeft(); } while(!IsMRightQtiBlack()){//超声波右侧qti在白色区域 SpinLeft(); } Stop();}voidODtoOC(void){ TurnLeftAnyPulse(10);//左转角度 while(!IsMLeftQtiBlack()){//超声波左侧qti在白色区域 SpinLeft(); } while(!IsMRightQtiBlack()){//超声波右侧qti在白色区域 SpinLeft(); } Stop();}voidOCtoOB(void){ TurnLeftAnyPulse(10);//左转角度 while(!IsMLeftQtiBlack()){//超声波左侧qti在白色区域 SpinLeft(); } while(!IsMRightQtiBlack()){//超声波右侧qti在白色区域 SpinLeft(); } Stop();}voidOBtoOA(void){ TurnLeftAnyPulse(10); while(!IsMLeftQtiBlack()){ SpinLeft(); } while(!IsMRightQtiBlack()){ SpinLeft(); } Stop();}voidOCtoOA(void){ TurnLeftAnyPulse(10);//左转角度 while(!IsELeftQtiBlack())//左电机qti在白色区域 { SpinLeft();//左转 } while(!IsERightQtiBlack())//右电机qti在白色区域 { SpinLeft(); } while(!IsTailQtiBlack())//中间qti在白色区域 { SpinLeft();//左转 }}voidOCtoOE(void){ TurnRightAnyPulse(10);//右转角度 while(!IsERightQtiBlack())//右轮qti在白色区域 { SpinRight();//右转 } while(!IsELeftQtiBlack())//左轮qti在白色区域 { SpinRight();//右转 } while(!IsTailQtiBlack())//盒子中间qti在白色区域 { SpinRight();//右转 } Stop(); }voidOEtoOO(void){ TurnRightAnyPulse(15); while(!IsMRightQtiBlack()){ SpinRight(); } while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidOAtoOO(void){ TurnLeftAnyPulse(15); while(!IsMLeftQtiBlack()){ SpinLeft(); } while(!IsMRightQtiBlack()){ SpinLeft(); } Stop();}voidFirstOtoMarkArea(void){ for(i=0;i<55;i++){ MoveMiddleQti(); } while(IsTailQtiBlack()){ MoveMiddleQti(); } for(i=0;i<7;i++){ MoveMiddleQti(); } Stop();}voidNoFirstOtoMarkArea(void){ for(i=0;i<50;i++){ MoveMiddleQti(); } while(IsTailQtiBlack()){ MoveMiddleQti(); } SetDistance(); CorrectPosition(); Stop();}voidTurn180(void){ i=0; delay(50); TurnRightAnyPulse(20);//右转角度 while(1){ SpinRight(); if(IsMRightQtiBlack()){//超声波右侧qti在黑色区域 i++; if(i==2){ i=0; Stop(); break; } } } if(IsMRightQtiBlack()){ while(IsMRightQtiBlack()){//超声波右侧qti在黑色区域 SpinRight(); } } elseif(IsMLeftQtiBlack()){ while(IsMLeftQtiBlack()){//超声波左侧qti在黑色区域 SpinLeft(); } } Stop();}voidStartRun(void){ Crossblk(25); while(!MoveEdgeQti());//执行轮子两端qti状态 while(!MoveMiddleQti());//执行超声波两端qti状态 Stop();//停止}voidEndRun(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Crossblk(40); Stop(); while(1);}voidCatchTop(void){//机械手抓取顶部物体 servoangle[0]=Init_servoangle[0]-Uint*37; servoangle[1]=Init_servoangle[1]-Uint*50; servoangle[2]=Init_servoangle[2]-Uint*6; servoangle[3]=Init_servoangle[3]+Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);//10 servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20}voidCatchMiddle(void){//机械手抓取中部物体 servoangle[0]=Init_servoangle[0]-Uint*49; servoangle[1]=Init_servoangle[1]-Uint*54; servoangle[2]=Init_servoangle[2]-Uint*23; servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);//15 servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20}voidCatchBottom(void){//机械手抓取底部物体 servoangle[0]=Init_servoangle[0]-Uint*70; servoangle[1]=Init_servoangle[1]-Uint*52; servoangle[2]=Init_servoangle[2]-Uint*31; servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);//10 servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);//20}voidDownTop(void){//机械手放顶部物体 servoangle[0]=Init_servoangle[0]-Uint*37;servoangle[1]=Init_servoangle[1]-Uint*50;servoangle[2]=Init_servoangle[2]-Uint*6; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5); servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; servoangle[3]=Init_servoangle[3]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidDownMiddle(void){//机械手放中部物体 servoangle[0]=Init_servoangle[0]-Uint*49; servoangle[1]=Init_servoangle[1]-Uint*54; servoangle[2]=Init_servoangle[2]-Uint*23; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5); servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; servoangle[3]=Init_servoangle[3]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidDownBottom(void){//机械手放底部物体 servoangle[0]=Init_servoangle[0]-Uint*70; servoangle[1]=Init_servoangle[1]-Uint*52; servoangle[2]=Init_servoangle[2]-Uint*31; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]-Uint*0; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5); servoangle[0]=Init_servoangle[0]; servoangle[1]=Init_servoangle[1]; servoangle[2]=Init_servoangle[2]; servoangle[3]=Init_servoangle[3]; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidResetHand(void){//初始化手臂 servoangle[0]=Init_servoangle[0]-Uint*30; servoangle[1]=Init_servoangle[1]+Uint*39; servoangle[2]=Init_servoangle[2]-Uint*58; servoangle[3]=Init_servoangle[3]-Uint*10; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(20);}voidRecogColorAction(void){//颜色识别角度舵机5抓紧 servoangle[0]=Init_servoangle[0]-Uint*32; servoangle[1]=Init_servoangle[1]+Uint*33; servoangle[2]=Init_servoangle[2]-Uint*64; servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10); servoangle[3]=Init_servoangle[3]+Uint*42; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);}voidRecogColor(void){//识别颜色角度舵机5松开 servoangle[0]=Init_servoangle[0]-Uint*32; servoangle[1]=Init_servoangle[1]+Uint*33; servoangle[2]=Init_servoangle[2]-Uint*64; servoangle[3]=Init_servoangle[3]+Uint*15; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(10);}voidRecogColorFinish(void){//识别颜色完成手臂状态 servoangle[3]=Init_servoangle[3]+Uint*49; MultiServo::SetJointsAngle(servoangle); MultiServo::UpdateMotion(5);}voidAOtoStartLine(void){ while(!IsERightQtiBlack()){ MoveMiddleQti(); } OEtoOO(); Stop();}voidBOtoStartLine(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Stop(); Crossblk(20); i=0; while(!IsELeftQtiBlack()){ FastGoForward(); } Stop(); TurnRightAnyPulse(10); while(!IsMLeftQtiBlack()){ SpinRight(); } Stop();}voidCOtoStartLine(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Crossblk(5); while(!MoveEdgeQti()); Stop();}voidDOtoStartLine(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); Stop(); delay(100); Crossblk(20); while(!IsERightQtiBlack()){//检测轮子右QTI是否在白色区域 FastGoForward(); } Stop(); TurnLeftAnyPulse(10); while(!IsMLeftQtiBlack()){//超声波左侧qti SpinLeft(); } while(!IsMRightQtiBlack()){//超声波右侧qti SpinLeft(); } Stop();}voidEOtoStartLine(void){ while(!IsELeftQtiBlack()){ MoveMiddleQti(); } OAtoOO(); Stop();}voidACarryWhite(void){ //A点搬运白色块 OAtoOB(); if(whitenum==0){ FirstOtoMarkArea(); } else{ NoFirstOtoMarkArea(); } delay(10); switch(whitenum){ case0: DownBottom(); break; case1: DownMiddle(); break; case2: DownTop(); break; default: Stop(); break; } for(i=0;i<7;i++){ MoveMiddleQti(); } Turn180(); BOtoStartLine(); for(i=0;i<50;i++){ MoveMiddleQti(); } Turn180(); whitenum++;}voidACarryBlack(void){ //A点搬运黑色块 OAtoOB(); OBtoOC(); OCtoOD(); if(blacknum==0){ FirstOtoMarkArea(); } else{ NoFirstOtoMarkArea(); } delay(10); switch(blacknum){ case0: DownBottom(); break; case1: DownMiddle(); break; case2: DownTop(); break; default: Stop(); break; } for(i=0;i<7;i++){ MoveMiddleQti(); } Turn180(); DOtoStartLine(); for(i=0;i<50;i++){ MoveMiddleQti(); } Turn180(); blacknum++;}voidACarryYellow(void){FirstOtoMarkArea(); DownBottom();for(i=0;i<5;i++){ MoveMiddleQti();}Turn180();AOtoStartLine();for(i=0;i<50;i++){ MoveMiddleQti();}Turn180();Stop();yellownum++;}voidCarryATop(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); BlackBack(10); Stop(); OCtoOA(); SetDistance(); CatchTop(); RecogColorAction(); delay(200); CurrentColor(); RecogColorFinish(); switch(currentcolor){ caseWhite: ACarryWhite(); break; caseBlack: ACarryBlack();//拿a点顶端有两种情况 break; default: OAtoOO(); for(i=0;i<90;i++){ MoveMiddleQti(); } Stop(); CorrectPosition(); Stop(); DownBottom(); Turn180(); fangsum++; break; } currentcolor=0;}//其他情况voidCarryAMiddle(void){ while(!MoveMiddleQti()); while(!MoveEdgeQti()); BlackBack(10); Stop(); OCtoOA(); SetDistance(); CatchMiddle(); RecogColorAction(); delay(500); CurrentColor(); RecogColorFinish(); switch(currentcolor){ caseWhite: ACarryWhite(); break; caseBlack: ACarryBlack();//两种情况 break; default: OAtoOO(); for(i=0;i<80;i++){ MoveMiddleQti(); } switch(fangsum){ case0: for(i=0;i<10;i++){ MoveMiddl