第七届工程训练大赛—物流搬运小车
距离湖北省省赛结束已经过去了两个月的时间，难得有时间来总结一下。
前言
这次我们参加的赛项是物流搬运小车。比赛的要求简单来说就是获取二维码的信息，通过该信息的要求，让小车在指定位置按照规定的顺序搬运物料。因此在整个小车的制作过程中要准备好四个部分：二维码的识别部分、图像识别部分、机械臂的控制部分、小车的运动定位部分
先看一段我们的调试视频：
第七届工程训练大赛---物流搬运小车-2021-7-22 21:06:54
一、比赛题目具体要求
初赛现场任务要求:
物料的形状和尺寸：
比赛场地：
二、各个部分的具体分析
1.二维码识别部分
对于二维码的识别部分，我们最先的方案是选择用OpenMV，利用OpenMV的二维码识别功能将二维码的信息解出来。但是在后面调试的过程中发现，OpenMV受光线的影响比较大，经常识别不出来从而导致小车被卡在二维码的地方。所以，我们最后直接买了二维码扫描模块，二维码模块输出的数据非常稳定，通过串口就可以读出，推荐使用。
二维码模块串口解码部分：
int count=0;
uint8_t Scan[7]={0};
int i=0;
uint16_t rebuf[7]={0};
void USART2_IRQHandler(void)
{
uint16_t Res;
static int flat=1;
uint8_t Clear=Clear;
if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
{
Res=USART_ReceiveData(USART2);
if(Res==0x02)
{
flat=0;
count=0;
}
count++;
if(count==8)flat=1;
if((Res =='1' || Res =='2' || Res =='3') && flat==1)
{
Res=Res-'0';
printf("%d",Res);
}
}
else if(USART_GetITStatus(USART2,USART_IT_IDLE)!=RESET)
{
Clear=USART2->SR;
Clear=USART2->DR;
}
}
2.图像识别部分
比赛的任务要求中涉及到图像识别部分的地方有两个，一是物料的颜色识别部分，二是靶环的识别。因为对于靶环的识别可以靠光电的定位来代替，所以正真必要的图像识别部分就只是物料的颜色的识别了，这个OpenMV完全足够。
利用OpenMV的色块识别功能，来实现区别不同颜色的物料并且返回物块的坐标
代码如下：
import sensor, image, time ,pyb, math
from pyb import UART
import json
import struct
import lcd
thresholds = [(6, 65, -9, 119, -30, 127), # generic_red_thresholds
(0, 45, -128, -9, -128, 127), # generic_green_thresholds
(0, 73, -128, 72, -128, -55)] # generic_blue_thresholds
stata = None
stata_1 = None
stata_2 = None
switch=-1
flag = 0
flag_1 = 0
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 30)
sensor.set_auto_gain(False) # must turn this off to prevent image washout...
uart = UART(3, 115200)
led = pyb.LED(3)
led2 = pyb.LED(2)
def sending_data(cx,cy):
data =struct.pack("<bbhhb", #格式为俩个字符俩个短整型(2字节)
0x2C, #帧头1
0x12, #帧头2
int(cx), # up sample by 4 #数据1,低位在前
int(cy), # up sample by 4 #数据2
)
uart.write(data); #必须要传入一个字节数组
print(data)
while(True):
img = sensor.snapshot() # Take a picture and return the image.
if uart.any():
stata = uart.read().decode()
if(stata == 'B'):
stata_1='B'
if(stata == 'b'):
stata_2='b'
stata_1='c'
if(stata== 'A'):
led.on()
for code in img.find_qrcodes():
img.draw_rectangle(code.rect(), color = 127)
print(code.payload())
output_str = code.payload()
up = output_str.split('+',2)[0]
down = output_str.split('+',2)[1]
sending_data(int(up),int(down))
up = list(map(int,up))
down = list(map(int,down))
if(stata_1=='B'):
led2.on()
if(flag == 0):
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_whitebal(False) # must be turned off for color tracking
flag = 1
if(stata=='C'):
switch = up[0]
blobs = img.find_blobs([thresholds[switch])
if blobs:
max_blob=find_max(blobs)
print('sum :', len(blobs))
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
sending_data(max_blob.cx(),max_blob.cy())
print(max_blob.cx(),max_blob.cy())
if(stata=='D'):
switch = up[1]
blobs = img.find_blobs([thresholds[switch])
if blobs:
max_blob=find_max(blobs)
print('sum :', len(blobs))
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
sending_data(max_blob.cx(),max_blob.cy())
print(max_blob.cx(),max_blob.cy())
if(stata=='E'):
switch = up[2]
blobs = img.find_blobs([thresholds[switch])
if blobs:
max_blob=find_max(blobs)
print('sum :', len(blobs))
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
sending_data(max_blob.cx(),max_blob.cy())
print(max_blob.cx(),max_blob.cy())
if(stata_2=='b'):
led2.on()
if(stata=='C'):
switch = down[0]
blobs = img.find_blobs([thresholds[switch])
if blobs:
max_blob=find_max(blobs)
print('sum :', len(blobs))
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
sending_data(max_blob.cx(),max_blob.cy())
print(max_blob.cx(),max_blob.cy())
if(stata=='D'):
switch = down[1]
blobs = img.find_blobs([thresholds[switch])
if blobs:
max_blob=find_max(blobs)
print('sum :', len(blobs))
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
sending_data(max_blob.cx(),max_blob.cy())
print(max_blob.cx(),max_blob.cy())
if(stata=='E'):
switch = down[2]
blobs = img.find_blobs([thresholds[switch])
if blobs:
max_blob=find_max(blobs)
print('sum :', len(blobs))
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
sending_data(max_blob.cx(),max_blob.cy())
print(max_blob.cx(),max_blob.cy())
3.机械臂部分
因为规则有要求，机械臂不准买整套的成品，所以我们的机械臂全部是由自己购买舵机拼接而成。具体部件有：180度舵机3个，270度舵机2个。其中180度舵机安装在云台和机械爪上，270度舵机安装在中间的两个关节处。
如下图：
关于机械臂的控制，比较简单。因为物料的搬运的位置是确定的，所以机械臂只要在各个位置坐规定的动作就可以了，这在调试的阶段完全可以完成.
头文件如下：
#ifndef __AUTOARM_H
#define __AUTOARM_H
void arm_control(double x,double z);
void dj_control(int Dj,double du);
int tran_angle(double du);
int tran_angle_1(double du);
int Dj_vel(int Dj,int v);
void arm_first(int du);
void arm_first_1(int du);
void arm_first_2(int du);
void arm_up(int du);
void arm_down(int du);
void arm_fang_car(int du);
void arm_na(int du);
void arm_na_ground(int du_1,int du_2);
void arm_na_ground_1(int du_1,int du_2);
void arm_fang_ground(int du);
void arm_fang_ground_1(int du);
void arm_fang_ground_2(int du);
void arm_fang_up(int du);
void arm_found(int du);
void arm_down_car(int du);
#endif
底层代码如下，分别是舵机的转速控制、舵机的角度控制：
int Dj_vel(int Dj,int v)
{
if(vel[Dj*2-4]<vel[Dj*2-3])
{
vel[Dj*2-4]=vel[Dj*2-4]+1;
dj_control(Dj,vel[Dj*2-4]);
delay_ms(v*5);
}
else if(vel[Dj*2-4]>vel[Dj*2-3])
{
vel[Dj*2-4]=vel[Dj*2-4]-1;
dj_control(Dj,vel[Dj*2-4]);
delay_ms(v*5);
}
else if(vel[Dj*2-4]==vel[Dj*2-3])
{
return 0;
}
return 1;
}
/**********************************************************
*Name :dj_control
*Funs :¿ØÖƶæ»úÔ˶¯
*Input :Dj£º¶æ»úµÄÐòºÅ£»du:¶æ»úת¹ýµÄ½Ç¶È
*Output :None
***********************************************************/
void dj_control(int Dj,double du)
{
switch(Dj)
{
case 1: TIM_SetCompare1(TIM3,tran_angle(du)); break;
case 2: TIM_SetCompare2(TIM3,tran_angle(du)); break;//¶æ»ú2×°±¸·´ÁË£¬ËùÒԽǶȷ´×ÅËã
case 3: TIM_SetCompare3(TIM3,tran_angle_1(du)); break;
case 4: TIM_SetCompare4(TIM3,tran_angle_1(du)); break;
case 5: TIM_SetCompare1(TIM14,tran_angle(du)); break;
default: break;
}
}
/**********************************************************
*Name :tran_angle
*Funs :½«½Ç¶Èת»»³ÉPWM²¨µÄÕ¼¿Õ±È¶ÔÓ¦Öµ
*Input :¶æ»úҪת¶¯µÄ½Ç¶È
*Output :ת»»³ÉµÄPWM²¨Õ¼¿Õ±È¶ÔÓ¦Öµ
***********************************************************/
int tran_angle(double du)
{
int angle=0;
angle=19500-(2000.0/180.0)*du;
return angle;
}
/**********************************************************
*Name :tran_angle_1
*Funs :½«½Ç¶Èת»»³ÉPWM²¨µÄÕ¼¿Õ±È¶ÔÓ¦Öµ
*Input :¶æ»úҪת¶¯µÄ½Ç¶È
*Output :ת»»³ÉµÄPWM²¨Õ¼¿Õ±È¶ÔÓ¦Öµ
***********************************************************/
int tran_angle_1(double du)
{
int angle=0;
angle=19500-(2000.0/270.0)*du;
return angle;
}
4.小车的运动定位部分
首先是小车的运动部分。
小车的轮子我们采用的是麦克纳姆轮，主要是看中它的全向移动的特点，这样小车在行驶的过程中就可不用通过旋转来改变方向。
小车沿着轨迹的行驶过程，我们采用的是陀螺仪加光电双重控制，其中通过陀螺仪反馈的角度信息来构成姿态PID，保证小车在行驶的过程中姿态始终保持正直。然后通过光电是否压线构成的反馈来限制小车在行驶的过程不脱离黑线。
其次是小车的定位部分。
在这个题目中由于有黑线构成的网格，所以对于定位而言最简单的方法是数黑线的条数。这个部分，我们运用了两个光电，小车前面放置一个光电，小车左边放置一个光电，当小车前后走时候，左边的光电进行计数，当右边的光电左右走的时候，前面的光电进行计数，这样就可以根据记得数目来确定小车的具体坐标。
程序部分：
循迹的头文件：
#define GD1 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_1) //PA1
#define GD2 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_2) //PA2
#define GD3 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0) //PA3
#define GD4 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4) //PA4
#define GD5 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5) //PA5
#define GD6 GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6) //PA6
#define GD7 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_11) //PF11
#define GD8 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_12) //PF12
#define GD9 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_13) //PF13
#define GD10 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_14) //PF14
void Line_Init(void);
void Line_Init_1(void);
void Line_Follow_x(int a,int vel);
void Line_Follow_x_1(int a,int vel);
void Line_Follow_y(int a,int vel);
void Line_Follow_y_1(int a,int vel);
void Line_Follow_MV(int mode,int vel);
void Line_Follow_2(int vel);
void Line_Position(int mode,int a,int b,int vel_1,int vel_2,int vel_3,int vel_4);
void Line_Follow_Ago(int vel);
void Line_Follow_Bgo(int vel);
void Line_Follow_Rgo(int vel);
void Line_Follow_Lgo(int vel);
部分底层程序：
/**********************************************************
*Name :Line_Position
*Funs :ʹС³µÔ˶¯µ½×ø±ê£¨a£¬b£©µÄλÖÃ
*Input :a,b£º×ø±ê£»mode£º°´ÕÕijÖÖ·½Ê½
*Output :1:±ê־룬Íê³ÉÐźš£
***********************************************************/
void Line_Position(int mode,int a,int b,int vel_1,int vel_2,int vel_3,int vel_4)
{
while(x<a || y<b )
{
control_DJ(mode,vel_1+adv,vel_2-adv,vel_3+adv,vel_4-adv);
}
STOP(mode,vel_1);
}
/**********************************************************
*Name :Line_Follow_y
*Funs :Ѽ£,¼ÆÊýy(¹âµç3)
*Input :b:Ä¿±êÊý£¬vel£ºÒÔvelµÄËÙ¶È
*Output :None
***********************************************************/
void Line_Follow_y(int b,int vel)
{
int v=20;
while(y<b)
{
if(GD6==1)//GD8µ±×ó±ßµÄ¹âµçÅöµ½ºÚÏߣ¬¸øÒ»¸öÏò×óµÄËÙ¶È
{
control_DJ(ago,vel-v,vel+v,vel+v,vel-v);
}
if(GD3==1)//GD7µ±ÓұߵĹâµçÅöµ½ºÚÏߣ¬¸øÒ»¸öÏòÓÒµÄËÙ¶È
{
control_DJ(ago,vel+v,vel-v,vel-v,vel+v);
}
if(GD3!=1 && GD6!=1)
{
control_DJ(ago,vel+adv,vel-adv,vel+adv,vel-adv);
}
}
STOP(ago,vel);
}
总结
因为代码太多了，就没有一一贴出来，需要的同学可以在下面的链接下载。
小车完整程序
版权声明：本文为CSDN博主「SuperPERSUE」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_45385612/article/details/118883617
暂无评论