openmv追踪小车

一.前言

博主最近在参加学校里的一个电设比赛，跟小车相关，所以我就花了一天时间用实验室里残留下来的小车废品搭载openmv实现了一个追踪小车用来做比赛前期的PID测试。当前在各大博客论坛，亦或是b站，Youtube，openmv官方上，有关用OpenMV实现追踪小车的教程数不胜数。其中有些是直接用openmv实现，不与其他单片机进行通信，有些是与stm32 F系列板，或者51，arduino通信，从而让单片机进行驱动小车。可以说是，只要是一款有UART通信功能的单片机，都能实现openmv的追踪功能。那么本期博客，博主将用stm32 mbed（一款老外的stm32）实现openmv的追踪小车。

二.材料准备

我们既然要做一个小车，那必然就得准备一些基本的材料。那我们需要那些材料呢？如果对于小白来说，感觉做一个小车是一个庞大并且难度巨大的工程，当你入手去尝试的话，事实证明这是一件非常轻松的事情（因为openmv IDE帮你写好了大部分的代码，哈哈哈哈，后面就知道了）。

我们需要以下材料：

1.小车的基本框架（淘宝上最基础的就行）注：第一次做小车建议不要用高电压电机

2.L298N电机驱动模块（建议多买两个）

3.18650电池组（建议三节一组的可充电电池）

4.OpenMV

我买的是星瞳科技的plus版本，非plus版本没试过，估计也是可以的，如果单纯是色块问题不大，如果你后期还要进行自己训练目标进行检测的话，我估计plus版本会好一点（虽然我现在训练的多目标检测效果也不是很好）

5. stm32mbed（可选）你用其他的单片机也一样，原理都是相通的，就是代码函数调用的名字和方法会略有区别。不过现在stm32mbed降价了，并且IDE也更新升级了，花个90多买一块玩玩问题也不大。

三.OpenMV端代码

3.1 数据发送

import sensor, image, time, math,time
from pyb import UART
import pyb
import json
import ustruct
from pyb import millis
from math import pi, isnan

class PID:
	_kp = _ki = _kd = _integrator = _imax = 0
	_last_error = _last_derivative = _last_t = 0
	_RC = 1/(2 * pi * 20)
	def __init__(self, p=0, i=0, d=0, imax=0):
		self._kp = float(p)
		self._ki = float(i)
		self._kd = float(d)
		self._imax = abs(imax)
		self._last_derivative = float('nan')
	def get_pid(self, error, scaler):
		tnow = millis()
		dt = tnow - self._last_t
		output = 0
		if self._last_t == 0 or dt > 1000:
			dt = 0
			self.reset_I()
		self._last_t = tnow
		delta_time = float(dt) / float(1000)
		output += error * self._kp
		if abs(self._kd) > 0 and dt > 0:
			if isnan(self._last_derivative):
				derivative = 0
				self._last_derivative = 0
			else:
				derivative = (error - self._last_error) / delta_time
			derivative = self._last_derivative + \
									 ((delta_time / (self._RC + delta_time)) * \
										(derivative - self._last_derivative))
			self._last_error = error
			self._last_derivative = derivative
			output += self._kd * derivative
		output *= scaler
		if abs(self._ki) > 0 and dt > 0:
			self._integrator += (error * self._ki) * scaler * delta_time
			if self._integrator < -self._imax: self._integrator = -self._imax
			elif self._integrator > self._imax: self._integrator = self._imax
			output += self._integrator
		return output
	def reset_I(self):
		self._integrator = 0
		self._last_derivative = float('nan')
		
		
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()
thresholds=[(30, 100, 15, 127, 15, 127)]
size_threshold = 2000

x_pid = PID(p=0.5, i=1, imax=100)
h_pid = PID(p=0.05, i=0.1, imax=50)

uart = UART(3,115200)
uart.init(115200, bits=8, parity=None, stop=1)
def sending_data(left_speed,right_speed,left_label,right_label):
	global uart
	data = ustruct.pack("<bbbb",
				   int(left_speed),
				   int(right_speed),
				   int(left_label),
				   int(right_label)
				   )
	uart.write(data)
	
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):
	pyb.LED(1).off()
	clock.tick()
	img = sensor.snapshot()
	blobs = img.find_blobs(thresholds)
	if blobs:
		pyb.LED(1).on()
		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)
		img.draw_rectangle(max_blob[0:4])
		img.draw_cross(max_blob[5], max_blob[6])
		x_output=x_pid.get_pid(x_error,1)
		h_output=h_pid.get_pid(h_error,1)
		#print("h_output",h_output)
		left_speed=(-h_output)-x_output
		right_speed=(-h_output)+x_output
		if left_speed<0:
		   left_label=0
		else:
		   left_label=1
		if right_speed<0:
		   right_label=0
		else :
		   right_label=1
		if left_speed>100:
		   left_speed=100
		if right_speed>100:
		   right_speed=100
		if right_speed<-100:
		   right_speed=156
		if left_speed<-100:
		   left_speed=156
		print(f"left_speed={left_speed} right_speed={right_speed} left_label={left_label} right_label={right_label}")
		
		sending_data(left_speed,right_speed,left_label,right_label)
	else:
		sending_data(30,30,1,0)

我在openmv官方的代码上进行了一些修改，我们用mbed端接受数据的过程中调用HC.read(buf,sizeof(buf))函数，那么read读取的是uint型整数，那么我们要从python中发送整数类型过去就需要进行一个数据打包，已字节数组的形式发过去。所以就有了def sending_data函数

def sending_data(left_speed,right_speed,left_label,right_label):
	global uart
	data = ustruct.pack("<bbbb",
				   int(left_speed),
				   int(right_speed),
				   int(left_label),
				   int(right_label)
				   )
	uart.write(data)

uart是我们在openmv端定义的一个UART通信协议，与我们后面stm32端命名为HC的UART通信协议进行通信。

3.2 数据处理

数据发送解决了，那么我们需要在openmv官方给的代码上进行一个数据处理。

		if left_speed<0:
		   left_label=0
		else:
		   left_label=1
		if right_speed<0:
		   right_label=0
		else :
		   right_label=1
		if left_speed>100:
		   left_speed=100
		if right_speed>100:
		   right_speed=100
		if right_speed<-100:
		   right_speed=156
		if left_speed<-100:
		   left_speed=156

我在官方的数据代码后面加上了这一段，原因是mbed接受的数据都是uint型整数，所以都是大于等于0的数据，接受不到负数，所以我们需要在openmv端用label标记一下，发送给mbed端，告诉mbed哪个数据是负，哪个数据是正。并且我在mbed端设置的是char buf[32]，所以范围是0-256，如果openmv发送端发送的是-10，那么mbed接受到的数据就是246。如果我们检测到目标就让pid实时更新数据，如果没有发现目标我们就让小车转圈寻找目标

else:
		sending_data(30,30,1,0)

四.stm32 mbed端

首先声明一下，mbed已经更换了新的线上编译IDE，可以说是终于摆脱了之前那个非常丑陋古老的IDE了，网址如下：Log in | Arm Keil Studio

如何使用自己去探索探索，但是一些函数的用法有细微的变更，比如不能使用wait函数了，必须用thread_sleep_for，相关的变换可以去Mbed OS API库中去查询

mbed端代码如下：

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "platform/mbed_thread.h"

BufferedSerial HC(D1,D0,115200);

char buf[32]={0};

DigitalOut in1(D9);
DigitalOut in2(D10);
DigitalOut in3(D11);
DigitalOut in4(D12);
PwmOut left(A1);
PwmOut right(A2);

char left_speed,right_speed;
char left_label,right_label;
int num;

void init()
{

    left.write(0.0);
    right.write(0.0);
    in1=0;
    in2=0;
    in3=0;
    in4=0;
    left_speed=0;
    right_speed=0;
    left_label=2;
    left_label=2;
}
void run()
{
  if((left_label==0 || left_label==1) &&(right_label==0 ||right_label==1)){
    if (left_label==0){
        in3=0;
        in4=1;
        left_speed=left_speed-256;
        }else{
            in3=1;
            in4=0;
            }
    float left_data=float(abs(left_speed)/100.0);
    left.write(left_data);
    if (right_label==0){
        in1=0;
        in2=1;
        right_speed=right_speed-256;
        }else{
            in1=1;
            in2=0;
            }
    float right_data=float(abs(right_speed)/100.0);
    right.write(right_data);
  }else{
      left.write(0.0);
      right.write(0.0);
  }
    }
void getdata_pack()
{
    num=HC.read(buf,sizeof(buf));
    left_speed=buf[0];
    right_speed=buf[1];
    left_label=buf[2];
    right_label=buf[3];
}


int main()
{
    init();
    while(1){
        getdata_pack();
        run();
        }


}

我们在主函数中可以发现，整个程序就用了三个函数-->init() getdata_pack() run()

从函数名字我们可以大概清楚，首先对程序进行init初始化操作，随后进入循环，实时接收openmv端发送过来的字节数组（也就是我们需要调节小车pwm的占空比数据） run()就是最终我们驱动小车的函数。我这里全部使用全局变量，这样也就方便在各个函数间调用变量。这里面唯一要注意的就是run（）函数里面的细节。仔细看代码的话，可能会看到left_speed=left_speed-256,这个其实就是我们在openmv端做的处理，因为不能发负数过来，所以如果label==0，那么我们就要用整数去减256，得到原始openmv端的数据。（负数代表反转），我们看openmv端，如果摄像头没有找到色块，我们就会让它发送label=2，这样子就进入run（）里面的else，也就是停止命令。

五.总结

每个车子的属性不同，所以PID可能需要大家自己去尝试调一调，如果不想让小车那么快的跑的话，完全可以不使用PID算法，直接从openmv端传中心点坐标回来，然后跟中点坐标比较从而控制小车的左右移动。（其实在高性能编码电机的麦克纳姆轮小车中不用PID的效果也会非常的好）。如果不用PID的话，代码可能就需要大家自己去稍微改动一下。具体就不详细讲了，有什么问题可以私信博主或者留言评论区。

stm32 mbed实现openmv追踪小车