光电比赛小车寻宝【1】--循迹策略

2023光电比赛总结：主要功能实现了，就是视频没做好，落选了非常的遗憾，也有很多的不甘心，也可以作为最后摆烂的惩罚吧，在这里总结一下经验教训。整体感觉时间不是非常充分，因为得到比赛的消息后突然一阳了，拖了几乎俩星期才开始，感觉身体状态不是特别好，需要做的东西很多，当然有的同门已经开始做了，硬件选取还有软件的实现可以借鉴一下思路，当然最多的还是自己亲自动手去实现。困难是常在的，而方法是需要摸索的。

循迹部分：为什么开始说循迹部分，因为这一部分花了特别多的时间去处理，开始的时候感觉没那么困难，因为查阅了相关的实现案例，不管是二路传感器，三路传感器都是可以按照规定的黑线进行循迹效果也特别好，我们购买的一款车是麦克纳姆轮的小车，这一款小车自带的四路传感器，因为传感器的距离定死了，所以循迹中出现问题的情况就非常的多，那么下面我就来说说具体有哪些问题。

使用四路传感器进行循迹，可能出现需要微调的情况0我分为三种：小微调、中微调、大微调，这三种调整方式是调整移动方向、转的角度不同来定的，这样能保证更平稳的进行循迹。因为正常情况下走直线需要传感器2、3同时在线上即都为True的情况,而麦轮小车按照此控制方式行走不能走直线所以需要时刻微调来保证车身随时调正行走，这样提高循迹的精准度。

1、小微调：

当传感器2或者3在线上的时候，如图5所示，中间两个传感器，需要进行小微调，当传感器2压线其他传感器都没压线需要往传感器2的方向微调（移动为向2的斜方向移动、加上一个很小的角速度）保证更快的纠正车身，直到2、3同时压线即直行，当传感器3压线其他不压线同理。情况如下：

图6小微调展示图

2、中微调：

在循迹过程中，当黑线在传感器1、2之间或者3、4之间，这个时候所有的传感器都没有信号，那么这个时候需要进行中微调，线在哪边向哪个方向微调。（或者前进一点点距离即可触碰到传感器1或者4，那么即可进入下一个大微调阶段）在转弯的时候容易出现这样的情况。

图6中微调展示图

3、大微调：

当传感器1或4压线，这个时候也是向传感器的方向进行微调，这个时候同时斜向移动和提供角速度的移动就体现出优势，可以快速的进行矫正。在转弯的时候容易出现这样的情况。

图7大微调展示图

图8走直线展示图

4、具体做法：微调操作使用的方法是角度加方向一起作用，这样的效果能够更快的反应更有效地找到路径。因为麦克纳姆轮可以全方向移动，那么可以利用这个点来进行微调。这里使用多线程控制信号获取，会出现信号遗漏的情况所以我用到了一个巧妙的方法：可以看这篇文章如何解决传感器交互信息（双线程）遗漏信号的问题

5、代码实现：

#!/usr/bin/python3
# coding=utf8
# sudo python3 TurboPi/HiwonderSDK/FourInfrared.py
import smbus
import numpy
import sys
sys.path.append('/home/pi/TurboPi/')
import cv2
import time
import math
import signal
import Camera
import argparse
import threading
import numpy as np
import yaml_handle
import HiwonderSDK.PID as PID
import HiwonderSDK.Misc as Misc
import HiwonderSDK.Board as Board
import HiwonderSDK.mecanum as mecanum
import baoCamera
import CameraCalibration.image_recognition as image_recognition

#四路巡线传感器
__isRunning = True
car = mecanum.MecanumChassis()
res_path = image_recognition.image_run()
# baoCamera_run = baoCamera.run()
print(11)
# print("1111111",res_path)
# print("qqqqqqq",res_path[0])

#定义一个状态值
godown = False
num_right=0
num_left=0
numA=0
numB=0
is_turn = False
sensor_turn_threading=None
linear = 0
turn_lists = res_path
res_path_little=[]
turn_require = 0
turn_lists = ([1,1,2,2,2,4],[2,1,1,2,2,4],[0,2,1,2,2,4],[0,2,1,2,2,4],[0,2,1,2,2,4],[0,2,1,2,2,4],[0,2,1,2,2,4],[0,2,1,2,2,4],[0,2,1,2,2,4])
# print(len(turn_lists))
i=0
j=0
k=0
m=0
# while i< len(res_path):
#     res_path_little = res_path[i]
#     # print(res_path_little)
#     j = len(res_path_little)
#     if res_path[i][j-1] == 1:
#         turn_lists[i][j-1] = 3
#     elif res_path[i][j-1] == 2:
#         turn_lists[i][j-1] = 4
#     elif res_path[i][j-1] == 0:
#         turn_lists[i][j - 1] = 5
#     i += 1

print("turn_lists",turn_lists)
class FourInfrared:
    def __init__(self, address=0x78, bus=1):
        self.address = address
        self.bus = smbus.SMBus(bus)

    def readData(self, register=0x01):
        value = self.bus.read_byte_data(self.address, register)
        return [True if value & v > 0 else False for v in [0x01, 0x02, 0x04, 0x08]]

line = FourInfrared()
# 停
def car_stop():
    car.set_velocity(0,0,0)  # 关闭所有电机 Turn off all motors
#左偏移
def car_leftlittle():
    # car.set_velocity(50, 90, -0.13)
    car.set_velocity(linear, 98, -0.05)  # 左上
#右偏移
def car_rightlittle():
    # car.set_velocity(50, 90, 0.13)
    car.set_velocity(linear, 82, 0.05)  # 右上
#左中调
def car_leftmiddle():
    car.set_velocity(linear, 110, -0.05)  # 右上
#右中调
def car_rightmiddle():
    car.set_velocity(linear, 70, 0.05)  # 右上
#直走
def car_go():
    car.set_velocity(linear, 90, 0)
#直走一点点
def car_golittle():
    car.set_velocity(linear, 90, 0)
    time.sleep(0.1)
#右转
def car_right():
    global data
    is_turning =True
    while is_turning:
        car.set_velocity(0, 90, 0.4)
        if data[3]:
            is_turning=False
    is_turning=True
    # car.set_velocity(0, 90, 0.75)
    # time.sleep(0.65)


#左转
def car_left():
    global data
    is_turning = True
    while is_turning:
        car.set_velocity(0, 90, -0.4)
        if data[0]:
            is_turning = False
    is_turning = True
# 关闭前处理  Processing before exit
def manual_stop(signum, frame):
    global numA
    global numB
    global __isRunning
    print('关闭中...')
    __isRunning = False
    print(f"numA={numA},numB={numB}")
    car_stop()  # 关闭所有电机  Turn off all motors
def move():
    global num_right
    global num_left
    global data
    global __isRunning
    global numB
    global linear
    global k
    global m
    global turn_require
    while m<len(turn_lists):
        # print("turn_lists[m]", len(turn_lists[m]))
        turn_pop_len = len(turn_lists[m])
        # turn_pop = turn_lists[m]#坑都跟着变化
        while k<turn_pop_len:
            # turn_require = 0
            print("m",m)
            print("len(turn_lists[m])",len(turn_lists[m]))
            print("len(turn_lists[m])",turn_lists[m])
            if __isRunning:
                print("线速度",linear)
                print("turn_require",turn_require)
                if linear == 0:
                    # print("拐弯信号", data[0], data[1], data[2], data[3])
                    if turn_lists[m]==[]:
                        break
                    turn = turn_lists[m].pop(0)
                    print("turn:",turn)
                    # if turn==-1:
                    #     break
                    if turn == 1:
                        k += 1
                        print("k",k)
                        print("正在左转...")
                        car_stop()
                        time.sleep(1)
                        car_golittle()
                        # print("正在zuo转...")
                        car_stop()
                        time.sleep(0.3)
                        car_left()
                        car_stop()
                        time.sleep(1)
                        # if turn_lists[m][0]==5:
                        #     car.set_velocity(linear, 90, 0)
                        #     # time.sleep(0.3)
                        #     turn_require = 1
                        #     time.sleep(0.7)
                        # print("停左转")
                        # car_golittle()
                    elif turn == 2:
                        k += 1
                        print("正在右转...")
                        car_stop()
                        # print("正在you转...")
                        time.sleep(1)
                        car_golittle()
                        car_stop()
                        time.sleep(0.3)
                        car_right()
                        car_stop()
                        time.sleep(1)
                        # if turn_lists[m][0]==5:
                        #     car.set_velocity(linear, 90, 0)
                        #     turn_require = 1
                        #     time.sleep(0.7)
                        # print("停右转")
                    elif turn == 0:
                        k += 1
                        print("直行...")
                        car.set_velocity(linear, 90, 0)
                        time.sleep(1)
                        if turn_lists[m][0]==5:
                            car.set_velocity(linear, 90, 0)
                            turn_require = 1
                            time.sleep(0.7)
                    elif turn == 3:
                        k += 1
                        print("正在左转到宝藏...")
                        car_stop()
                        time.sleep(1)
                        car_golittle()
                        # print("正在zuo转...")
                        car_stop()
                        time.sleep(0.3)
                        car_left()
                        car_stop()
                        time.sleep(1)
                        # print("停左转")
                        # car_golittle()
                        baoCamera_run = baoCamera.run()
                        car_stop()
                        time.sleep(0.3)
                        if baoCamera_run == 1:
                            car.set_velocity(linear, 90, 0)
                            time.sleep(1.3)
                            car_left()
                            baoCamera_run == 0
                        else:
                            car.set_velocity(linear, 90, 0)
                            time.sleep(0.7)
                            car_right()

                    elif turn == 4:
                        k += 1
                        print("正在右转到宝藏...")
                        car_stop()
                        time.sleep(1)
                        car_golittle()
                        # print("正在zuo转...")
                        car_stop()
                        time.sleep(0.3)
                        car_right()
                        car_stop()
                        time.sleep(1)
                        # print("停左转")
                        # car_golittle()
                        baoCamera_run = baoCamera.run()
                        car_stop()
                        time.sleep(0.3)
                        if baoCamera_run == 1:
                            car.set_velocity(linear, 90, 0)
                            time.sleep(1.3)
                            car_left()
                            baoCamera_run == 0
                        else:
                            car.set_velocity(linear, 90, 0)
                            time.sleep(0.7)
                            car_right()
                    elif turn == 5:
                        # car.set_velocity(0, 0, 0)
                        k += 1
                        print("直行到宝藏...")
                        car.set_velocity(linear, 90, 0)
                        time.sleep(2)
                        turn_require == 0
                        baoCamera_run = baoCamera.run()
                        car_stop()
                        time.sleep(2)
                        if baoCamera_run == 1:
                            # turn_require == 0
                            print("是")
                            car.set_velocity(50, 90, 0)
                            time.sleep(2.5)
                            car_left()
                            car_stop()
                            time.sleep(0.6)
                            baoCamera_run == 0
                            turn_require = 0
                            time.sleep(0.7)
                            # break
                        else:
                            print("不是")
                            car.set_velocity(50, 90, 0)
                            time.sleep(0.7)
                            car_right()
                            car_stop()
                            # time.sleep(1)
                            # 如果等于0那么就不停
                            turn_require = 0
                            time.sleep(0.7)
                            # break
                        # turn_require == 0
                else:
                    if not data[0] and data[1] and data[2] and not data[3]:     #第2、3个传感器是true走
                        car_go()
                        # print("直走",data[0] , data[1],data[2] , data[3])
                    elif not data[0] and not data[1] and data[2] and not data[3]:  # 第2个传感器是false向右偏移
                        car_rightlittle()
                        # print("右偏移")
                        # print("右偏移信号",data[0] , data[1],data[2] , data[3])
                    elif not data[2] and not data[0] and not data[3] and data[1]:  # 第3个传感器是false向左偏移
                        car_leftlittle()
                        # print("左偏移信号",data[0] , data[1],data[2] , data[3])

                    #增加只有一个传感器在线上的情况
                    elif data[0] and not data[1] and not data[2] and not data[3]:  # 第1个传感器是true,前进一点，停，等待左转
                        car.set_velocity(linear, 110, -0.15)
                        # car.set_velocity(50, 90, -0.3)
                        # print("左偏移大微调...", data[0], data[1], data[2], data[3])
                    elif data[3] and not data[0] and not data[1] and not data[2]:  # 第1个传感器是true,前进一点，停，等待左转
                        car.set_velocity(linear, 70, 0.15)
                        # car.set_velocity(50, 90, 0.3)
                        # print("右偏移大微调...", data[0], data[1], data[2], data[3])
                    #增加线在第1、2或者3、4之间的情况
                    elif not data[0] and not data[1] and not data[2] and not data[3]:  # 所有传感器是false停
                        car_golittle()
            else:
                car_stop()
        # k=0
        m += 1

def sensor():
    global data
    global __isRunning
    global numA
    global linear
    global turn_require
    while __isRunning:
        data = line.readData()
        # print("转弯信号识别。。。",data[0],data[1],data[2],data[3])
        # print("turn_lende===================",turn_require)
        if ((data[0] and data[1]) or (data[2] and data[3]) or turn_require == 1):
            # print("停的信号：",data[0],data[1],data[2],data[3])
            linear = 0
            time.sleep(0.3)
        else:
            linear = 50
            # print("linear-----------------===================",linear)
# 运行子线程move,sensor

if __name__ == "__main__":
    global data
    data=[]
    # global __isRunning
    __isRunning = True
    # th3 = threading.Thread(target=sensor_turn)
    # th3.setDaemon(True)
    # th3.start()
    th2 = threading.Thread(target=sensor)
    th2.setDaemon(True)
    th2.start()
    th = threading.Thread(target=move)
    th.setDaemon(True)
    th.start()
    signal.signal(signal.SIGINT, manual_stop)
    while __isRunning:
        1
        # print("Sensor1:", data[0], " Sensor2:", data[1], " Sensor3:", data[2], " Sensor4:", data[3])
        # data = line.readData()
        #True表示识别到黑线，False表示没有识别到黑线
        # print("asdsad",line.readData()[0])
        # time.sleep(0.5)
        # move()