144 lines
3.0 KiB
Python
144 lines
3.0 KiB
Python
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()
|