openMV小车追球

目录

一、总体思路

二、openMV代码

三、MSP430部分代码（处理openMV传来的数据）

四、视频演示

---

一、总体思路

使用openMV寻找最大色块，使用pid对色块的xy轴坐标运算，然后利用串口输出到MSP430，最终MSP430输出PWM控制小车动作，实现追小球

二、openMV代码

• 配置感光元件sensor
• 初始化串口Uart，pid参数，颜色阈值
• 编写函数找最大色块
• 主函数，获取一张图，找最大色块，找到pid计算，没有找到返回-404
• 串口发送xy轴通过pid计算出来的值

# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time, pyb
#import car
from pid import PID
from pyb import UART,LED
import json
import ustruct
# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
uart = UART(3,115200)   #定义串口3变量
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters
def sending_data(xw,xw2):
    global uart;
    data = ustruct.pack("<bbiib",      #格式为俩个字符俩个短整型(2字节)
                   0x2C,                      #帧头1
                   0x12,                      #帧头2
                   int(xw), # up sample by 4   #数据1
                   int(xw2),
                   0x5B)
    uart.write(data);   #必须要传入一个字节数组
# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
green_threshold   = (27, 79, 23, 69, 6, 52)#球颜色参数
size_threshold = 310#球面积，判断距离是否近
#x_pid = PID(p=2.3, i=1, imax=100)#方向
#h_pid = PID(p=0.2, i=0.1, imax=50)#速度
x_pid = PID(p=2.3, i=1, imax=100)#方向
h_pid = PID(p=0.2, i=0.01, imax=50)#速度
def find_max(blobs):#找最大球
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([green_threshold])
    if blobs:
        max_blob = find_max(blobs)
        x_error = max_blob[5]-img.width()/2
        h_error = max_blob[2]*max_blob[3]-size_threshold
        print("x error: ", x_error)
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        img.draw_rectangle(max_blob[0:4]) # rect
        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
        xw=x_pid.get_pid(x_error,1)
        xw2=h_pid.get_pid(h_error,1)
        print("h_output",xw2)
        print("x_output",xw)
        #car.run(-h_output-x_output,-h_output+x_output)
    else:
        #car.run(18,-18)
        xw=-404.0
        xw2=-404.0
        print(xw)
    FH = ustruct.pack("<bbiib",      #格式为俩个字符俩个短整型(2字节)
                   0x2C,                      #帧头1
                   0x12,                      #帧头2
                   int(xw*1000), # up sample by 4   #数据1
                   int(xw2*1000),
                   0x5B)
    uart.write(FH)

三、MSP430部分代码（处理openMV传来的数据）

• 主函数，初始化openMV和小车用到的PWM，读取openMV数据，判断是否发现小球，未发现原地转圈，发现控制小车的速度和方向追小球
• openMV部分，串口接收openMV发送过来的数据，一帧数据是11个字节，因为发送过程中可能会丢包，所以接收2帧数据做一次处理
• 使用联合体获取数据处理

typedef union{          //联合体，供数据处理，4个char合并为1个long或float
  long word32;
  float  byte32_arry;
  char data[4];
}Openmv_word_bytes;

static int num=0;    
Openmv_word_bytes x;
Openmv_word_bytes y;
static char Float_databuf[23];//接收数组
/********************接收中断*************************/
__interrupt void openMV_UART_RX_IRQ_Handler()
{ 
  if(UART_GetITStatus(UART0,UART_RX_IRQn) == TRUE)   //清除串口某一个中断标志
  {
    if(num == 22){      //接收到10字节就不接收，等待数据处理完成
      UART_ClearITPendingBit(UART0,UART_RX_IRQn);
      return;
    }
    Float_databuf[num]=UART_GetChar(UART0);                 //读取一个字节1个字节
    num++;
    UART_ClearITPendingBit(UART0,UART_RX_IRQn);    //清除串口某一个中断标志
  
  }
}

/***************************初始化串口******************/
void openMV_Init()
{
  UART_Init(UART0,115200); //初始化UART0模块,波特率115200，TX_P33_RX_P34, openmv p5-R p4-T
  Set_Vector_Handler(VECTOR_UART0,openMV_UART_RX_IRQ_Handler);    //设置接收中断向量
  UART_ITConfig  (UART0,UART_RX_IRQn,TRUE);                //开串口接收中断
}

/*判断头帧位置*/
int pd(int a){
  while(Float_databuf[a] != 0x2C){
    a++;
  }
  return a;
}

/************************小车追球*********************/
//返回浮点数，xy轴
void openMV_fdata()
{
  int a;
  if(num == 22){
    a = pd(0);
    x.data[0] = Float_databuf[a+2];
    x.data[1] = Float_databuf[a+3];
    x.data[2] = Float_databuf[a+4];
    x.data[3] = Float_databuf[a+5];
    
    y.data[0] = Float_databuf[a+6];
    y.data[1] = Float_databuf[a+7];
    y.data[2] = Float_databuf[a+8];
    y.data[3] = Float_databuf[a+9];
    
    x.byte32_arry = (float)(x.word32/1000.0);//方向
    y.byte32_arry = (float)(y.word32/1000.0);//速度

    num = 0;
    memset(Float_databuf,'\0',23);    
  } 
}

四、视频演示

openMV小车追球