一、作品简介
作者：贺沅、聂开发、王兴文、石宇航、盛余庆
单位：黑龙江科技大学
指导老师：邵文冕、苑鹏涛
1. 项目背景
受新冠疫情的影响，大学校园内都采取封闭式管理来降低传染的风险，导致学生不能外出，学校工作人员不能入校。通过封闭式的管理以此来尽最大可能保证学生在当前新冠传染和大规模人群被感染的情况下的安全，在此种情况下出现了取件困难、取件效率低、快递堆积在驿站等诸多快递服务问题，严重时也导致了快递无法进校。同时也严重提升了在校学生们的感染风险，严重影响了同学们的日常生活需要。
为了解决在校快递取件的问题，我们设计了一种由行走机构、抓取机构、控制系统和视觉系统组成的校园智能无人快递小车，以实现”无接触“式、消毒式快递配送，解决快递取件效率低的问题，减小了人力和物力，使得快递处理简单高效快捷，在快递的最后一站充分降低学生拿快递时新冠病毒感染风险，同时避免了校外人员入校传播病毒的风险。
2. 项目进展
2.1技术路线
如上图所示，我们所设计的快递附件机器人由机器人本体与被检测物（货物）组成。其总体流程如下：机器人通过一部分检测模块识别货物所在位置；将该信息反馈于快递附件机器人的控制板模块，控制板模块则命令驱动模块驱动，行走模块按照指定路线进行运动，等待抓取模块完成操作，抓取操作完成后控制模块驱动小车的行走模块进行下一步运动。
2.2设计路线
2.3项目创新点
2.3.1结构部分
采用了连杆机构，其运动副一般均为低副。低副两运动副元素为面接触，压强较小，可承受较大的载荷；可以方便地用来达到增力、扩大行程和实现远距离传动的优势，可方便机械臂抓取高层快递，我们采用中型球型件代替普通连杆使传动更稳定，且具有各部件之间不易松动的特点；采用齿轮传动结构，通过6个齿轮进行传动能保证稳定传动的同时具有准确的传动比，可以实现平行轴、相交轴、交错轴等空间任意两轴间传动的优点。
2.3.2运动部分
运动部分通过四个直流马达支架将四个直流马达固定并配合四个轮胎组成运动机构，采用差速法控制小车转向，采用循迹进行路线规划，使用pwm进行调速，具有速度快的特点，且采用提取取件码第一位数字的方式，判断快递架位置和小车取完快递从后门出发，具有高效、快捷的优点，减少了空间的占用和取件的时间。
2.3.3视觉部分
采用了图像畸变矫正处理、轮廓提取算法和神经网络模型训练，解决了图像显示不清晰，识别率不够高问题的同时，实现了在不同光照条件下快递编号的识别，且有较高的准确率。
3. 项目总结
为了解决受新冠疫情影响、学校封闭式管理、学生不能外出取件、快递取件难、快递在快递站堆积的问题，我们设计了一种由行走机构、抓取机构、控制系统和视觉系统组成的校园智能无人快递小车，以实现”无接触“式、消毒式快递配送，这样避免了校外人员入校传播病毒的潜在风险，由智能快递付件机器人帮忙取校外快递，仅需对小车和快递进行消毒处理，简化了消毒流程，减少了人力、物力的开销，方便快捷了学生生活，减少了快递长时间不取退回的现象。
二、功能介绍
1. 产品结构图
智能快递付件机器人由行走机构、控制模块、抓取机构和视觉模块组成，整个系统由两个12v锂电池分别对控制模块和视觉模块进行供电，以延长机器人的使用时间间隔。控制模块以Basra为主控，实现对机器人的行走、控制、抓取、视觉等过程的控制。机器人搭载了无线蓝牙和语音识别模块，在实现了蓝牙远程操控的同时也能够完成操作参数的动态调整等操作；行走机构采用探索者套件中的轮胎与联轴器相互配合，由直流减速电机驱动，在电机转动下控制小车行走。通过循迹进行路线规划；抓取机构采用连杆机构控制机械爪，对快递进行抓取；视觉模块采用Edge impulse对数字模型进行神经网络训练来实现快递编号的识别，并与下位机实现通信。
2. 主要功能
① 可自主抵达相应的快递架
② 可对所需要取的快递进行识别
③ 可实现远程操作与抓取参数调整
④ 可实现识别与抓取时的状况监控
3. 结构介绍
本作品总体结构由探索者套件拼装，分为主板平面、运动机构、机械臂、抓取结构、载物台、电源仓。
3.1主板平面
使用四个7*11孔平板和两块5*7孔平板构成的搭载主体平台，作为承载机械臂和载物台、连接运动机构主体，同时放置开发板和传感器等其他元器件。
3.2运动机构
通过四个直流马达支架将四个直流马达固定并配合四个轮胎组成运动机构，采用差速法控制小车转向。
3.3机械臂
使用4个输出支架和两个双足连杆搭建机械臂在主板平面上的支座，使用四个大舵机支架连接大舵机，机械臂的底盘舵机装上大舵机输出头后与大臂的舵机支架连接，再将两个大舵机U型支架反向连接作为机械臂大臂，一端连接大臂舵机一端连接小臂舵机。
① 机械臂小臂：由与抓取机构连接的舵机和舵机架构成，另一端连接大舵机U型支架，可实现正转70°，反转55°，可小范围调整抓取机构抓取角度。
② 机械臂大臂：由两个大舵机U型支架反向连接形成，正转110°反转70°，调整抓取结构置前置后，置前时抓取，置后时放置。
③ 机械臂底盘：由支座和舵机支架构成，可使机械手左右转动，调整抓取机构在水平方向上的位置。
3.4抓取结构
抓取结构的运动采用了齿轮传动结构和连杆结构，使用六个30齿齿轮两两叠加构成三个双层齿轮、使用5×7 孔平板作为机械爪零件主板，四个机械手指和四个机械手40mm驱动、两个3×5 折弯、中型球型件构成，滑动零件连接处使用轴套连接，以便抓取机构活动顺畅，且不易松动。抓机构自由度在0-55，如下图所示：
① 连杆结构：由中型球型件和大舵机输出头组成，将舵机产生的扭力，通过连杆传到齿轮上使齿轮转动，并且由于使用的连杆是弧形，中间不会因为触碰到零件主板而导致活动不顺畅。
② 传动结构：传动结构通过三组齿轮啮合将扭力均匀施加到两侧与齿轮连接的机械手40mm驱动上，带动机械手指，使机械手实现张合功能。
3.5载物台
使用一块7×11 孔平板、四块3×5 折弯、和两个大轮组成的载物平台，每个圆板为一个载物平台，每次可装载两件物品，如下图所示，载物台下方留有一定的空腔，作为电池仓，用于放置电源，在一定程度上节约了空间且载物台抬高可减少机械臂运行路程，使机械臂方便、快速放置快递，提高了运行效率。
4. 电控部分
4.1控制板的选择
在开发板上我们选择Basra，Basra是一款基于Arduino开源方案设计的一款开发板，Basra的处理器核心是ATmega328，同时具有14路数字输入/输出口（其中6路可作为PWM输出），6路模拟输入，一个16MHz晶体振荡器，一个USB口，一个电源插座，一个ICSPheader和一个复位按钮。主CPU采用AVRATMEGA328型控制芯片，支持C语言编程方式。该系统的硬件电路包括：电源电路、串口通信电路、MCU基本电路、烧写接口、显示模块、AD/DA转换模块、输入模块、IIC存储模块等其他电路模块电路。控制板尺寸不超过60*60mm，便于安装。CPU硬件软件全部开放，除能完成对小车控制外，还能使用本实验板完成单片机所有基础实验。供电范围宽泛，支持5v~9v的电压，干电池或锂电池都适用。控制板含3A6V的稳压芯片，可为舵机提供6v额定电压。
4.2传感器的选择
灰度传感器又称黑标传感器，可以帮助进行黑线的跟踪，可以识别白色背景中的黑色区域，或悬崖边缘。寻线信号可以提供稳定的输出信号，使寻线更准确更稳定。有效距离在0.7cm~3cm之间。
工作电压：4.7～5.5V，
工作电流：1.2mA。
① 固定孔，便于用螺丝将模块固定于机器人上；
② 四芯输入线接口，连接四芯输入线；
③ 黑标/白标传感器元件，用于检测黑线/白线信号。
4.3语音模块
语音处理技术是下一代多模式交互的人机界面设计中的核心技术之一。随着消费类电子产品中对于高性能、高稳健性的语音接口需求的快速增加，嵌入式语音处理技术快速发展。
根据市场对嵌入式语音识别系统的需求，探索者推出了新的语音识别模块。该模块采用了基于helios-adsp新一代中大词汇语音识别协处理方案的语音识别专用芯片HBR740，非特定人语音识别技术可对用户的语音进行识别，支持中文音素识别，可任意指定中文识别词条(小于8个汉字)，单次识别可支持1000条以上的语音命令，安静环境下，标准普通话，识别率大于95%，可自动检测环境噪声，噪声环境下也能保证较好的识别率。
4.4电动机的选择
我们经过讨论确定选用轮式驱动，但是考虑到只是为了完成自主行走功能，实验也无需越障爬坡，所以我们选择双轴直流电机作为与轮子配合的驱动电机。
除了驱动机器人需要引用电机，检测功能也会需要电机。由于舵机的可控性强，可以在工作范围内精确控制电机的转动角度，而快递机器人的主要工作就是“识别快递、精确定位、作出处理”，所以舵机能够为智能快递付件机器人的工作提供极大的便利。四个舵机使得机器人有了四个自由度，工作范围由线性转变为面性，大大提高了机器人的工作效率。
5. 视觉部分
5.1 训练神经网络模型
通过对大量的图像收集，在Edge Impulse进行图像分类、统一贴标签和训练对应的数据集，以此完成在识别过程中的识别不稳定以及减少错误信息的传递，多次调整图片训练数据集来提高匹配准确率。
5.2 灰度转化（轮廓提取）以及畸变图像处理
① 灰度转化
通过灰度变换来使图像对比度扩展，图像清晰，特征明显，有选择的突出图像感兴趣的特征或者去抑制图像中不需要的特征，进而更加有效的改变图像的直方图分布，使得像素的分布更加均匀，从而提高图像识别精度。
以12数字为例，1代表通道第一层，2代表第二个（从左到右）。先进行整体分开显示，再进行判断快递所在的位置，来传回下位机具体的信息返回值。为了提升识别的准确值，在与训练模型匹配时，再去使用轮廓提取的方法，提取出数字的形状。
② 轮廓提取算法
使用闭运算的方法，即梯度=膨胀-腐蚀，得到图像的轮廓外形，通过使用findcontour ()函数，对灰度图处理过后的图像，找取边界点的坐标，存储到contours参数中，运用drawcontours绘制轮廓线。
下面是findcontour函数的六个参数值：
③ 畸变矫正处理
在测试识别时出现了识别精度低，图像信息获取不全，识别效率低等问题，为此我们采用图像畸变矫正处理，以提高识别精度和效率。
畸变矫正处理是像差的一种，在人们的感官上看原本直线变成了曲线，但是图像的信息不会丢失，调用openmv官方库中的库函数进行图像的处理。对镜头进行了畸变矫正，以去除镜头滤波造成的图像鱼眼效果。
5.3 取件抓取视觉流程图
三、程序代码
1. 示例程序
1.1上位机程序
① data_collection.py
import sensor, lcdfrom Maix import GPIOfrom fpioa_manager import fmfrom board import board_infoimport os, sysimport timeimport imageimport KPU as kpusensor.reset()sensor.set_pixformat(sensor.RGB565)sensor.set_framesize(sensor.QVGA)set_windowing = (224,224)sensor.set_windowing(set_windowing)sensor.set_hmirror(0)sensor.run(1)#####Other####deinit_flag = False #用于在拍照的时候将yolo模型卸载，等到循环重新开始时再加载，减少内存消耗################## lcd config ####lcd.init(type=1, freq=15000000)lcd.rotation(2)#### boot key ####boot_pin = 16fm.register(boot_pin, fm.fpioa.GPIOHS0)key = GPIO(GPIO.GPIOHS0, GPIO.PULL_UP)####################################KPU#######task = kpu.load("/sd/number.kmodel")f=open("num_anchors.txt","r") #修改锚点处anchor_txt=f.read()L=[]for i in anchor_txt.split(","): #直接读出来的i是str类型L.append(float(i))anchor=tuple(L)f.close()a = kpu.init_yolo2(task, 0.6, 0.3, 5, anchor)f=open("num_labels.txt","r") #修改锚点处labels_txt=f.read()labels = labels_txt.split(",")f.close()###################### main ####def capture_main(key):global deinit_flag,anchor,taskdef draw_string(img, x, y, text, color, scale, bg=None , full_w = False):if bg:if full_w:full_w = img.width()else:full_w = len(text)*8*scale+4img.draw_rectangle(x-2,y-2, full_w, 16*scale, fill=True, color=bg)img = img.draw_string(x, y, text, color=color,scale=scale)return imgdef del_all_images():os.chdir("/sd")images_dir = "cap_images"if images_dir in os.listdir():os.chdir(images_dir)types = os.listdir()for t in types:os.chdir(t)files = os.listdir()for f in files:os.remove(f)os.chdir("..")os.rmdir(t)os.chdir("..")os.rmdir(images_dir)# del_all_images()os.chdir("/sd")dirs = os.listdir()images_dir = "cap_images" #cap_images_1last_dir = 0for d in dirs: #把每个已经存在的以cap_images开头的目录遍历一遍得到本次拍照的总目录序号if d.startswith(images_dir):if len(d) > 11:n = int(d[11:])if n > last_dir:last_dir = n'''这一段的作用是每次上电都重新创建一个新的最大文件夹'''#images_dir = "{}_{}".format(images_dir, last_dir+1)#print("save to ", images_dir)#if images_dir in os.listdir():##print("please del cap_images dir")#img = image.Image()#img = draw_string(img, 2, 200, "please del cap_images dir", color=lcd.WHITE,scale=1, bg=lcd.RED)#lcd.display(img)#sys.exit(1)#os.mkdir(images_dir)'''这一段的作用是每次上电只有手动才重新创建一个新的最大文件夹,默认是从已经创建的编号最大的文件夹开始'''images_dir = "{}_{}".format(images_dir, last_dir)if not images_dir in os.listdir():os.mkdir(images_dir)'''开机检测第二级目录的起始位置'''os.chdir("/sd/{}".format(images_dir))dirs = os.listdir()last_type = 0for d in dirs: #把每个已经存在的以cap_images开头的目录遍历一遍得到本次拍照的总目录序号n = int(d[0:])if n > last_type:last_type = nif not str(last_type) in os.listdir(): #不存在要重新创建os.chdir("/sd")os.mkdir("{}/{}".format(images_dir, str(last_type)))'''开机检测第三级目录的起始位置'''os.chdir("/sd/{}/{}".format(images_dir,last_type))dirs = os.listdir()last_image = -1for d in dirs: #把每个已经存在的以cap_images开头的目录遍历一遍得到本次拍照的总目录序号n = int(d[len(str(last_type))+1:][:-4]) #去除.jpgif n > last_image:last_image = nos.chdir("/sd")last_cap_time = 0 #用于记录按键松手前按下时候的系统时间last_btn_status = 1 #用于松手检测save_dir = last_type #change type 第二级目录,默认跟上次开机目录一样save_count = last_image + 1 #保存的第几张图片while(True):if deinit_flag:task = kpu.load("/sd/number.kmodel")a = kpu.init_yolo2(task, 0.6, 0.3, 5, anchor)deinit_flag = Falseimg0 = sensor.snapshot()code = kpu.run_yolo2(task, img0)if code:for i in code:a=img0.draw_rectangle(i.rect(),(0,255,0),2)lcd.draw_string(i.x()+45, i.y()-5, labels[i.classid()]+" "+'%.2f'%i.value(), lcd.WHITE,lcd.GREEN)b=str(labels[i.classid()])b.replace(" ","")if set_windowing:img = image.Image()img = img.draw_image(img0, (img.width() - set_windowing[0])//2, img.height() - set_windowing[1]) #//2取整else:img = img0.copy()if key.value() == 0:time.sleep_ms(30)if key.value() == 0 and (last_btn_status == 1) and (time.ticks_ms() - last_cap_time > 500):last_btn_status = 0last_cap_time = time.ticks_ms()else:#2秒内直接拍照，四秒内提示切换数字种类，6秒内提示切换总目录，8秒后切换总目录if time.ticks_ms() - last_cap_time > 4000 and time.ticks_ms() - last_cap_time <6000:img = draw_string(img, 2, 200, "release to change type", color=lcd.WHITE,scale=1, bg=lcd.RED)elif time.ticks_ms() - last_cap_time > 8000:img = draw_string(img, 2, 200, "release to change images directory", color=lcd.WHITE,scale=1, bg=lcd.RED)elif time.ticks_ms() - last_cap_time <= 8000 and time.ticks_ms() - last_cap_time >6000:img = draw_string(img, 2, 200, "release to change type", color=lcd.WHITE,scale=1, bg=lcd.RED)img = draw_string(img, 2, 160, "keep push to images directory", color=lcd.WHITE,scale=1, bg=lcd.RED)elif time.ticks_ms() - last_cap_time <= 4000:img = draw_string(img, 2, 200, "release to change type", color=lcd.WHITE,scale=1, bg=lcd.RED)if time.ticks_ms() - last_cap_time > 2000:img = draw_string(img, 2, 160, "keep push to change type", color=lcd.WHITE,scale=1, bg=lcd.RED)else:time.sleep_ms(30)if key.value() == 1 and (last_btn_status == 0):a = kpu.deinit(task)del taskdeinit_flag = Trueif time.ticks_ms() - last_cap_time >= 4000 and time.ticks_ms() - last_cap_time < 8000:img = draw_string(img, 2, 200, "change object type", color=lcd.WHITE,scale=1, bg=lcd.RED)lcd.display(img)time.sleep_ms(1000)save_dir += 1save_count = 0dir_name = "{}/{}".format(images_dir, save_dir)os.mkdir(dir_name)elif time.ticks_ms() - last_cap_time >= 8000:img = draw_string(img, 2, 200, "change images directory", color=lcd.WHITE,scale=1, bg=lcd.RED)lcd.display(img)time.sleep_ms(1000)last_dir += 1save_dir = 0save_count = 0images_dir = "{}_{}".format('cap_images', last_dir)os.mkdir(images_dir)print("save to ", images_dir)dir_name = "{}/{}".format(images_dir, save_dir)os.mkdir(dir_name)else:draw_string(img, 2, 200, "capture image {}".format(save_count), color=lcd.WHITE,scale=1, bg=lcd.RED)lcd.display(img)f_name = "{}/{}/{}.jpg".format(images_dir, save_dir, str(save_dir)+'_'+str(save_count))img0.save(f_name, quality=95) #报错ENOENT表示路径不存在save_count += 1last_btn_status = 1img = draw_string(img, 2, 0, "will save to {}/{}/{}.jpg".format(images_dir, save_dir, str(save_dir)+'_'+str(save_count)), color=lcd.WHITE,scale=1, bg=lcd.RED, full_w=True)lcd.display(img)del imgdel img0def main():try:capture_main(key)except Exception as e:print("error:", e)import uios = uio.StringIO()sys.print_exception(e, s)s = s.getvalue()img = image.Image()img.draw_string(0, 0, s)lcd.display(img)main()② shijue.py
import sensorimport imageimport lcdimport KPU as kpulcd.init()sensor.reset()sensor.set_pixformat(sensor.RGB565)sensor.set_framesize(sensor.QVGA)sensor.set_windowing((224, 224))sensor.set_hmirror(0)sensor.run(1)task = kpu.load(0x300000)anchor=[] #放你的标签labels = [] #放anchora = kpu.init_yolo2(task, 0.6, 0.3, 5, anchor)while(True):img = sensor.snapshot()code = kpu.run_yolo2(task, img)if code:for i in code:a=img.draw_rectangle(i.rect(),(0,255,0),2)a = lcd.display(img)for i in code:lcd.draw_string(i.x()+45, i.y()-5, labels[i.classid()]+" "+'%.2f'%i.value(), lcd.WHITE,lcd.GREEN)else:a = lcd.display(img)a = kpu.deinit(task)1.2下位机程序
① jixiebi.ino
#include<Servo.h>//使用servo库Servo base,fArm,rArm,claw;//创建4个servo对象//base(底座舵机11)fArm(第三关节3)rArm(第二关节12)claw(爪4)#include <SoftwareSerial.h>//实例化软串口，设置虚拟输入输出串口。SoftwareSerial BT(2, 3); // 设置数字引脚2是arduino的RX端,3是TX端VoiceRecognition Voice;//声明一个语音识别对象#define Led 8 //定义LED控制引脚#define pi 3.1415926void dateProcessing();void armDataCmd(char serialCmd);//实现机械臂在openmv指示下工作void armLanYaCmd(char serialCmd);void servoCmd(char serialCmd,int toPos);//电机旋转功能函数void vel_segmentation(int fromPos,int toPos,Servo arm_servo);void reportStatus();//电机状态信息控制函数void Arminit();void GrabSth();//建立4个int型变量存储当前电机角度值，设定初始值int basePos=70;int rArmPos=90;int fArmPos=30;int clawPos=45;int data2dArray[4][5] = { //建立二维数组用以控制四台舵机{0, 45, 90, 135, 180},{45, 90, 135, 90, 45},{135, 90, 45, 90, 135},{180, 135, 90, 45, 0}};//存储电机极限值const int PROGMEM baseMin=0;const int PROGMEM baseMax=180;const int PROGMEM rArmMin=0;//留一定裕度空间const int PROGMEM rArmMax=180;//留一定裕度空间const int PROGMEM fArmMin=0;const int PROGMEM fArmMax=270;const int PROGMEM clawMin=0;const int PROGMEM clawMax=54;const int PROGMEM Dtime = 15;//机械臂运动延迟时间，通过改变该值来控制机械臂运动速度//机械臂运动角速度为：π*1000/(180*Dtime) rad/sbool mode = 0;//mode = 1:指令模式；mode = 0:蓝牙模式const int PROGMEM moveStep = 5;//按下按键，舵机的位移量void serialEvent(){while (Serial.available ()) {[char inChar = (char)Serial.read();shuju += inChar;if (inChar == (' n'){[stringComplete = true;}}}void yuyin();{switch(Voice.read()) //判断识别{case 0: //若是指令“kai deng”digitalWrite(Led,HIGH); //点亮LEDbreak;case 1: //若是指令“guan deng”digitalWrite(Led,LOW);//熄灭LEDbreak;default:break;}}void setup() {// put your setup code here, to run once:Serial.begin(9600); //设置arduino的串口波特率与蓝牙模块的默认值相同为9600BT.begin(9600); //设置虚拟输入输出串口波特率与蓝牙模块的默认值相同为9600Serial.println("HELLO"); //如果连接成功，在电脑串口显示HELLO，在蓝牙串口显示helloBT.println("hello");pinMode(Led,OUTPUT); //初始化LED引脚为输出模式digitalWrite(Led,LOW); //LED引脚低电平Voice.init(); //初始化VoiceRecognition模块Voice.addCommand("kai deng",0); //添加指令，参数（指令内容，指令标签（可重复））Voice.addCommand("qidongixiebi",0);Voice.addCommand("guan deng",1); //添加指令，参数（指令内容，指令标签（可重复））Voice.addCommand("tingzhi",1);Voice.start();//开始识别base.attach(12);delay(200);rArm.attach(9);delay(200);fArm.attach(5);delay(200);claw.attach(6);delay(200);// Serial.begin(9600);dateProcessing();base.write(90);delay(10);fArm.write(140);delay(10);rArm.write(90);delay(10);claw.write(30);delay(10);}void loop() {// put your main code here, to run repeatedly:if(Serial.available()>0){ //判断串口缓冲区是否有数值char serialCmd = Serial.read(); //将Arduino串口输入的字符赋给serialCmdSerial.println(serialCmd); //在串口监视器打印出输入的字符serialCmdBT.println(serialCmd); //蓝牙模块的串口（在手机屏幕上显示）打印出电脑输入的字符serialCmd}//蓝牙模块有数据输入，就显示在电脑上if(BT.available()>0){int ch = BT.read(); //读取蓝牙模块获得的数据Serial.println(ch);}if(Serial.available()>0){char serialCmd=Serial.read();//read函数为按字节读取，要注意!delay(10);if(mode == 1){armDataCmd(serialCmd);//openmv控制}else{armLanYaCmd(serialCmd);//蓝牙控制机械臂}}}void dateProcessing(){//数据处理}void armDataCmd(char serialCmd){//实现机械臂在openmv指令下工作if(serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){Serial.print("serialCmd = ");Serial.print(serialCmd);int servoData = Serial.parseInt();//读取指令中的电机转角servoCmd(serialCmd,servoData);}else{{//x的位置int X location;int Y location;//Y的位置//Y的位置int B location;string X str;String Y str;x location = foundstr('X');Y location = foundstr('Y');x str=shujuduan(X location+1，Y location); //x到y的位置Xlocation = foundstr('Y');B location = foundstr('B');Y str=shujuduan(Y location+1，B location); //Y到B的位置Centerx-x str.toInt();//转成可以用的整型CenterY=Y str.toInt();Serial.print("Centerx:");Serial.print(Centerx);Serial.print("Centery: ");Serial.printIn(Centery);for (j1 = 0; j1 < 180; j1++)j1 *= RAD2ANG;j3 = acos(pow(a, 2) + pow(b, 2) + pow(Ll, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a*L1*sin(1) - 2 * b*L1*cos(j1)) / (2 * L2*L3);//if (abs(ANG2RAD(j3)) >= 135) [ j1 = ANG2RAD(j1); continue; }m = L2 * sin(j1) + L3 * sin(j1)*cos(j3) + L3 * cos(j1)*sin(j3);n = L2 * cos(j1) + L3 * cos (j1)*cos(j3) - L3 * sin(j1)*sin(j3);t = a - Ll *sin(jl);p = pow(pow(n,2) + pow(m,2)，0.5);q = asin(m / p);j2 = asin(t / p) - q;x1 = (Ll * sin(j1) + L2 * sin(jl + j2) + L3 * sin(jl + j2 + j3))*cos(j0);y1 =(Ll *sin(jl) + L2 *sin(jl + j2) + L3 * sin(jl + j2 + j3))*sin(jo);zl = Ll * cos(j1) + L2 * cos(jl + j2) + L3 * cos(jl + j2 + j3);j1 = ANG2RAD(j1) ;j2 = ANG2RAD(j2);j3 = ANG2RAD(j3) ;if (xl<(x + 0.1) && x1 >(x - 0.1) && yly + 0.1) && yl >ly - 0.1) && zl(z + 0.1) && 21 >(2 - 0.1))if(j0>0&&j0<180&&j1>0&&j1<180&&j2>0&&j2<180&&j3>0&&j3<180){Serial.println(ANG2RAD(j0));Serial.println( j1);Serial.println( j2);Serial.println( j3);Serial.println( x1);Serial.println( yl);Serial.println( z1);for (int i = 0; i <= 4; i++){ base.write(data2dArray[0][i]); //base舵机对应data2dArray数组中第1“行”元素delay(100); rArm.write(data2dArray[1][i]); //rArm舵机对应data2dArray数组中第2“行”元素delay(100); fArm.write(data2dArray[2][i]); //fArm舵机对应data2dArray数组中第3“行”元素delay(100); claw.write(data2dArray[3][i]); //claw舵机对应data2dArray数组中第4“行”元素delay(500); }for (int i = 4; i >= 0; i--){base.write(data2dArray[0][i]); //base舵机对应data2dArray数组中第1“行”元素delay(100); rArm.write(data2dArray[1][i]); //rArm舵机对应data2dArray数组中第2“行”元素delay(100); fArm.write(data2dArray[2][i]); //fArm舵机对应data2dArray数组中第3“行”元素delay(100); claw.write(data2dArray[3][i]); //claw舵机对应data2dArray数组中第4“行”元素delay(500); }}}}void armLanYaCmd(char serialCmd){//实现机械臂在蓝牙模式下工作int baseJoyPos;int rArmJoyPos;int fArmJoyPos;int clawJoyPos;switch(serialCmd){case 'a'://小臂正转fArmJoyPos = fArm.read() - moveStep;servoCmd('f',fArmJoyPos);delay(10);break;case 's'://底盘左转baseJoyPos = base.read() + moveStep;servoCmd('b',baseJoyPos);delay(10);break;case 'd'://大臂正转rArmJoyPos = rArm.read() + moveStep;servoCmd('r',rArmJoyPos);delay(10);break;case 'w'://钳子闭合clawJoyPos = claw.read() - moveStep;servoCmd('c',clawJoyPos);delay(10);break;case '4'://小臂反转fArmJoyPos = fArm.read() + moveStep;servoCmd('f',fArmJoyPos);delay(10);break;case '5'://底盘右转baseJoyPos = base.read() - moveStep;servoCmd('b',baseJoyPos);delay(10);break;case '6'://大臂反转rArmJoyPos = rArm.read() - moveStep;servoCmd('r',rArmJoyPos);delay(10);break;case '8'://钳子张开clawJoyPos = claw.read() + moveStep;servoCmd('c',clawJoyPos);delay(10);break;case 'i': //输出电机状态信息reportStatus();delay(10);break;case 'l'://电机位置初始化Arminit();delay(10);break;case 'g'://抓取功能GrabSth();delay(10);break;case 'm':Serial.println("meArm has been changed into Instruction Mode");mode = 1;break;default:Serial.println();Serial.println("【Error】出现错误!");Serial.println();break;}}void servoCmd(char serialCmd,int toPos){//电机旋转功能函数Serial.println("");Serial.print("Command:Servo ");Serial.print(serialCmd);Serial.print(" to ");Serial.print(toPos);Serial.print(" at servoVelcity value ");Serial.print(1000*pi/(180*Dtime));Serial.println(" rad/s.");int fromPos;//起始位置switch(serialCmd){case 'b':if(toPos >= baseMin && toPos <= baseMax){fromPos = base.read();vel_segmentation(fromPos,toPos,base);basePos = toPos;Serial.print("Set base servo position value: ");Serial.println(toPos);Serial.println();break;}else{Serial.println("【Warning】Base Servo Position Value Out Of Limit!");Serial.println();return;}break;case 'r':if(toPos >= rArmMin && toPos <= rArmMax){fromPos = rArm.read();vel_segmentation(fromPos,toPos,rArm);rArmPos = toPos;Serial.print("Set rArm servo position value: ");Serial.println(toPos);Serial.println();break;}else{Serial.println("【Warning】Base Servo Value Position Out Of Limit!");Serial.println();return;}break;case 'f':if(toPos >= fArmMin && toPos <= fArmMax){fromPos = fArm.read();vel_segmentation(fromPos,toPos,fArm);fArmPos = toPos;Serial.print("Set fArm servo position value: ");Serial.println(toPos);Serial.println();break;}else{Serial.println();Serial.println("【Warning】Base Servo Value Position Out Of Limit!");Serial.println();return;}break;case 'c':if(toPos >= clawMin && toPos <= clawMax){fromPos = claw.read();vel_segmentation(fromPos,toPos,claw);clawPos = toPos;Serial.print("Set claw servo position value: ");Serial.println(toPos);Serial.println();break;}else{Serial.println("【Warning】Base Servo Position Value Out Of Limit!");Serial.println();return;}break;}}void vel_segmentation(int fromPos,int toPos,Servo arm_servo){//速度控制函数//该函数通过对位置的细分和延时实现电机速度控制//这样的控制平均角速度大约为：1°/15ms = 1.16 rad/sif(fromPos < toPos){for(int i=fromPos;i<=toPos;i++){arm_servo.write(i);delay(Dtime);}}else{for(int i=fromPos;i>=toPos;i--){arm_servo.write(i);delay(Dtime);}}}void reportStatus(){//电机状态信息控制函数Serial.println();Serial.println("---Robot-Arm Status Report---");Serial.print("Base Position: ");Serial.println(basePos);Serial.print("Claw Position: ");Serial.println(clawPos);Serial.print("rArm Position: ");Serial.println(rArmPos);Serial.print("fArm Position: ");Serial.println(fArmPos);Serial.println("-----------------------------");Serial.println("Motor Model:Micro Servo 9g-SG90");Serial.println("Motor size: 23×12.2×29mm");Serial.println("Work temperature:0-60℃");Serial.println("Rated voltage: 5V");Serial.println("Rated torque: 0.176 N·m");Serial.println("-----------------------------");}void Arminit(){//电机初始化函数//将电机恢复初始状态char ServoArr[4] = {'c','f','r','b'};for(int i=0;i<4;i++){servoCmd(ServoArr[i],90);}delay(200);Serial.println("meArm has been initized!");Serial.println();}void GrabSth(){//抓取函数//抓取功能int GrabSt[4][2] = {{'b',135},{'r',150},{'f',30},{'c',40}};for(int i=0;i<4;i++){servoCmd(GrabSt[i][0],GrabSt[i][1]);}}② sketch_nov05a.ino
char serial_data;// 将从串口读入的消息存储在该变量中#define STOP 0#define RUN 1#define BACK 2#define LEFT 3#define RIGHT 4VoiceRecognition Voice;//声明一个语音识别对象int a1 = 6;//左电机1int a2 = 7;//左电机2int b1 = 8;//右电机1int b2 = 9;//右电机2int sensorLeft = 3; //从车头方向的最右边开始排序 探测器int sensorRight = 2;int ENA = 10;//L298N使能端左int ENB = 11;//L298N使能端右int SL;//左边灰度传感器int SR;//右边灰度传感器void setup(){Serial.begin(9600);pinMode(a1, OUTPUT);pinMode(a2, OUTPUT);pinMode(b1, OUTPUT);pinMode(b2, OUTPUT);pinMode(ENA, OUTPUT);pinMode(ENB, OUTPUT);pinMode(sensorLeft, INPUT);//寻迹模块引脚初始化pinMode(sensorRight, INPUT);Voice.init();//初始化VoiceRecognition模块Voice.addCommand("lu kou yi",1);//添加指令，参数（指定内容，指令标签）Voice.addCommand("lu kou er",2);//添加指令，参数（指定内容，指令标签）Voice.addCommand("lu kou san",3);//添加指令，参数（指定内容，指令标签）Voice.addCommand("lu kou si",4);//添加指令，参数（指定内容，指令标签）Voice.start();}void loop(){digitalWrite(ENA,HIGH);digitalWrite(ENB,HIGH);SL = digitalRead(sensorLeft);SR = digitalRead(sensorRight);switch(Voice.read())//判断识别{case 1://指令是 lu kou yicrossing1();delay(3000);WORK(STOP);//停下通过openmv识别delay(5000);WORK(RUN);//识别完毕继续前进delay(2000);//前进两秒后停止WORK(STOP);serial_data = Serial.read();//当抓取完成后发送一个wif( serial_data == 'w' ){WORK(BACK);}if(SR == HIGH & SL == HIGH)//再次识别到黑线时右转{WORK(RIGHT);}tracing();//继续前进break;case 2://指令是 lu lou ercrossing2();delay(3000);WORK(STOP);//停下通过openmv识别delay(5000);WORK(RUN);//识别完毕继续前进delay(2000);//前进两秒后停止准备抓取WORK(STOP);serial_data = Serial.read();//当抓取完成后发送一个wif( serial_data == 'w' ){WORK(BACK);}if(SR == HIGH & SL == HIGH)//再次识别到黑线时左转{WORK(LEFT);}tracing();//继续前进break;case 3:tracing();if(SR == HIGH & SL == HIGH){crossing3();}delay(3000);WORK(STOP);//停下通过openmv识别delay(5000);WORK(RUN);//识别完毕继续前进delay(2000);//前进两秒后停止准备抓取WORK(STOP);serial_data = Serial.read();//当抓取完成后发送一个wif( serial_data == 'w' ){WORK(BACK);}if(SR == HIGH & SL == HIGH)//再次识别到黑线时右转{WORK(RIHGT);}tracing();//继续前进break;case 4:tracing();if(SR == HIGH & SL == HIGH){crossing4();}delay(3000);WORK(STOP);//停下通过openmv识别delay(5000);WORK(RUN);//识别完毕继续前进delay(2000);//前进两秒后停止准备抓取WORK(STOP);serial_data = Serial.read();//当抓取完成后发送一个wif( serial_data == 'w' ){WORK(BACK);}if(SR == HIGH & SL == HIGH)//再次识别到黑线时左转{WORK(LEFT);}tracing();//继续前进}}void WORK(int cmd){switch(cmd){case RUN:Serial.println("RUN"); //输出状态digitalWrite(a1, HIGH);digitalWrite(a2, LOW);analogWrite(leftPWM, 200);digitalWrite(b1, HIGH);digitalWrite(b2, LOW);analogWrite(rightPWM, 200);break;case BACK:Serial.println("BACK"); //输出状态digitalWrite(a1, LOW);digitalWrite(a2, HIGH);analogWrite(leftPWM, 200);digitalWrite(b1, LOW);digitalWrite(b2, HIGH);analogWrite(rightPWM, 200);break;case LEFT:Serial.println("TURN LEFT"); //输出状态digitalWrite(a1, HIGH);digitalWrite(a2, LOW);analogWrite(leftPWM, 100);digitalWrite(b1, LOW);digitalWrite(b2, HIGH);analogWrite(rightPWM, 200);break;case RIGHT:Serial.println("TURN RIGHT"); //输出状态digitalWrite(a1, LOW);analogWrite(leftPWM,200);digitalWrite(a2, HIGH);digitalWrite(b1, HIGH);digitalWrite(b2, LOW);analogWrite(rightPWM,100);break;default:Serial.println("STOP"); //输出状态digitalWrite(a1, LOW);digitalWrite(a2, LOW);digitalWrite(b1, LOW);digitalWrite(b2, LOW);}}void crossing1()//路口1函数{if (SL == LOW && SR == LOW)//左右两边都没有检测到黑线{WORK(RUN);}if (SL == HIGH & SR == LOW)//左侧检测到黑线{WORK(LEFT);}if (SR == HIGH & SL == LOW)//右侧检测到黑线{WORK(RIGHT);}if (SR == HIGH & SL == HIGH)//左右两边都检测到黑线{WORK(RIGHT);}}void crossing2()//路口2函数{if (SL == LOW && SR == LOW)//左右两边都没有检测到黑线{WORK(RUN);}if (SL == HIGH & SR == LOW)//左侧检测到黑线{WORK(LEFT);}if (SR == HIGH & SL == LOW)//右侧检测到黑线{WORK(RIGHT);}if (SR == HIGH & SL == HIGH)//左右两边都检测到黑线{WORK(LEFT);}}void crossing3()//路口3函数{if (SL == LOW && SR == LOW)//左右两边都没有检测到黑线{WORK(RUN);}if (SL == HIGH & SR == LOW)//左侧检测到黑线{WORK(LEFT);}if (SR == HIGH & SL == LOW)//右侧检测到黑线{WORK(RIGHT);}if (SR == HIGH & SL == HIGH)//左右两边都检测到黑线{WORK(LEFT);void crossing4()//路口函数{if (SL == LOW && SR == LOW)//左右两边都没有检测到黑线{WORK(RUN);}if (SL == HIGH & SR == LOW)//左侧检测到黑线{WORK(LEFT);}if (SR == HIGH & SL == LOW)//右侧检测到黑线{WORK(RIGHT);}if (SR == HIGH & SL == HIGH)//左右两边都检测到黑线{WORK(RIGHT);void tracing(){if (SL == LOW && SR == LOW)//左右两边都没有检测到黑线{WORK(RUN);}if (SL == HIGH & SR == LOW)//左侧检测到黑线{WORK(LEFT);}if (SR == HIGH & SL == LOW)//右侧检测到黑线{WORK(RIGHT);}if (SR == HIGH & SL == HIGH)//左右两边都检测到黑线{WORK(RUN);}}更多详细资料请参考 【S021】智能快递付件机器人
