openMV摄像头循迹小车

主控MSP430F5529，电机驱动模块L298N，openMV摄像头

一、总体思路

使用openMV线性回归，然后使用pid输出，使用串口发送到MSP430，在经过处理输出PWM到车轮，实现循迹

二、openMV代码

THRESHOLD = (0, 20, -128, 127, -128, 127) # Grayscale threshold for dark things...
import sensor, image, time
from pyb import UART,LED
from pid import PID
import ustruct
rho_pid = PID(p=0.17, i=0 , d=0)#y=ax+b b截距
theta_pid = PID(p=0.001, i=0, d=0)#a斜率


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
sensor.skip_frames(time = 2000)     # 跳过2s帧
clock = time.clock()                # to process a frame sometimes.

uart = UART(3,115200)   #定义串口3变量
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters
def sending_data(xw):
    global uart;
    data = ustruct.pack("<bbib",      
                   0x2C,                      #帧头1
                   0x12,                      #帧头2
                   int(xw), # up sample by 4   #数据1
                   0x5B)
    uart.write(data);   #必须要传入一个字节数组


while(True):
    clock.tick()
    img = sensor.snapshot().binary([THRESHOLD])#线设置为白色，其他为黑色
    line = img.get_regression([(100,100)], robust = True)#返回直线蓝色
    if (line):
        rho_err = abs(line.rho())-img.width()/2#直线偏移距离
        if line.theta()>90:#直线角度
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()
        img.draw_line(line.line(), color = 127)#画出蓝色直线
        #print(rho_err,line.magnitude(),rho_err)
        if line.magnitude()>8:#线性回归效果，好进行下一步，否则不进行s
            #if -40<b_err<40 and -30<t_err<30:
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            xw = rho_output+theta_output
        else:
            xw=0.0
    else:
        xw=-404.0
        pass
    FH = ustruct.pack("<bbib",      #格式为俩个字符俩个短整型(2字节)
                               0x2C,                      #帧头1
                               0x12,                      #帧头2
                               int(xw*1000), # up sample by 4   #数据1
                               0x5B)
    uart.write(FH)

三、MSP430部分代码

1、串口接收openMV发送过来的数据，一帧数据是7个字节，因为发送过程中可能会丢包，所以接收2帧数据做一次处理

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;//巡线联合体
static char Float_databuf[15];//接收数组
/********************接收中断*************************/
__interrupt void openMV_UART_RX_IRQ_Handler()
{ 
  if(UART_GetITStatus(UART0,UART_RX_IRQn) == TRUE)   //清除串口某一个中断标志
  {
    if(num == 14){      //接收到14字节就不接收，等待数据处理完成
      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;
}

/************************巡线小车*********************/
//返回浮点数
void openMV_fdata()
{
  int a;
  if(num == 14){
    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];
    
    x.byte32_arry = (float)(x.word32/1000.0);
    //OLED_DispFolatAt(FONT_ASCII_6X8,2,0,x.byte32_arry,3);  //在指定位置显示一个浮点数数字

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

openMV循迹小车