“来拒去留”基于Openmv测距识物，并保持恒定距离的小车

最近一位同学来找我，原因是他的毕业设计要结题了，他的项目是制作一个可以识别二维码的小车，并且能够实现测距，与此同时，还希望小车与二维码保持恒定的距离。即二维码向小车靠近，小车就后退，二维码向小车远离，小车就向前。（有种欲擒故纵的感觉）
实现这一功能，我制作了如下的教程，供大家参考，源码放在文中。
Part 1
所需材料和模块：L298n电机，L298n电机驱动板2块（这个比较实用，便宜而且性能强大），Openmv一块，麦克纳姆轮，arduino及扩展板一块，12v电源一块），杜邦线若干
00:29
VID_20210507_120807

“来拒去留”基于Openmv测距识物，并保持恒定距离的小车

方案如下
1.驱动电机，小车能够进行行走
2.Openmv不断的进行测距，不断感知与Apriltag的距离
3.Openmv将感知到的距离与arduino进行串口通信，arduino进行判断
4.arduino判断后，对电机发出指令，如果距离较小，则小车向后退，如果距离较大，小车向前走
理清思路后，是不是就很简单了呢？
Part 2
背景知识：AprilTag是一个视觉基准系统，可用于各种任务，包括AR，机器人和相机校准。这个tag可以直接用打印机打印出来，而AprilTag检测程序可以计算相对于相机的精确3D位置，方向和id。对于OpenMV来说，这个特别有用！ 它大概长这个样子：

功能很强大的一个tag

简单来讲，就是Openmv可以识别到Apriltag的x，y，z坐标，还有角度旋转量，是一种非常牛的tag。建系如下，Openmv作为原点中心的，所以一般情况下，z一直是负的。（因为Openmv朝前方）

官方代码

# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
import sensor, image, time, math

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
clock = time.clock()

# 注意！与find_qrcodes不同，find_apriltags 不需要软件矫正畸变就可以工作。

# 注意，输出的姿态的单位是弧度，可以转换成角度，但是位置的单位是和你的大小有关，需要等比例换算

# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）

# c_x 是图像的x中心位置
# c_y 是图像的y中心位置

f_x = (2.8 / 3.984) * 160 # 默认值
f_y = (2.8 / 2.952) * 120 # 默认值
c_x = 160 * 0.5 # 默认值(image.w * 0.5)
c_y = 120 * 0.5 # 默认值(image.h * 0.5)

def degrees(radians):
    return (180