import sensor, image, time
from pid import PID
from pyb import Servo

# 初始化舵机
pan_servo = Servo(1)  # 水平方向
tilt_servo = Servo(2)  # 垂直方向

# 舵机校准
pan_servo.calibration(500, 2500, 500)
tilt_servo.calibration(500, 2500, 500)

# ✅ 深红色 LAB 阈值（适用于远处目标）
target_threshold = (20, 50, 25, 70, 10, 50)

# PID 控制器（减速，平稳）
pan_pid = PID(p=0.06, i=0, imax=90)
tilt_pid = PID(p=0.06, i=0, imax=90)

# 摄像头初始化
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()

# 找最大目标区域
def find_max(blobs):
    max_size = 0
    for blob in blobs:
        area = blob[2] * blob[3]
        if area > max_size:
            max_blob = blob
            max_size = area
    return max_blob

# 平滑回水平函数
def smooth_tilt_to_angle(target):
    current = tilt_servo.angle()
    step = 1 if current < target else -1
    for angle in range(current, target + step, step):
        tilt_servo.angle(angle)
        time.sleep_ms(20)


# 扫描相关变量
scan_angle = -90
scan_speed = 1
last_scan_time = time.ticks_ms()
scanning = True

# 扫描速度控制（左慢右快）
def get_scan_interval(angle):
    if angle < 0:
        return 60  # 左边慢
    else:
        return 30  # 右边快

# 锁定与恢复控制
last_seen_time = time.ticks_ms()
target_locked = False

# 主循环
while True:
    clock.tick()
    img = sensor.snapshot()
    blobs = img.find_blobs([target_threshold], pixels_threshold=10, area_threshold=10)

    if blobs:
        # === 进入锁定模式 ===
        scanning = False
        target_locked = True
        last_seen_time = time.ticks_ms()

        max_blob = find_max(blobs)
        cx = max_blob.cx()
        cy = max_blob.cy()

        pan_error = cx - img.width() // 2
        tilt_error = cy - img.height() // 2

        img.draw_rectangle(max_blob.rect())
        img.draw_cross(cx, cy)
        print("🎯 锁定目标中")

        # PID 控制输出（平滑减速）
        pan_output = pan_pid.get_pid(pan_error, 1) / 3
        tilt_output = tilt_pid.get_pid(tilt_error, 1) / 2

        # 舵机角度控制
        new_pan = min(max(pan_servo.angle() + pan_output, -90), 90)
        new_tilt = min(max(tilt_servo.angle() + tilt_output, -90), 90)

        pan_servo.angle(new_pan)
        tilt_servo.angle(new_tilt)

    else:
        # === 目标丢失超过3秒，恢复扫描 ===
        time_since_last_seen = time.ticks_diff(time.ticks_ms(), last_seen_time)
        if target_locked and time_since_last_seen > 3000:
            print("⚠️ 目标丢失超过3秒，进入扫描")
            scanning = True
            target_locked = False
            scan_angle = -90
            smooth_tilt_to_angle(90)  
 

        if scanning:
            current_time = time.ticks_ms()
            scan_interval = get_scan_interval(scan_angle)
            if time.ticks_diff(current_time, last_scan_time) > scan_interval:
                last_scan_time = current_time
                print("🔄 扫描中...角度:", scan_angle)
                pan_servo.angle(scan_angle)
                scan_angle += scan_speed
                if scan_angle > 90:
                    scan_angle = -90  # 模拟360度回扫