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()
|