stackforce_openmv/例程/色块识别/OpenMV端/main.py
StackForce ec9c10c613 update
2024-10-25 16:24:25 +08:00

119 lines
4.4 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

import sensor, image, time, math
from pyb import UART
from ulab import numpy as np
import json
import utime
from SF_Serial import MotorController
motor_controller = MotorController() #创建电机控制对象
motor_controller.initialize_serial(port=3, baudrate=921600) # 选择openmv的串口3波特率921600
#yel_threshold = (54, 100, -27, 44, 40, 71)#黄色
#gre_threshold = (0, 100, -50, -15, 0, 45)#绿色
ora_threshold = (0, 100, 15, 55, 25, 55)#橙色
sensor.reset() # 初始化摄像头
sensor.set_pixformat(sensor.RGB565) # 格式为 RGB565.
sensor.set_framesize(sensor.QVGA) # 使用 QQVGA 速度快一些
sensor.skip_frames(time = 2000) # 跳过2000s使新设置生效,并自动调节白平衡
sensor.set_auto_gain(False) # 关闭自动自动增益。默认开启的,在颜色识别中,一定要关闭白平衡。
#sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False)
sensor.set_vflip(True)
clock = time.clock() # 追踪帧率
def _constrain(amt,low,high):
if amt<low:
amt = low
elif amt>high:
amt = high
return amt
#创建pid类
class PIDController:
def __init__(self, P=0, I=0, D=0, limit=0):
self.P = P
self.I = I
self.D = D
self.limit = limit
self.timestamp_prev = 0
self.integral_prev = 0
self.error_prev = 0
def operator(self,error):
# gloabal
timestamp_now = utime.time()
Ts = timestamp_now - self.timestamp_prev
if Ts <= 0 or Ts > 0.5:
Ts = 1e-3
proportional = self.P * error
integral = self.integral_prev + self.I*Ts*0.5*(error + self.error_prev)
integral = _constrain(integral, -self.limit, self.limit)
derivative = self.D*(error - self.error_prev)/Ts
output = proportional + integral + derivative
output = _constrain(output, -self.limit, self.limit)
self.integral_prev = integral
self.output_prev = output
self.error_prev = error
self.timestamp_prev = timestamp_now
return output
#创建pid对象
angle_loop = PIDController(P=1.7,I=0.1,limit=8) #位置环
inertia_loop = PIDController(P=0.6,limit=3) #大减速环
Minertia_loop = PIDController(P=0.1,limit=8) #小减速环
#返回看到的最大色块
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
prev_time = 0
prev_X_targe = 0
while(True):
clock.tick()
img = sensor.snapshot() # 从感光芯片获得一张图像
blobs = img.find_blobs([ora_threshold],area_threshold = 240, pixels_threshold = 240, merge = True)
if blobs:
max_blob = find_max(blobs)#返回看到的最大色块
rect = max_blob.rect()
x, y, w, h = rect[0], rect[1], rect[2], rect[3] #色块左上角x,y坐标和长宽
img.draw_rectangle(rect, color = (255, 0, 0)) #在图像中画框
X_targe = (x+w/2-160)*0.006 #将目标X坐标转为以图像中心为原点的坐标轴坐标并将坐标转为大致角度值
# pid计算
now_time = utime.time() #获取当前时间戳
Ts = now_time - prev_time #获取时间差
if Ts==0: #防止程序卡死
Ts = 0.001
vel_targe = (X_targe - prev_X_targe)/Ts*0.01 #根据目标值变化速度得到电机大致的移动速度
if abs(X_targe) > abs(prev_X_targe): #当前目标值比上一个目标值大,色块往图像外移动,加速追踪阶段不需要减速
outout = X_targe
else:
if X_targe < 0.4: #相机快要追到色块(色块往图像里移动),减速阶段,给非常大的减速环
outout = X_targe + inertia_loop.operator(error=vel_targe)#减速环,速度越大,反向力矩越大
else: #相机还有段距离才能追到色块,小减速阶段,给一个小的减速环
outout = X_targe + Minertia_loop.operator(error=vel_targe)
output_angle = angle_loop.operator(error=outout)#位置环,将减速环作为内环串联到位置环里
output_str="%f"%output_angle #转为字符串
motor_controller.send_target(0, output_str) #串口发送目标位置给M1电机
prev_time = now_time
prev_X_targe = X_targe