主控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循迹小车