from smbus2 import SMBus 
import time 
import signal
import sys 

# 信号处理函数，用于捕获Ctrl+C
def signal_handler(sig, frame):
    print("\n接收到中断信号，正在停止电机...")
    stop()
    print("电机已停止，程序退出")
    sys.exit(0)

# 注册信号处理函数
signal.signal(signal.SIGINT, signal_handler)

# ====================== 
# PCA9685 初始化 
# ====================== 
bus = SMBus(1) 
addr = 0x60 

MODE1 = 0x00 
PRESCALE = 0xFE 

bus.write_byte_data(addr, MODE1, 0x00) 
bus.write_byte_data(addr, PRESCALE, 60) 

# ====================== 
# 电机通道映射（每个电机 IN1 / IN2） 
# ====================== 
MOTOR = { 
    "M1": (0, 1),   # 前左 
    "M2": (2, 3),   # 前右 
    "M3": (4, 5),   # 后左 
    "M4": (6, 7),   # 后右 
} 

# ====================== 
# 方向修正（麦克纳姆关键） 
# 根据你 / \ + \ / 结构校正 
# ====================== 
DIR = { 
    "M1": 1, 
    "M2": -1, 
    "M3": -1, 
    "M4": 1, 
} 

# ====================== 
# PWM输出函数 
# ====================== 
def set_pwm(ch, value): 
    value = max(0, min(4095, value)) 
    bus.write_byte_data(addr, 0x06 + 4 * ch, 0) 
    bus.write_byte_data(addr, 0x07 + 4 * ch, 0) 
    bus.write_byte_data(addr, 0x08 + 4 * ch, value & 0xFF) 
    bus.write_byte_data(addr, 0x09 + 4 * ch, value >> 8) 

# ====================== 
# 单电机控制 
# speed: -1 ~ 1 
# ====================== 
def motor_drive(name, speed): 
    in1, in2 = MOTOR[name] 

    speed *= DIR[name] 
    pwm = int(abs(speed) * 4095) 

    if speed > 0: 
        set_pwm(in1, pwm) 
        set_pwm(in2, 0) 

    elif speed < 0: 
        set_pwm(in1, 0) 
        set_pwm(in2, pwm) 

    else: 
        # 刹车（关键，不是0！） 
        set_pwm(in1, 4095) 
        set_pwm(in2, 4095) 

# ====================== 
# 基础运动控制 
# ====================== 
def forward(speed=0.6): 
    motor_drive("M1", speed) 
    motor_drive("M2", speed) 
    motor_drive("M3", speed) 
    motor_drive("M4", speed) 

def backward(speed=0.6): 
    motor_drive("M1", -speed) 
    motor_drive("M2", -speed) 
    motor_drive("M3", -speed) 
    motor_drive("M4", -speed) 

# ====================== 
# 麦克纳姆左右移动 
# ====================== 
def move_left(speed=0.6): 
    motor_drive("M1", -speed) 
    motor_drive("M2", speed) 
    motor_drive("M3", speed) 
    motor_drive("M4", -speed) 

def move_right(speed=0.6): 
    motor_drive("M1", speed) 
    motor_drive("M2", -speed) 
    motor_drive("M3", -speed) 
    motor_drive("M4", speed) 

# ====================== 
# 停止（刹车模式） 
# ====================== 
def stop(): 
    motor_drive("M1", 0) 
    motor_drive("M2", 0) 
    motor_drive("M3", 0) 
    motor_drive("M4", 0) 

# ====================== 
# 测试程序 
# ====================== 
if __name__ == "__main__": 

    print("前进") 
    forward(0.5) 
    time.sleep(1) 

    print("停止") 
    stop() 
    time.sleep(1) 

    print("后退") 
    backward(0.5) 
    time.sleep(1) 

    print("停止") 
    stop() 
    time.sleep(1) 

    print("停止") 
    stop()