mirror of
https://gitee.com/StackForce/stackforce_openmv
synced 2025-09-26 22:19:11 +08:00
119 lines
4.4 KiB
Python
119 lines
4.4 KiB
Python
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
|
||
|
||
|
||
|