import machine
import time
import DRV8833


LEFT = 0
MID = 1
RIGHT = 2

# V1.43


class Car:
    def __init__(self, s1, s2, s3, m1, m2):  # 3个循迹传感器；  2个马达
        self.sensor_left = machine.ADC(machine.Pin(s1))  # 2
        self.sensor_left.atten(machine.ADC.ATTN_11DB)

        self.sensor_middle = machine.ADC(machine.Pin(s2))  # 1
        self.sensor_middle.atten(machine.ADC.ATTN_11DB)

        self.sensor_right = machine.ADC(machine.Pin(s3))  # 10
        self.sensor_right.atten(machine.ADC.ATTN_11DB)

        self.motor = DRV8833.MOTOR()
        self.motor_left = m1  # P5 = 0
        self.motor_right = m2  # P6 = 1

        # self.last_Offset = 0  #上一次偏差值
        self.outline_pb = 1  # 传感器全白的时候偏的方向，-1左，1右

        self.DBlackcnt = 0  # 传感器全黑的时候计时
        self.DWhitecnt = 0  # 传感器全黑的时候计时
        self.Crosscounts = 0  # 十字口计数器
        self.CrossState = 0  # 当前状态0表示不在十字口，1表示正在十字口

        self.kp = 18  # 比例增益
        self.ki = 0  # 积分增益
        self.kd = 12  # 微分增益
        self.prev_error = 0  # 上一次误差
        self.integral = 0  # 累积误差

        self.left_spd = 40
        self.right_spd = 40

        self.left_dir_t = self.motor.MOT_CW
        self.left_dir_r = self.motor.MOT_CCW

        self.right_dir_t = self.motor.MOT_CCW
        self.right_dir_r = self.motor.MOT_CW

        self.sensor_sep = [30000, 30000, 30000]  # 区分黑白的中间值,0左，1中，2右

    def SetPower(self, dir1, pwd1, dir2, pwd2):
        self.left_spd = pwd1
        self.right_spd = pwd2

        if dir1 == 0:
            self.left_dir_t = self.motor.MOT_CW
            self.left_dir_r = self.motor.MOT_CCW
        else:
            self.left_dir_t = self.motor.MOT_CCW
            self.left_dir_r = self.motor.MOT_CW

        if dir2 == 0:
            self.right_dir_t = self.motor.MOT_CW
            self.right_dir_r = self.motor.MOT_CCW
        else:
            self.right_dir_t = self.motor.MOT_CCW
            self.right_dir_r = self.motor.MOT_CW

    def SetPID(self, P, I, D):
        self.kp = P  # 比例增益
        self.ki = I  # 积分增益
        self.kd = D  # 微分增益

    def SetThreshold(
        self, t_left, t_mid, t_right
    ):  # 手动设置传感器阈值，并关闭自适应阀值
        self.sensor_sep[LEFT] = t_left
        self.sensor_sep[MID] = t_mid
        self.sensor_sep[RIGHT] = t_right

    def Set_Adaptive_threshold(self):  # 开启自适应阀值功能
        sen_left, sen_middle, sen_right = self.GetSensor()
        tsen_max = max(sen_left, sen_middle, sen_right)
        tsen_min = min(sen_left, sen_middle, sen_right)
        tsen_sep = (tsen_min + tsen_max) / 2 * 1.15

        self.sensor_sep[LEFT] = tsen_sep
        self.sensor_sep[MID] = tsen_sep
        self.sensor_sep[RIGHT] = tsen_sep

    def Set_Adaptive_threshold_twice(self, style):  # 开启自适应阀值功能
        if style == "max":
            sen_left, sen_middle, sen_right = self.GetSensor()
            self.max = [sen_left, sen_middle, sen_right]
        elif style == "min":
            sen_left, sen_middle, sen_right = self.GetSensor()
            self.min = [sen_left, sen_middle, sen_right]
        elif style == "set":
            self.sensor_sep[LEFT] = (self.max[0] + self.min[0]) / 2
            self.sensor_sep[MID] = (self.max[1] + self.min[1]) / 2
            self.sensor_sep[RIGHT] = (self.max[2] + self.min[2]) / 2

    def GetSensor(self):  # 获取左中右三个传感器的数据
        return (
            self.sensor_left.read_u16(),
            self.sensor_middle.read_u16(),
            self.sensor_right.read_u16(),
        )

    def Car_drive(self, Steering):  # Steering  -100偏左  - 100偏右
        blindzone = 0
        if Steering >= 0 and Steering < 50:
            self.motor.dc_motor(
                self.motor_left, self.left_dir_t, int(self.left_spd)
            )  # 左马达
            self.motor.dc_motor(
                self.motor_right,
                self.right_dir_t,
                int(self.right_spd - (self.right_spd - blindzone) * Steering / 50),
            )  # 右马达

        elif Steering >= 50:
            self.motor.dc_motor(
                self.motor_left, self.left_dir_t, int(self.left_spd)
            )  # 左马达
            self.motor.dc_motor(
                self.motor_right,
                self.right_dir_r,
                int(blindzone + (self.right_spd - blindzone) * (Steering - 50) / 50),
            )  # 右马达

        elif Steering < 0 and Steering > -50:
            self.motor.dc_motor(
                self.motor_left,
                self.left_dir_t,
                int(self.left_spd - (self.left_spd - blindzone) * (-Steering) / 50),
            )  # 左马达
            self.motor.dc_motor(
                self.motor_right, self.right_dir_t, int(self.right_spd)
            )  # 右马达
        elif Steering <= -50:
            self.motor.dc_motor(
                self.motor_left,
                self.left_dir_r,
                int(blindzone + (self.left_spd - blindzone) * (-Steering - 50) / 50),
            )  # 左马达
            self.motor.dc_motor(
                self.motor_right, self.right_dir_t, int(self.right_spd)
            )  # 右马达

    def Car_Toward(self):  # 不循迹前进
        self.motor.dc_motor(
            self.motor_left, self.left_dir_t, int(self.left_spd)
        )  # 左马达
        self.motor.dc_motor(
            self.motor_right, self.right_dir_t, int(self.right_spd)
        )  # 右马达

    def Car_Reverse(self):  # 不循迹后退
        self.motor.dc_motor(
            self.motor_left, self.left_dir_r, int(self.left_spd)
        )  # 左马达
        self.motor.dc_motor(
            self.motor_right, self.right_dir_r, int(self.right_spd)
        )  # 右马达

    def Car_Stop(self):  # 快速刹车
        self.motor.dc_motor(self.motor_left, self.motor.MOT_P, 0)  # 左马达
        self.motor.dc_motor(self.motor_right, self.motor.MOT_P, 0)  # 右马达

    def Car_BlindLeft(self):  # 不循迹原地左转
        self.motor.dc_motor(
            self.motor_left, self.left_dir_r, int(self.left_spd)
        )  # 左马达
        self.motor.dc_motor(
            self.motor_right, self.right_dir_t, int(self.right_spd)
        )  # 右马达

    def Car_BlindRight(self):  # 不循迹原地右转
        self.motor.dc_motor(
            self.motor_left, self.left_dir_t, int(self.left_spd)
        )  # 左马达
        self.motor.dc_motor(
            self.motor_right, self.right_dir_r, int(self.right_spd)
        )  # 右马达

    def Car_BlindLeftToward(self):  # 不循迹前进左转
        self.motor.dc_motor(self.motor_left, self.motor.MOT_P, 0)  # 左马达
        self.motor.dc_motor(
            self.motor_right, self.right_dir_t, int(self.right_spd)
        )  # 右马达

    def Car_BlindRightToward(self):  # 不循迹前进右转
        self.motor.dc_motor(
            self.motor_left, self.left_dir_t, int(self.left_spd)
        )  # 左马达
        self.motor.dc_motor(self.motor_right, self.motor.MOT_P, 0)  # 右马达

    def Car_BlindLeftReverse(self):  # 不循迹倒退左转
        self.motor.dc_motor(
            self.motor_left, self.left_dir_r, int(self.left_spd)
        )  # 左马达
        self.motor.dc_motor(self.motor_right, self.motor.MOT_P, 0)  # 右马达

    def Car_BlindRightReverse(self):  # 不循迹倒退右转
        self.motor.dc_motor(self.motor_left, self.motor.MOT_P, 0)  # 左马达
        self.motor.dc_motor(
            self.motor_right, self.right_dir_r, int(self.right_spd)
        )  # 右马达

    def Car_TurnLeft(self):  # 循迹左转
        sen_left, sen_middle, sen_right = self.GetSensor()
        while sen_left < self.sensor_sep[LEFT] and sen_right < self.sensor_sep[RIGHT]:
            self.Car_Toward()
            sen_left, sen_middle, sen_right = self.GetSensor()

        self.Car_BlindLeftToward()
        time.sleep(0.1)

        if sen_left >= self.sensor_sep[LEFT]:  # 左侧传感器先检测到白线

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_left > self.sensor_sep[LEFT]:  # 左转直到 左传感器测移出白线
                self.Car_BlindLeftToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            time.sleep(0.1)

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_left < self.sensor_sep[LEFT]:  # 左转直到 左传感器测移出黑线
                self.Car_BlindLeftToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            """
            if (sen_middle <= self.sensor_sep[MID]): # 中间传感器在线上（最常规的情况）
            
                sen_left, sen_middle, sen_right = self.GetSensor()
            
                while (sen_left > self.sensor_sep[LEFT]): # 左转直到 左传感器测移出白线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                time.sleep(0.1)
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_left < self.sensor_sep[LEFT]): # 左转直到 左传感器测移出黑线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
            else: # 中间传感器在线外（最麻烦的一种情况）
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_right < self.sensor_sep[RIGHT]): # 左转直到 右传感器测移出黑线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                time.sleep(0.1)
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_right > self.sensor_sep[RIGHT]): # 左转直到 右传感器测移出白线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                time.sleep(0.1)
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_right < self.sensor_sep[RIGHT]): # 左转直到 右传感器测移出黑线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_right > self.sensor_sep[RIGHT]): # 左转直到 右传感器测移出白线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()    
            """

        else:  # 右侧传感器先检测到白线，不管线内还是线外
            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_left < self.sensor_sep[LEFT]:  # 左转直到 左传感器测移出黑线
                self.Car_BlindLeftToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            time.sleep(0.1)

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_left > self.sensor_sep[LEFT]:  # 左转直到 左传感器测移出白线
                self.Car_BlindLeftToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            time.sleep(0.1)

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_left < self.sensor_sep[LEFT]:  # 左转直到 左传感器测移出黑线
                self.Car_BlindLeftToward()
                sen_left, sen_middle, sen_right = self.GetSensor()

        self.outline_pb = -1
        self.Car_Stop()
        time.sleep(0.1)

    def Car_TurnRight(self):  # 循迹右转
        sen_left, sen_middle, sen_right = self.GetSensor()
        while sen_right < self.sensor_sep[RIGHT] and sen_left < self.sensor_sep[LEFT]:
            self.Car_Toward()
            sen_left, sen_middle, sen_right = self.GetSensor()

        self.Car_BlindRightToward()
        time.sleep(0.1)

        if sen_right >= self.sensor_sep[RIGHT]:  # 右侧传感器先检测到白线

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_right > self.sensor_sep[RIGHT]:  # 右转直到 右传感器测移出白线
                self.Car_BlindRightToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            time.sleep(0.1)

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_right < self.sensor_sep[RIGHT]:  # 右转直到 右传感器测移出黑线
                self.Car_BlindRightToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            """
            if (sen_middle <= self.sensor_sep[MID]): # 中间传感器在线上（最常规的情况）
            
                sen_left, sen_middle, sen_right = self.GetSensor()
            
                while (sen_right > self.sensor_sep[RIGHT]): # 右转直到 右传感器测移出白线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                time.sleep(0.1)
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_right < self.sensor_sep[RIGHT]): # 右转直到 右传感器测移出黑线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
            else: # 中间传感器在线外（最麻烦的一种情况）
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_left < self.sensor_sep[LEFT]): # 右转直到 左传感器测移出黑线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                time.sleep(0.1)
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_left > self.sensor_sep[LEFT]): # 右转直到 左传感器测移出白线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                time.sleep(0.1)
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_left < self.sensor_sep[LEFT]): # 右转直到 左传感器测移出黑线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
                    
                sen_left, sen_middle, sen_right = self.GetSensor()
                while (sen_left > self.sensor_sep[LEFT]): # 右转直到 左传感器测移出白线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()    
            """

        else:  # 左侧传感器先检测到白线，不管线内还是线外
            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_right < self.sensor_sep[RIGHT]:  # 右转直到 右传感器测移出黑线
                self.Car_BlindRightToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            time.sleep(0.1)

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_right > self.sensor_sep[RIGHT]:  # 右转直到 右传感器测移出白线
                self.Car_BlindRightToward()
                sen_left, sen_middle, sen_right = self.GetSensor()
            time.sleep(0.1)

            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_right < self.sensor_sep[RIGHT]:  # 右转直到 右传感器测移出黑线
                self.Car_BlindRightToward()
                sen_left, sen_middle, sen_right = self.GetSensor()

        self.outline_pb = 1
        self.Car_Stop()
        time.sleep(0.1)

    def Car_TurnAround(self):
        # 掉头  左侧掉头
        self.Car_BlindLeft()  # 先转0.1秒
        time.sleep(0.1)
        sen_left, sen_middle, sen_right = self.GetSensor()
        while sen_left < self.sensor_sep[LEFT]:  # 左转直到 中间传感器测移出黑线
            self.Car_BlindLeft()
            sen_left, sen_middle, sen_right = self.GetSensor()

        while sen_left > self.sensor_sep[LEFT]:  # 左转直到 中间传感器测移出白线
            self.Car_BlindLeft()
            sen_left, sen_middle, sen_right = self.GetSensor()

        self.Car_Stop()
        time.sleep(0.1)

    def Car_TurnAround_Right_Advanced(self,times=0): 
        """
        适用于仅左侧存在黑色背景干扰，从右侧调头
        """
        time.sleep(0.1)
        sen_left, sen_middle, sen_right = self.GetSensor()

        # 右转直到双黑
        while sen_left > self.sensor_sep[LEFT] or sen_right > self.sensor_sep[RIGHT]:
            self.Car_BlindRight()
            sen_left, sen_middle, sen_right = self.GetSensor()
        if times != 0:
            self.Car_Stop()
            time.sleep(times)
        # 右转直到双白
        while sen_left < self.sensor_sep[LEFT] or sen_right < self.sensor_sep[RIGHT]:
            self.Car_BlindRight()
            sen_left, sen_middle, sen_right = self.GetSensor()
        if times != 0:
            self.Car_Stop()
            time.sleep(times)
        # # 右转直到左侧传感器测到白线，且右侧传感器测到黑线
        # while sen_left < self.sensor_sep[LEFT] or sen_right > self.sensor_sep[RIGHT]:
        #     self.Car_BlindRight()
        #     sen_left, sen_middle, sen_right = self.GetSensor()

        # 右转直到左侧传感器测到黑线，且右侧传感器测到白线
        while sen_left > self.sensor_sep[LEFT] or sen_right < self.sensor_sep[RIGHT]:
            self.Car_BlindRight()
            sen_left, sen_middle, sen_right = self.GetSensor()
        
        if times != 0:
            self.Car_Stop()
            time.sleep(times)
        time.sleep(0.1)

    def Car_TurnAround_Left_Advanced(self,times=0):  # 掉头  左侧掉头
        """
        适用于周围都是黑线的情况，从左侧调头
        """
        time.sleep(0.1)
        sen_left, sen_middle, sen_right = self.GetSensor()

        # 左转直到双黑
        while sen_left > self.sensor_sep[LEFT] or sen_right > self.sensor_sep[RIGHT]:
            self.Car_BlindLeft()
            sen_left, sen_middle, sen_right = self.GetSensor()
        if times != 0:
            self.Car_Stop()
            time.sleep(times)
        # 左转直到双白
        while sen_left < self.sensor_sep[LEFT] or sen_right < self.sensor_sep[RIGHT]:
            self.Car_BlindLeft()
            sen_left, sen_middle, sen_right = self.GetSensor()
        if times != 0:
            self.Car_Stop()
            time.sleep(times)
        # 左转直到左侧传感器测到黑线，且右侧传感器测到白线
        while sen_left > self.sensor_sep[LEFT] or sen_right < self.sensor_sep[RIGHT]:
            self.Car_BlindLeft()
            sen_left, sen_middle, sen_right = self.GetSensor()
        if times != 0:
            self.Car_Stop()
            time.sleep(times)
        self.Car_Stop()
        time.sleep(0.1)

    def Car_LineLeft(self):  # 线上左转
        self.Car_BlindLeftReverse()
        time.sleep(0.25)
        sen_left, sen_middle, sen_right = self.GetSensor()
        while sen_left <= self.sensor_sep[LEFT]:  # 直到传感器测移出黑线
            self.Car_BlindLeftReverse()
            sen_left, sen_middle, sen_right = self.GetSensor()

        while sen_left > self.sensor_sep[LEFT]:  # 直到传感器测移出白线
            self.Car_BlindLeftReverse()
            sen_left, sen_middle, sen_right = self.GetSensor()

        while sen_left <= self.sensor_sep[LEFT]:  # 直到传感器测移出黑线 为了前进一点
            self.Car_BlindRightToward()
            sen_left, sen_middle, sen_right = self.GetSensor()

        for i in range(10):
            if sen_right > self.sensor_sep[RIGHT]:
                while sen_right > self.sensor_sep[RIGHT]:  # 直到传感器测移出白线
                    self.Car_BlindRightReverse()
                    sen_left, sen_middle, sen_right = self.GetSensor()
            else:
                while sen_right <= self.sensor_sep[RIGHT]:  # 直到传感器测移出黑线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()

            if i == 2:
                break

            if sen_left > self.sensor_sep[LEFT]:
                while sen_left > self.sensor_sep[LEFT]:  # 直到传感器测移出白线
                    self.Car_BlindLeftReverse()
                    sen_left, sen_middle, sen_right = self.GetSensor()
            else:
                while sen_left <= self.sensor_sep[LEFT]:  # 直到传感器测移出黑线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
        self.Car_Stop()
        time.sleep(0.1)

    def Car_LineRight(self):  # 线上右转
        self.Car_BlindRightReverse()
        time.sleep(0.25)
        sen_left, sen_middle, sen_right = self.GetSensor()
        while sen_right <= self.sensor_sep[RIGHT]:  # 直到传感器测移出黑线
            self.Car_BlindRightReverse()
            sen_left, sen_middle, sen_right = self.GetSensor()

        while sen_right > self.sensor_sep[RIGHT]:  # 直到传感器测移出白线
            self.Car_BlindRightReverse()
            sen_left, sen_middle, sen_right = self.GetSensor()

        while sen_right <= self.sensor_sep[RIGHT]:  # 直到传感器测移出黑线 为了前进一点
            self.Car_BlindLeftToward()
            sen_left, sen_middle, sen_right = self.GetSensor()

        for i in range(10):
            if sen_left > self.sensor_sep[LEFT]:
                while sen_left > self.sensor_sep[LEFT]:  # 直到传感器测移出白线
                    self.Car_BlindLeftReverse()
                    sen_left, sen_middle, sen_right = self.GetSensor()
            else:
                while sen_left <= self.sensor_sep[LEFT]:  # 直到传感器测移出黑线
                    self.Car_BlindRightToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()

            if i == 2:
                break

            if sen_right > self.sensor_sep[RIGHT]:
                while sen_right > self.sensor_sep[RIGHT]:  # 直到传感器测移出白线
                    self.Car_BlindRightReverse()
                    sen_left, sen_middle, sen_right = self.GetSensor()
            else:
                while sen_right <= self.sensor_sep[RIGHT]:  # 直到传感器测移出黑线
                    self.Car_BlindLeftToward()
                    sen_left, sen_middle, sen_right = self.GetSensor()
        self.Car_Stop()
        time.sleep(0.1)

    def diffspd(self, diff, t):  # mixly调用，用于差速绕障碍物
        self.Car_drive(diff)

        time.sleep(t)

        sen_left, sen_middle, sen_right = self.GetSensor()
        while sen_middle > self.sensor_sep[MID]:  # 直到传感器测移出白线
            sen_left, sen_middle, sen_right = self.GetSensor()
        time.sleep(0.05)
        while sen_middle < self.sensor_sep[MID]:  # 直到传感器测移出黑线
            sen_left, sen_middle, sen_right = self.GetSensor()
        self.Car_Stop()

        if diff >= 0:
            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_left > self.sensor_sep[LEFT]:  # 直到传感器测移出白线
                self.Car_BlindLeft()
                sen_left, sen_middle, sen_right = self.GetSensor()

            while sen_left < self.sensor_sep[LEFT]:  # 直到传感器测移出黑线
                self.Car_BlindLeft()
                sen_left, sen_middle, sen_right = self.GetSensor()
        else:
            sen_left, sen_middle, sen_right = self.GetSensor()
            while sen_right > self.sensor_sep[RIGHT]:  # 直到传感器测移出白线
                self.Car_BlindRight()
                sen_left, sen_middle, sen_right = self.GetSensor()

            while sen_right < self.sensor_sep[RIGHT]:  # 直到传感器测移出黑线
                self.Car_BlindRight()
                sen_left, sen_middle, sen_right = self.GetSensor()

        self.Car_Stop()
        time.sleep(0.1)

    def crossClear(self):  # 清除路口计数
        self.Crosscounts = 0  # 十字口计数器

    def getCrossCnt(self):  # 获取路口计数
        return self.Crosscounts

    def Offset_cal(self):  # -3 -2 -1  0  1  2  3
        result = 0
        sen_left, sen_middle, sen_right = self.GetSensor()

        if (
            sen_left > self.sensor_sep[LEFT]
            and sen_middle < self.sensor_sep[MID]
            and sen_right > self.sensor_sep[RIGHT]
        ):  # 白黑白
            result = 0
            self.DBlackcnt = 0
            self.DWhitecnt += 1
            if self.DWhitecnt > 500:
                self.CrossState = 0
        elif (
            sen_left < self.sensor_sep[LEFT]
            and sen_middle < self.sensor_sep[MID]
            and sen_right > self.sensor_sep[RIGHT]
        ):  # 黑黑白
            result = 1
            self.DBlackcnt = 0
            self.DWhitecnt += 1
            if self.DWhitecnt > 500:
                self.CrossState = 0
            self.outline_pb = 1
        elif (
            sen_left > self.sensor_sep[LEFT]
            and sen_middle < self.sensor_sep[MID]
            and sen_right < self.sensor_sep[RIGHT]
        ):  # 白黑黑
            result = -1
            self.outline_pb = -1
            self.DBlackcnt = 0
            self.DWhitecnt += 1
            if self.DWhitecnt > 500:
                self.CrossState = 0
        elif (
            sen_left < self.sensor_sep[LEFT]
            and sen_middle > self.sensor_sep[MID]
            and sen_right > self.sensor_sep[RIGHT]
        ):  # 黑白白
            result = 2
            self.outline_pb = 1
            self.DBlackcnt = 0
            self.DWhitecnt += 1
            if self.DWhitecnt > 500:
                self.CrossState = 0
        elif (
            sen_left > self.sensor_sep[LEFT]
            and sen_middle > self.sensor_sep[MID]
            and sen_right < self.sensor_sep[RIGHT]
        ):  # 白白黑
            result = -2
            self.outline_pb = -1
            self.DBlackcnt = 0
            self.DWhitecnt += 1
            if self.DWhitecnt > 500:
                self.CrossState = 0
        elif (
            sen_left > self.sensor_sep[LEFT]
            and sen_middle > self.sensor_sep[MID]
            and sen_right > self.sensor_sep[RIGHT]
        ):  # 白白白
            if self.outline_pb > 0:
                result = 3
            else:
                result = -3
            self.DBlackcnt = 0
            self.DWhitecnt += 1
            if self.DWhitecnt > 500:
                self.CrossState = 0
        elif (
            sen_left < self.sensor_sep[LEFT] and sen_right < self.sensor_sep[RIGHT]
        ):  # 黑x黑

            result = 0
            # result = self.last_Offset
            self.DBlackcnt += 1
            self.DWhitecnt = 0
            if ++self.DBlackcnt > 10:  # 双黑线计数+1
                if self.CrossState == 0:
                    self.Crosscounts += 1  # 十字口计数器
                self.CrossState = 1

        # self.last_Offset = result
        return result

    def PID_update(self, error):
        proportional = self.kp * error  # 计算比例项

        self.integral += error  # 计算积分项
        integral_term = self.ki * self.integral

        derivative = self.kd * (error - self.prev_error)  # 计算微分项

        control_output = proportional + integral_term + derivative  # 综合控制输出

        self.prev_error = error
        return control_output

    def Tracking(self):
        for _ in range(50):
            offset = self.Offset_cal()  # 获取当前偏置数据
            error = 0 - offset  # 计算偏差
            steering = self.PID_update(error)  # 使用PID控制器计算马达指令
            steering = max(-100, min(100, steering))  # 限制马达指令范围在-100到100之间
            self.Car_drive(steering)  # 控制小车驶向指定方向