人形机器人全能赛openmv巡线代码
import sensor, image, time
from pyb import LED, millis, UART
from math import pi, isnan
import lcd
# PID类
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')
class RobotControl:
flag = 0
def __init__(self):
# 设置pid的初始值
self.rho_pid = PID(p=0.4, i=0)
self.theta_pid = PID(p=0.001, i=0)
lcd.init()
# 获取三种颜色的灯
self.red_led = LED(1)
self.green_led = LED(2)
self.blue_led = LED(3)
# 图像翻转
#sensor.set_pixformat(sensor.GRAYSCALE) # 读入灰色图片
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565) # 读入彩色图片
sensor.set_framesize(sensor.QQVGA) # 160x120 设置图像大小
#sensor.set_vflip(True) #纵向旋转
#sensor.set_hmirror(True) # 水平旋转
#sensor.set_framesize(sensor.CIF) # 320×240
# sensor.set_framesize(sensor.HQQQVGA) # 80x40 设置图像大小
sensor.skip_frames(time=2000) # 跳过2000张图片
# 线的阈值
#self.THRESHOLD = (35, 100, -47, 75, -28, 87) # 黑色
self.THRESHOLD = (29, 0, 61, -86, -69, 55) # 黑色0
#self.THRESHOLD = (2, 34, 61, -83, 80, -71) # 灰度的
#黑色识别
#self.THRESHOLD = (43, 100, -48, 127, -54, 127) #黑色
#self.THRESHOLD = (88,255)
# 通信初始化
self.uart = UART(3, 19200) # 注意波特率
# 计算差值
self.middle = 0
# 每次检测区域 （6块） (x, y, w, h)
self.init_roi = [(5, 50, 70, 10), (85, 50, 70, 10),
(5, 70, 70, 10), (85, 70, 70, 10),
(5, 90, 70, 10), (85, 90, 70, 10), ]
self.roi = []
self.py = 5 # 找的正方形的竖向偏差
self.line_to_middle = 10 # 正常情况下边界到中线的距离 要调的
self.img = 0 # 每次的图片存在这，方便操作
self.sleeptime = 0
self.clocktime = 0
self.blinblin = 0
# 关灯
def close_rgb(self):
self.red_led.off()
self.green_led.off()
self.blue_led.off()
# 开白灯
def while_rgb(self):
self.close_rgb()
self.red_led.on()
self.green_led.on()
self.blue_led.on()
# 开红灯
def red_rgb(self):
self.close_rgb()
self.red_led.on()
# 开绿灯
def green_rgb(self):
self.close_rgb()
self.green_led.on()
# 开蓝灯
def blue_rgb(self):
self.close_rgb()
self.blue_led.on()
def send_message(self, mes):
"""
:param mes: 0 小左 1 左 2 右 3 直走
:param sleeptime: 执行该动作需要睡眠的时间
:return:
"""
# 维护一个睡眠池
sleep_dic = {
0: 3950,
1: 3950,
2: 3950,
3: 3950,
}
self.uart.write(str(mes) + "\n")
self.blinblin += 1
#print(self.blinblin)
if self.blinblin%2 == 1:
self.green_rgb() # 闪了绿灯代表发送消息了
else:
self.blue_rgb()
print(mes)
self.sleeptime = sleep_dic.get(mes, 1000)
#print(self.sleeptime)
#time.sleep_ms(sleep_dic.get(mes, 1000))
def recv_message(self):
pass
# 获取偏差（可能用不上）
def get_output(self, line, img):
output = 0
if (line):
# print(line)
# 中心位置的差值
rho_err = abs(line.rho()) - img.width() / 2
# 计算直线的角度
if line.theta() > 90:
theta_err = line.theta() - 180
else:
theta_err = line.theta()
# 把线画出来
img.draw_line(line.line(), color=127)
print(rho_err, line.magnitude(), theta_err)
if line.magnitude() > 8: # 表示线性回归的效果怎么样
rho_output = self.rho_pid.get_pid(rho_err, 1)
theta_output = self.theta_pid.get_pid(theta_err, 1)
output = rho_output + theta_output
# 缺运动控制
return output
# 获取边界点（6个）
def get_dot(self, roi):
most_pixels = 0
max_x = 0
if self.img:
blob0 = self.img.find_blobs([self.THRESHOLD], roi=roi, pixels_threshold=2, area_threshold=2, merge=True)
if blob0:
for n in range(len(blob0)):
if blob0[n].pixels() > most_pixels:
most_pixels = blob0[n].pixels()
max_x = n
# 将矩形标出
self.img.draw_rectangle(blob0[max_x].x(), blob0[max_x].y(), blob0[max_x].w(), blob0[max_x].h(),
color=(0, 0, 255), thickness=1, fill=False)
# 将矩形中点标出
self.img.draw_cross(blob0[max_x].x() + int(blob0[max_x].w() / 2), blob0[max_x].y() + int(
blob0[max_x].h() / 2), color=(0, 0, 255), size=1, thickness=1)
return blob0[max_x].x() + int(blob0[max_x].w() / 2), \
blob0[max_x].y() + int(blob0[max_x].h() / 2) # 矩形中心
else:
return 0
else:
return 0
def get_dx(self):
left_dot_dic = {}
right_dot_dic = {}
middle_dot = []
if not RobotControl.flag:
RobotControl.flag = 1
roi = self.init_roi
else:
roi = self.roi
# 获取边界坐标
for i in range(6):
roii = roi[i]
dot = self.get_dot(roii)
#print(dot)
if dot:
if i % 2 == 0:
left_dot_dic[dot[1]] = dot[0]
else:
right_dot_dic[dot[1]] = dot[0]
# 获取中点坐标
for y1, x1 in left_dot_dic.items():
if right_dot_dic.get(y1, -1) != -1:
x2 = right_dot_dic.get(y1, -1)
middle_dot.append(((x1 + x2) // 2, y1))
# 将路中点标出
self.img.draw_cross((x1 + x2) // 2, y1, color=(0, 255, 0), size=1, thickness=1)
# 中点排个序
middle_dot = sorted(middle_dot, key=lambda x: x[1], reverse=True)
# 清空变动的坐标列表
self.roi = []
if len(middle_dot) == 3:
numy = 3
for y1, x1 in left_dot_dic.items():
x2 = right_dot_dic.get(y1, -1)
if x1 >= 5:
x1 -= 5
else:
x1 = 0
if x2 >= 160 - 70 - 1:
x2 = 89
self.roi.append((x1, y1 - self.py, 60, 10))
self.roi.append((x2, y1 - self.py, 60, 10))
dx = middle_dot[0][0] - middle_dot[2][0]
middle_x = (middle_dot[0][0] + middle_dot[1][0] + middle_dot[2][0])//3
elif len(middle_dot) == 2:
numy = 2
RobotControl.flag = 0
dx = middle_dot[0][0] - middle_dot[1][0]
middle_x = (middle_dot[0][0] + middle_dot[1][0])//2
elif len(middle_dot) == 1:
numy = 1
RobotControl.flag = 0
dx = 0
try:
middle_x = middle_dot[0][0]
except:
middle_x = 80 # 如果没找到中线的话返回中线值即可
else:
RobotControl.flag = 0
numy = 0
middle_x = 0
if not left_dot_dic:
dx = -1
else:
dx = -2
return dx, middle_x, numy
def run(self):
while 1:
img = sensor.snapshot().binary([self.THRESHOLD], invert=True)
line1 = img.get_regression([(100, 100)], roi=(0, 0, 80, 120), robust=True)
line2 = img.get_regression([(100, 100)], roi=(80, 0, 80, 120), robust=True)
output1 = self.get_output(img, line1)
output2 = self.get_output(img, line2)
if line1 and line2:
img.draw_line(((line1.x1() + line2.x1()) // 2, (line1.y1() + line2.y1()) // 2,
(line1.x2() + line2.x2()) // 2, (line1.y2() + line2.y2()) // 2),
color=(255, 0, 0))
elif line1:
img.draw_line((line1.x1() + self.middle, line1.y1(), line1.x2() + self.middle, line1.y2()),
color=(255, 0, 0))
elif line2:
img.draw_line((line2.x1() - self.middle, line2.y1(), line2.x2() - self.middle, line2.y2()),
color=(255, 0, 0))
else:
pass
def run2(self):
self.clocktime = time.time_ns()
#print(self.clocktime)
while 1:
self.img = sensor.snapshot()
#self.img.rotation_corr(x_rotation=45)
dx, middle_x, numy = self.get_dx()
#print(dx, middle_x, numy)
#print("dx:", dx, "middle_x:", middle_x)
#print(time.time_ns())
#print(self.clocktime)
if (time.time_ns() - self.clocktime)*0.000001 >= self.sleeptime - 100:
print((time.time_ns() - self.clocktime)*0.000001)
self.clocktime = time.time_ns()
if numy == 2:
dx *= 2
elif numy == 1 and middle_x <= 80:
self.send_message(1)
elif numy == 1 and middle_x >= 80:
self.send_message(3)
elif numy == 0:
if dx == -1:
self.send_message(1)
else:
self.send_message(2)
else:
if dx >= 5: # 小左
self.send_message(0)
elif dx <= -10: # 右
self.send_message(2)
else: # 直行
self.send_message(3)
lcd.display(self.img)
if __name__ == '__main__':
robot1 = RobotControl()
robot1.run2()
浙公网安备 33010602011771号