from smbus2 import SMBus
import time

bus = SMBus(1)
addr = 0x60

# ======================
# PCA9685 初始化
# ======================
bus.write_byte_data(addr, 0x00, 0x00)
bus.write_byte_data(addr, 0xFE, 60)

# ======================
# PWM输出（0~4095）
# ======================
def set_pwm(ch, val):
    val = max(0, min(4095, val))
    base = 0x06 + 4 * ch
    bus.write_byte_data(addr, base, 0)
    bus.write_byte_data(addr, base+1, 0)
    bus.write_byte_data(addr, base+2, val & 0xFF)
    bus.write_byte_data(addr, base+3, val >> 8)

# ======================
# ⭐ 你的真实映射（8通道版本）
# ======================
MOTORS = {
    "M1": {"in1": 0, "in2": 1},
    "M2": {"in1": 2, "in2": 3},
    "M3": {"in1": 4, "in2": 5},
    "M4": {"in1": 6, "in2": 7},
}

# ======================
# 电机控制（重点）
# ======================
def motor(name, speed=0, direction="stop"):
    m = MOTORS[name]

    # 速度限制
    speed = max(0, min(4095, speed))

    if direction == "f":
        set_pwm(m["in1"], speed)
        set_pwm(m["in2"], 0)

    elif direction == "b":
        set_pwm(m["in1"], 0)
        set_pwm(m["in2"], speed)

    else:
        set_pwm(m["in1"], 0)
        set_pwm(m["in2"], 0)

# ======================
# 四轮控制
# ======================
def all_motors(speed, direction):
    for m in MOTORS:
        motor(m, speed, direction)

def forward(speed=3500):
    all_motors(speed, "f")

def backward(speed=3500):
    all_motors(speed, "b")

def stop():
    all_motors(0, "stop")

# ======================
# 转向（差速）
# ======================
def turn_left(speed=3500):
    motor("M1", speed, "b")
    motor("M3", speed, "b")
    motor("M2", speed, "f")
    motor("M4", speed, "f")

def turn_right(speed=3500):
    motor("M1", speed, "f")
    motor("M3", speed, "f")
    motor("M2", speed, "b")
    motor("M4", speed, "b")

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

    print("前进")
    forward(3500)
    time.sleep(3)

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

    print("后退")
    backward(3500)
    time.sleep(3)

    print("停止")
    stop()