使用openMV线性回归,然后使用pid输出,使用串口发送到MSP430,在经过处理输出PWM到车轮,实现循迹
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("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
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循迹小车