网站的访问量怎么查,网站建设学习,企业管理培训课程,win怎么没有wordpress一、作品简介
作者#xff1a;贺沅、聂开发、王兴文、石宇航、盛余庆 单位#xff1a;黑龙江科技大学 指导老师#xff1a;邵文冕、苑鹏涛
1. 项目背景
受新冠疫情的影响#xff0c;大学校园内都采取封闭式管理来降低传染的风险#xff0c;导致学生不能外出#xff0c…一、作品简介
作者贺沅、聂开发、王兴文、石宇航、盛余庆 单位黑龙江科技大学 指导老师邵文冕、苑鹏涛
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控制板的选择 在开发板上我们选择BasraBasra是一款基于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.75.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(type1, freq15000000)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)fopen(num_anchors.txt,r) #修改锚点处anchor_txtf.read()L[]for i in anchor_txt.split(,): #直接读出来的i是str类型L.append(float(i))anchortuple(L)f.close()a kpu.init_yolo2(task, 0.6, 0.3, 5, anchor)fopen(num_labels.txt,r) #修改锚点处labels_txtf.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, bgNone , full_w False):if bg:if full_w:full_w img.width()else:full_w len(text)*8*scale4img.draw_rectangle(x-2,y-2, full_w, 16*scale, fillTrue, colorbg)img img.draw_string(x, y, text, colorcolor,scalescale)return imgdef del_all_images():os.chdir(/sd)images_dir cap_imagesif 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_dir1)#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, colorlcd.WHITE,scale1, bglcd.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:aimg0.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)bstr(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, colorlcd.WHITE,scale1, bglcd.RED)elif time.ticks_ms() - last_cap_time 8000:img draw_string(img, 2, 200, release to change images directory, colorlcd.WHITE,scale1, bglcd.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, colorlcd.WHITE,scale1, bglcd.RED)img draw_string(img, 2, 160, keep push to images directory, colorlcd.WHITE,scale1, bglcd.RED)elif time.ticks_ms() - last_cap_time 4000:img draw_string(img, 2, 200, release to change type, colorlcd.WHITE,scale1, bglcd.RED)if time.ticks_ms() - last_cap_time 2000:img draw_string(img, 2, 160, keep push to change type, colorlcd.WHITE,scale1, bglcd.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, colorlcd.WHITE,scale1, bglcd.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, colorlcd.WHITE,scale1, bglcd.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), colorlcd.WHITE,scale1, bglcd.RED)lcd.display(img)f_name {}/{}/{}.jpg.format(images_dir, save_dir, str(save_dir)_str(save_count))img0.save(f_name, quality95) #报错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)), colorlcd.WHITE,scale1, bglcd.RED, full_wTrue)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:aimg.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
#includeServo.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 basePos70;int rArmPos90;int fArmPos30;int clawPos45;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 baseMin0;const int PROGMEM baseMax180;const int PROGMEM rArmMin0;//留一定裕度空间const int PROGMEM rArmMax180;//留一定裕度空间const int PROGMEM fArmMin0;const int PROGMEM fArmMax270;const int PROGMEM clawMin0;const int PROGMEM clawMax54;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 serialCmdSerial.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 strshujuduan(X location1Y location); //x到y的位置Xlocation foundstr(Y);B location foundstr(B);Y strshujuduan(Y location1B location); //Y到B的位置Centerx-x str.toInt();//转成可以用的整型CenterYY 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(j00j0180j10j1180j20j2180j30j3180){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 ifromPos;itoPos;i){arm_servo.write(i);delay(Dtime);}}else{for(int ifromPos;itoPos;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 i0;i4;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 i0;i4;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】智能快递付件机器人
