第七屆工程訓練大賽—物流搬運小車

 距離湖北省省賽結束已經過去了兩個月的時間，難得有時間來總結一下，

前言

這次我們參加的賽項是物流搬運小車，比賽的要求簡單來說就是獲取二維碼的資訊，通過該資訊的要求，讓小車在指定位置按照規定的順序搬運物料，因此在整個小車的制作程序中要準備好四個部分：二維碼的識別部分、影像識別部分、機械臂的控制部分、小車的運動定位部分

先看一段我們的除錯視頻：

一、比賽題目具體要求

初賽現場任務要求:

物料的形狀和尺寸：

比賽場地：

二、各個部分的具體分析

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￡o???úμ?Dòo?￡?du:???ú×a1yμ????è
*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   :?????è×a??3éPWM2¨μ?????±è??ó|?μ
*Input  :???úòa×a?ˉμ????è
*Output :×a??3éμ?PWM2¨????±è??ó|?μ
***********************************************************/
int tran_angle(double du)
{
  int angle=0;
	
	angle=19500-(2000.0/180.0)*du;
	
	return angle;
	
}

/**********************************************************
*Name   :tran_angle_1
*Funs   :?????è×a??3éPWM2¨μ?????±è??ó|?μ
*Input  :???úòa×a?ˉμ????è
*Output :×a??3éμ?PWM2¨????±è??ó|?μ
***********************************************************/
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   :ê1D?3μ???ˉμ?×?±ê￡¨a￡?b￡?μ?????
*Input  :a,b￡o×?±ê￡?mode￡o°′???3??·?ê?
*Output :1:±ê????￡?íê3éD?o??￡
***********************************************************/
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   :?-?￡,??êyy(1aμ?3)
*Input  :b:??±êêy￡?vel￡oò?velμ??ù?è
*Output :None
***********************************************************/
void Line_Follow_y(int b,int vel)
{
	int v=20;
 while(y<b)
 {
  if(GD6==1)//GD8μ±×ó±?μ?1aμ???μ?oú??￡???ò????ò×óμ??ù?è
	{ 
	 control_DJ(ago,vel-v,vel+v,vel+v,vel-v);
	}
	if(GD3==1)//GD7μ±óò±?μ?1aμ???μ?oú??￡???ò????òóòμ??ù?è
	{
	 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);
}

總結

因為代碼太多了，就沒有一一貼出來，需要的同學可以在下面的鏈接下載，
小車完整程式