import sensor, image, time, math
from pyb import LED, millis, UART
class RobotControl:
flag = 0 # 代表是否使用更新的区域来进行下一次检测
def __init__(self):
# 获取三种颜色的灯
self.red_led = LED(1)
self.green_led = LED(2)
self.blue_led = LED(3)
# 图像翻转
sensor.reset()
#sensor.set_vflip(True)
#sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565) # 读入彩色图片
sensor.set_framesize(sensor.QQVGA) # 160×120
sensor.skip_frames(time=2000) # 跳过2000张图片
# 线的阈值
self.THRESHOLD = (29, 0, 61, -86, -69, 55) # 黑色
self.red_threshold = (0, 100, 19, -62, -81, 61)
# 通信初始化
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.img = None # 每次的图片存在这，方便操作
self.count_turn_left = 0
self.is_cross_barr = 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_to_arduino(self, mes):
print("send_message:", mes) # 调试用
#time.sleep(0.1)
send_str = "{" + str(mes) + "&}"
self.uart.write(send_str)
# 数据接收
def recv_from_arduino(self):
recv = self.uart.read()
if recv:
return recv
else:
return None
# 获取边界点（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
roi = self.init_roi
# 获取边界坐标
for i in range(6):
roii = roi[i]
dot = self.get_dot(roii)
if dot:
dot = list(dot)
if 50 <= dot[1] <= 59:
dot[1] = 55
if 70 <= dot[1] <= 79:
dot[1] = 75
if 90 <= dot[1] <= 99:
dot[1] = 95
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_x并返回
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 # 如果没找到中线的话返回中线值即可
if len(right_dot_dic) >= len(left_dot_dic):
num_list = right_dot_dic.values()
num_list = sorted(num_list, reverse=True) # 降序
else:
num_list = left_dot_dic.values()
num_list = sorted(num_list, reverse=True) # 降序
if len(num_list) == 1:
dx = 0
else:
dx = num_list[0] - num_list[-1]
# 没有找到一个中点的情况，需要进行更多的判断
else:
RobotControl.flag = 0
numy = 0
middle_x = 0
if not left_dot_dic and not right_dot_dic:
dx = -10 # 一个点都看到
elif not left_dot_dic: # 没有左边的边界
num_list = right_dot_dic.values()
num_list = sorted(num_list, reverse=True) # 降序
if len(num_list) == 1:
dx = -1 # 只有右边边界的一个点
else:
dx = -6 # 右边边界有两个及以上个点
print(num_list)
middle_x = num_list[0] - num_list[-1]
elif not right_dot_dic: # 没有右边的点
num_list = left_dot_dic.values()
num_list = sorted(num_list, reverse=True) # 降序
if len(num_list) == 1:
dx = -2 # 只有左边的一个点
else:
dx = -7 # 左边边界有两个及以上个点
middle_x = num_list[0] - num_list[-1]
elif len(right_dot_dic) == 2 and len(left_dot_dic) == 1:
dx = -3 # 右边两个点，左边一个点
elif len(right_dot_dic) == 1 and len(left_dot_dic) == 2:
print("right_dot_dic:", right_dot_dic)
print("left_dot_dic:", left_dot_dic)
dx = -4 # 左边两个点，右边一个点
else:
print("right_dot_dic:", right_dot_dic)
print("left_dot_dic:", left_dot_dic)
dx = -5
return dx, middle_x, numy
def get_distance(self):
blobs = self.img.find_blobs([self.red_threshold], invert=True)
max_pix = 0
count = -1
if blobs:
local_i = -1
for ele in blobs:
count += 1
if ele[4] > max_pix:
max_pix = ele[4]
local_i = count
if local_i >= 0:
b = blobs[local_i]
self.img.draw_rectangle(b[0:4]) # 画正方形
self.img.draw_cross(b[5], b[6]) # 画十字
cx = b.cx()
cy = b.cy() # 获取像素大小
pixels = b.pixels()
rotation = math.degrees(b.rotation()) - 90
return cx, cy, pixels, rotation
return 0, 0, 0, 0
def send_next_move_order(self):
# define GO_STRAIGHT 2 // 直行
# define GO_LITTLE_STRAIGHT 3 // 小直行
# define GO_MICRO_STRAIGHT 4 // 小小直行
# define GO_RIGHT 5 // 右转
# define GO_LITTLE_RIGHT 6 // 小右转
# define GO_LEFT 7 // 左转
# define GO_LITTLE_LEFT 8 // 小左转
# define GO_LEFT_TRANSVERSE 9 // 原地左胯一步
# define GO_RIGHT_TRANSVERSE 10 // 原地右胯一步
# define GO_OBSTACLE 11 // 越障
self.img = sensor.snapshot()
dx, middle_x, numy = self.get_dx() # dx 中点偏差 middle_x 中点的中间值（算出来的） numy 中间点的个数
print("dx, middle_x, numy:", dx, middle_x, numy)
# 若有三个中点， 则只用dx斜率判断
if numy == 3:
# 直行
if -1 <= dx <= 4:
self.send_to_arduino(2) # 直行
elif dx <= -2:
# 在赛道偏外侧，还是能直行
if middle_x <= 80:
self.send_to_arduino(2) # 直行
else:
self.send_to_arduino(5) # 右转
elif dx >= 10:
if middle_x <= 70:
self.send_to_arduino(8) # 小左转
else:
self.send_to_arduino(2) # 直走
elif 5 <= dx <= 10:
if middle_x >= 90:
self.send_to_arduino(5) # 右转
elif 82 <= middle_x <= 89:
self.send_to_arduino(2) # 直行
else:
self.send_to_arduino(7) # 左转
elif numy == 2:
if middle_x >= 85:
self.send_to_arduino(2) # 直行
else:
self.send_to_arduino(8) # 小左转
elif numy == 1:
if dx >= 10:
if 86<=middle_x<=100:
self.send_to_arduino(5) # 右转
else:
self.send_to_arduino(7) # 左转
elif dx >= 5:
if middle_x>=90:
self.send_to_arduino(2) # 直行
else:
self.send_to_arduino(8) # 小左转
else:
self.send_to_arduino(2) # 直行
#if middle_x <= 85:
#self.send_to_arduino(8)
#elif middle_x >= 105:
#self.send_to_arduino(5)
#else:
#self.send_to_arduino(2)
else: # 没有扫到正常的中点 dx有 -1 -2 -3 -4 -5 -6 -7 -10
if dx == -1: # 只有右边一个点
self.send_to_arduino(9) # 左胯一步
elif dx == -2: # 只有左边的一个点
self.send_to_arduino(10) # 右胯一步
elif dx == -3: # 右边两个点，左边一个点
self.send_to_arduino(8) # 小左
elif dx == -4: # 左边两个点，右边一个点
self.send_to_arduino(5) # 右转
elif dx == -6: # 右边边界有两个及以上个点
if middle_x >= 20:
self.send_to_arduino(7) # 左转
elif middle_x >= 15:
self.send_to_arduino(8) # 小左转
else:
self.send_to_arduino(9) # 原地左胯一步
#self.send_to_arduino(10) # 原地右胯一步
#self.send_to_arduino(5) # 原地右转
elif dx == -7:
if 5<=middle_x <= 20:
self.send_to_arduino(5) # 右转
elif middle_x >= 21:
self.send_to_arduino(7) # 左转
else:
self.send_to_arduino(2) # 直行
elif dx == -5:
self.send_to_arduino(7) # 左转
elif dx == -10:
self.send_to_arduino(8)
def find_line_and_red(self):
# define GO_STRAIGHT 2 // 直行
# define GO_LITTLE_STRAIGHT 3 // 小直行
# define GO_MICRO_STRAIGHT 4 // 小小直行
# define GO_RIGHT 5 // 右转
# define GO_LITTLE_RIGHT 6 // 小右转
# define GO_LEFT 7 // 左转
# define GO_LITTLE_LEFT 8 // 小左转
# define GO_LEFT_TRANSVERSE 9 // 原地左胯一步
# define GO_RIGHT_TRANSVERSE 10 // 原地右胯一步
# define GO_OBSTACLE 11 // 越障
# 20 // 小小小直行
# 18 // 原地右转
# 19 // 原地左转
# 99 小小直行+越障
# 98 左转+右胯
self.img = sensor.snapshot()
cx, cy, pixels, rotation = self.get_distance()
if pixels <= 450 and self.is_cross_barr:
if self.is_cross_barr:
if cx>130:
self.is_cross_barr = 0
self.send_to_arduino(97)
return
self.send_next_move_order()
else:
self.is_cross_barr = 1
# 先判断距离，再判断朝向，最后判断左右距离
if cy <= 75:
self.send_to_arduino(2)
elif cy <= 95:
self.send_to_arduino(20)
else:
if rotation >= 0 or rotation <= -85:
self.count_turn_left += 1
if self.count_turn_left == 2:
self.send_to_arduino(98)
else:
self.send_to_arduino(19)
elif -79 <= rotation <= 0:
self.send_to_arduino(18)
else:
if 15 <= cx <= 75:
self.send_to_arduino(99)
elif cx > 76:
self.send_to_arduino(10)
else:
self.send_to_arduino(9)
# 运行入口
def run(self):
self.while_rgb()
while 1:
order = self.recv_from_arduino()
# self.img = sensor.snapshot()
# print(self.get_distance())
sensor.snapshot()
#self.send_next_move_order()
if order:
order = order.decode("utf-8")
print("接受消息：", order)
if order == "g":
self.send_next_move_order()
if order == "r":
self.find_line_and_red()
if __name__ == '__main__':
robot = RobotControl()
robot.run()
浙公网安备 33010602011771号