104 lines
2.1 KiB
Python
104 lines
2.1 KiB
Python
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() |