目录
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的话,代码可能就需要大家自己去稍微改动一下。具体就不详细讲了,有什么问题可以私信博主或者留言评论区。