"""
@Program  : 工训比赛2022年智能车的源码
@Author  : 李论 18667199758
@Time   : 2022/05/25
@Function   : 主程序

通讯:
0. 告知小车初始化完成，比赛开始 发"\x03\x0a" 收b"OK"

1. 接收通知需要进行二维码扫描： 收b"QR"
1. 告知小车二维码已经扫描结束： 发"\x02\x0a" 收b"OK"

2. 接收通知需要进行抓取东西放到车上：收b"GRAB"
2. 告知小车完成：发"\x04\x0a" 收b"OK"

3. 接收通知需要进行拼装：收b"ASSEMBLE"
3. 告知小车完成：发"\x04\x0a" 收b"OK"

"""
# from carlib import lib_usb_cam  # USB摄像头支持库
# from carlib import lib_serial_arm  # 机械臂支持库
from carlib.lib_serial_init import Serial_device  # 串口类
import cv2
import numpy as np
from pyzbar.pyzbar import decode
import threading
import time
import requests

print("Car Raspberry Pi Started!")
# 初始化串口对象 负责所有的通讯
serial_device = Serial_device()
print("here")

# 等待启动
print("0.等待’启动‘指令")
serial_device.wait_vehicle_msg(b'START')

print("0.初始化完成，比赛开始！")
# 初始化完成
serial_device.send_to_vehicle("\x03\x0a")


def set_servo_pos(ID, pos, ms):  # ID为舵机编号，pos为舵机目标位置，ms为运行时间，单位毫秒
    if pos < 0 or pos > 1000:
        print("position set error!")
        return 0
    if ID < 0 or ID > 6:
        print("servo set error!")
        return 0
    pos = int(pos)
    serial_device.ser_arm.write("\x55\x55".encode('utf-8'))  # 帧头
    serial_device.ser_arm.write(chr(8).encode('utf-8'))  # 指令长度
    serial_device.ser_arm.write(chr(3).encode('utf-8'))  # 指令值：设定舵机目标位置与执行时间
    serial_device.ser_arm.write(chr(1).encode('utf-8'))  # 需控制的舵机数量
    serial_device.ser_arm.write((ms % 256).to_bytes(1, 'big'))  # 时间低八位，转换成字节
    serial_device.ser_arm.write((ms // 256).to_bytes(1, 'big'))  # 时间高八位，转换成字节
    serial_device.ser_arm.write(chr(ID).encode('utf-8'))  # 舵机编号
    serial_device.ser_arm.write((pos % 256).to_bytes(1, 'big'))  # 位置低八位
    serial_device.ser_arm.write((pos // 256).to_bytes(1, 'big'))  # 位置高八位
    # ser_arm.close()                                 #关闭串口
    return


def set_servo_pos_group(p1, p2, p3, p4, p5, p6):
    set_servo_pos(1, p1, 1500)
    set_servo_pos(2, p2, 1500)
    set_servo_pos(3, p3, 1500)
    set_servo_pos(4, p4, 1500)
    set_servo_pos(5, p5, 1500)
    set_servo_pos(6, p6, 1500)


# print("调整舵机角度，准备扫码")
# set_servo_pos_group(500, 500, 500, 500, 500, 500)

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)

# 扫码
print("1.等待’扫码‘指令")
serial_device.wait_vehicle_msg(b'QR')
print("1.开始扫描二维码")

print("调整舵机角度，准备拍照")
set_servo_pos_group(0, 524, 1000, 706, 99, 39)

# 扫二维码读取颜色
color_list = []
cap = cv2.VideoCapture(0)
camera_offset_para = 0  # 摄像头偏角修正值，只对位置偏差起作用


# 获取颜色
def cam_find_color(color: str):  # 返回值：x偏移，y偏移
    # cap = cv2.VideoCapture(0)
    try:
        cv2.destroyWindow('edge')  # 切换窗口，窗口太多影响树莓派性能
    except Exception as e:
        pass
    rect, img_color = cap.read()
    hsv = cv2.cvtColor(img_color, cv2.COLOR_BGR2HSV)
    if color == 'red':
        lower_range = np.array([0, 43, 146], dtype=np.uint8)
        upper_range = np.array([10, 255, 255], dtype=np.uint8)
        mask1 = cv2.inRange(hsv, lower_range, upper_range)
        lower_range = np.array([156, 43, 146], dtype=np.uint8)
        upper_range = np.array([180, 255, 255], dtype=np.uint8)
        mask2 = cv2.inRange(hsv, lower_range, upper_range)
        mask = cv2.add(mask1, mask2)
    elif color == 'green':
        lower_range = np.array([35, 53, 46], dtype=np.uint8)
        upper_range = np.array([77, 255, 255], dtype=np.uint8)
        mask = cv2.inRange(hsv, lower_range, upper_range)
    elif color == 'blue':
        lower_range = np.array([100, 53, 100], dtype=np.uint8)
        upper_range = np.array([124, 255, 255], dtype=np.uint8)
        mask = cv2.inRange(hsv, lower_range, upper_range)
    elif color == 'orange':
        lower_range = np.array([11, 153, 100], dtype=np.uint8)
        upper_range = np.array([40, 255, 255], dtype=np.uint8)
        mask = cv2.inRange(hsv, lower_range, upper_range)
    else:
        print('camera return: bad command')
        return False
    # print("mask:", mask)
    img_temp, contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    cv2.line(img_color, (320, 0), (320, 480), (0, 255, 0), 2)
    cv2.line(img_color, (0, 240), (640, 240), (0, 255, 0), 2)
    if len(contours) > 0:
        c = max(contours, key=cv2.contourArea)
        M = cv2.moments(c)
        if cv2.contourArea(c) > 100:
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            x_offset = cx - 320 + camera_offset_para
            y_offset = 240 - cy
            cv2.line(img_color, (cx - 20, cy), (cx + 20, cy), (255, 0, 0), 2)
            cv2.line(img_color, (cx, cy - 20), (cx, cy + 20), (255, 0, 0), 2)
            x, y, w, h = cv2.boundingRect(c)
            brcnt = np.array([[[x, y]], [[x + w, y]], [[x + w, y + h]], [[x, y + h]]])
            cv2.drawContours(img_color, [brcnt], -1, (255, 0, 0), 2)
            # cv2.imshow('color', img_color)
            return [x_offset, y_offset]
    # cv2.imshow('color', img_color)
    return False


myData = ""
n = -10
while True:
    set_servo_pos_group(0, 524, 1000, 706, 99, 39 + n * 2)
    n += 1
    print(n)
    time.sleep(1)
    success, img = cap.read()
    for barcode in decode(img):
        # print(barcode.data)
        # myData = str(barcode.data).encode('raw_unicode_escape').decode()
        myData = barcode.data.decode("UTF8")
        print(myData)
        if myData:
            break
    if myData:
        break
if "郤｢濶ｲ" in myData:
    # print("红色")
    color_list.append("红色")
if "扈ｿ濶ｲ" in myData:
    # print("绿色")
    color_list.append("绿色")
if "闢晁牡" in myData:
    # print("蓝色")
    color_list.append("蓝色")

# 处理二维码数据并向服务器发起请求
myData = myData.replace("闢晁牡", "蓝色")
myData = myData.replace("扈ｿ濶ｲ", "绿色")
myData = myData.replace("郤｢濶ｲ", "红色")

url = "http://train.liuzhehao.com/api/update-string.php?STRING=" + myData

payload = {}
headers = {}

response = requests.request("GET", url, headers=headers, data=payload)

print(response.text)

# 二维码扫描完成
serial_device.send_to_vehicle("\x02\x0a")
print("1.二维码扫描完成", color_list)

print("调整舵机角度，准备抓取")
set_servo_pos_group(500, 500, 500, 500, 500, 500)

# # 抓取A区域东西
print("2.等待’抓取‘指令")
serial_device.wait_vehicle_msg(b"GRAB")
# print("2.执行抓取并把物品放到车上的操作")
time.sleep(1)
print("准备读取颜色")
# 查颜色在图片中的位置(无指定颜色返回False)
trans_dict = {
    "红色": "red",
    "蓝色": "blue",
    "绿色": "green"
}
put_time = 0

# 扫左
set_servo_pos_group(0, 500, 607, 627, 160, 619)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
color_position = cam_find_color(trans_dict[color_list[0]])
print(color_position)
print(type(color_position))
if color_position:
    print("确认" + trans_dict[color_list[0]] + "颜色在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 500, 639, 640, 125, 630)  # 抓左1-准备
    time.sleep(2)
    set_servo_pos_group(200, 500, 639, 640, 125, 630)  # 抓左1
color_position = cam_find_color(trans_dict[color_list[1]])
if color_position:
    print("确认" + trans_dict[color_list[1]] + "颜色在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 500, 639, 640, 125, 630)  # 抓左1-准备
    time.sleep(2)
    set_servo_pos_group(200, 500, 639, 640, 125, 630)  # 抓左1
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
if color_position:
    time.sleep(3)
    set_servo_pos_group(200, 500, 500, 500, 500, 500)
    if put_time == 0:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
        time.sleep(5)
    else:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
        time.sleep(5)
    put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
time.sleep(3)
# 扫中
set_servo_pos_group(0, 491, 793, 856, 202, 515)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
color_position = cam_find_color(trans_dict[color_list[0]])
print(color_position)
print(type(color_position))
if color_position:
    print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 491, 793, 856, 202, 515)
    time.sleep(2)
    set_servo_pos_group(200, 491, 793, 856, 202, 515)
color_position = cam_find_color(trans_dict[color_list[1]])
if color_position:
    print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 491, 793, 856, 202, 515)
    time.sleep(2)
    set_servo_pos_group(200, 491, 793, 856, 202, 515)
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
if color_position:
    time.sleep(3)
    set_servo_pos_group(200, 500, 500, 500, 500, 500)
    if put_time == 0:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
        time.sleep(5)
    else:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
        time.sleep(5)
    put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
time.sleep(3)
# 扫右
set_servo_pos_group(0, 532, 708, 679, 73, 395)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
color_position = cam_find_color(trans_dict[color_list[0]])
print(color_position)
print(type(color_position))
if color_position:
    print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 532, 708, 679, 73, 395)
    time.sleep(2)
    set_servo_pos_group(200, 532, 708, 679, 73, 395)
color_position = cam_find_color(trans_dict[color_list[1]])
if color_position:
    print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 532, 708, 679, 73, 395)
    time.sleep(2)
    set_servo_pos_group(200, 532, 708, 679, 73, 395)
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
if color_position:
    time.sleep(3)
    set_servo_pos_group(200, 500, 500, 500, 500, 500)
    if put_time == 0:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
        time.sleep(5)
    else:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
        time.sleep(5)
    put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
serial_device.send_to_vehicle("\x04\x0a")
print("2.结束抓取")

# # 抓取B区域东西
print("3.等待’抓取‘指令")
serial_device.wait_vehicle_msg(b"GRAB")
# print("2.执行抓取并把物品放到车上的操作")
time.sleep(1)
print("准备读取颜色")
# 查颜色在图片中的位置(无指定颜色返回False)
trans_dict = {
    "红色": "red",
    "蓝色": "blue",
    "绿色": "green"
}
put_time = 0

# 扫左
set_servo_pos_group(0, 500, 607, 627, 160, 619)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
color_position = cam_find_color(trans_dict[color_list[0]])
print(color_position)
print(type(color_position))
if color_position:
    print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 500, 639, 640, 125, 630)  # 抓左1-准备
    time.sleep(2)
    set_servo_pos_group(200, 500, 639, 640, 125, 630)  # 抓左1
color_position = cam_find_color(trans_dict[color_list[1]])
if color_position:
    print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 500, 639, 640, 125, 630)  # 抓左1-准备
    time.sleep(2)
    set_servo_pos_group(200, 500, 639, 640, 125, 630)  # 抓左1
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
if color_position:
    time.sleep(3)
    set_servo_pos_group(200, 500, 500, 500, 500, 500)
    if put_time == 0:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
        time.sleep(5)
    else:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
        time.sleep(5)
    put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
time.sleep(3)
# 扫中
set_servo_pos_group(0, 491, 793, 856, 202, 515)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
color_position = cam_find_color(trans_dict[color_list[0]])
print(color_position)
print(type(color_position))
if color_position:
    print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 491, 793, 856, 202, 515)
    time.sleep(2)
    set_servo_pos_group(200, 491, 793, 856, 202, 515)
color_position = cam_find_color(trans_dict[color_list[1]])
if color_position:
    print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 491, 793, 856, 202, 515)
    time.sleep(2)
    set_servo_pos_group(200, 491, 793, 856, 202, 515)
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
if color_position:
    time.sleep(3)
    set_servo_pos_group(200, 500, 500, 500, 500, 500)
    if put_time == 0:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
        time.sleep(5)
    else:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
        time.sleep(5)
    put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
time.sleep(3)
# 扫右
set_servo_pos_group(0, 532, 708, 679, 73, 395)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
color_position = cam_find_color(trans_dict[color_list[0]])
print(color_position)
print(type(color_position))
if color_position:
    print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 532, 708, 679, 73, 395)
    time.sleep(2)
    set_servo_pos_group(200, 532, 708, 679, 73, 395)
color_position = cam_find_color(trans_dict[color_list[1]])
if color_position:
    print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
    set_servo_pos_group(0, 532, 708, 679, 73, 395)
    time.sleep(2)
    set_servo_pos_group(200, 532, 708, 679, 73, 395)
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
if color_position:
    time.sleep(3)
    set_servo_pos_group(200, 500, 500, 500, 500, 500)
    if put_time == 0:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
        time.sleep(5)
    else:
        print("放置")
        time.sleep(5)
        set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
        time.sleep(5)
        set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
        time.sleep(5)
    put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
serial_device.send_to_vehicle("\x04\x0a")
print("3.结束抓取")


# 第三次抓取
# # 抓取C区域东西
print("4.等待’抓取‘指令")
serial_device.wait_vehicle_msg(b"GRAB")
# print("2.执行抓取并把物品放到车上的操作")
# time.sleep(1)
# print("准备读取颜色")
# # 查颜色在图片中的位置(无指定颜色返回False)
# trans_dict = {
#     "红色": "red",
#     "蓝色": "blue",
#     "绿色": "green"
# }
put_time = 0

# 扫左
set_servo_pos_group(0, 500, 607, 627, 160, 619)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
# color_position = cam_find_color(trans_dict[color_list[0]])
# print(color_position)
# print(type(color_position))
# if color_position:
#     print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
set_servo_pos_group(0, 500, 639, 640, 125, 630)  # 抓左1-准备
time.sleep(2)
set_servo_pos_group(200, 500, 639, 640, 125, 630)  # 抓左1
# color_position = cam_find_color(trans_dict[color_list[1]])
# if color_position:
#     print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
# set_servo_pos_group(0, 500, 639, 640, 125, 630)  # 抓左1-准备
# time.sleep(2)
# set_servo_pos_group(200, 500, 639, 640, 125, 630)  # 抓左1
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
# if color_position:
time.sleep(3)
set_servo_pos_group(200, 500, 500, 500, 500, 500)
if put_time == 0:
    print("放置")
    time.sleep(5)
    set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
    time.sleep(5)
    set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
    time.sleep(5)
else:
    print("放置")
    time.sleep(5)
    set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
    time.sleep(5)
    set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
    time.sleep(5)
put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
time.sleep(3)
# 扫中
set_servo_pos_group(0, 491, 793, 856, 202, 515)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
# color_position = cam_find_color(trans_dict[color_list[0]])
# print(color_position)
# print(type(color_position))
# if color_position:
#     print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
# set_servo_pos_group(0, 491, 793, 856, 202, 515)
# time.sleep(2)
set_servo_pos_group(200, 491, 793, 856, 202, 515)
# color_position = cam_find_color(trans_dict[color_list[1]])
# if color_position:
#     print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
#     set_servo_pos_group(0, 491, 793, 856, 202, 515)
#     time.sleep(2)
#     set_servo_pos_group(200, 491, 793, 856, 202, 515)
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
# if color_position:
time.sleep(3)
set_servo_pos_group(200, 500, 500, 500, 500, 500)
if put_time == 0:
    print("放置")
    time.sleep(5)
    set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
    time.sleep(5)
    set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
    time.sleep(5)
else:
    print("放置")
    time.sleep(5)
    set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
    time.sleep(5)
    set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
    time.sleep(5)
put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
time.sleep(3)
# 扫右
set_servo_pos_group(0, 532, 708, 679, 73, 395)
time.sleep(2)
# color_position = cam_find_color("red")
# print("颜色：", color_position)
# color_position = cam_find_color(trans_dict[color_list[0]])
# print(color_position)
# print(type(color_position))
# if color_position:
#     print("确认颜色" + trans_dict[color_list[0]] + "在需要抓取的列表里，进行抓取")
# set_servo_pos_group(0, 532, 708, 679, 73, 395)
# time.sleep(2)
set_servo_pos_group(200, 532, 708, 679, 73, 395)
# color_position = cam_find_color(trans_dict[color_list[1]])
# if color_position:
#     print("确认颜色" + trans_dict[color_list[1]] + "在需要抓取的列表里，进行抓取")
#     set_servo_pos_group(0, 532, 708, 679, 73, 395)
#     time.sleep(2)
#     set_servo_pos_group(200, 532, 708, 679, 73, 395)
# color_position = cam_find_color("")

# set_servo_pos_group(0, 500, 500, 500, 500, 500)
# if color_position:
time.sleep(3)
set_servo_pos_group(200, 500, 500, 500, 500, 500)
if put_time == 0:
    print("放置")
    time.sleep(5)
    set_servo_pos_group(200, 461, 278, 1000, 697, 1000)
    time.sleep(5)
    set_servo_pos_group(0, 461, 278, 1000, 697, 1000)
    time.sleep(5)
else:
    print("放置")
    time.sleep(5)
    set_servo_pos_group(200, 642, 288, 1000, 698, 1000)
    time.sleep(5)
    set_servo_pos_group(0, 642, 288, 1000, 698, 1000)
    time.sleep(5)
put_time += 1

# 初始化
set_servo_pos_group(0, 500, 500, 500, 500, 500)
serial_device.send_to_vehicle("\x04\x0a")
print("4.结束抓取")



# # serial_device.wait_vehicle_msg("QR")
#
# # 到点拼装
# print("3.等待’到点拼装‘指令")
# serial_device.wait_vehicle_msg(b"ASSEMBLE")
# print("3.执行拼装的操作")
# time.sleep(2)
# serial_device.send_to_vehicle("\x04\x0a")
# print("3.结束拼装")


# 等等小车的消息（起一个多线程的样例）
# procession_thread = threading.Thread(target=serial_device.wait_vehicle_msg, args=("QR", ))
# procession_thread.start()


# print("Car Raspberry Pi Program Over")


# set_servo_pos_group(0, 500, 675, 821, 284, 500)  # 扫中


# 设置机械臂指定舵机位置
# set_servo_pos(ID, pos, ms)  # ID为舵机编号，pos为舵机目标位置，ms为运行时间，单位毫秒