176 lines
3.9 KiB
Python
176 lines
3.9 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
|
||
|
||
bus.write_byte_data(addr, 0x00, 0x00)
|
||
bus.write_byte_data(addr, 0xFE, 60)
|
||
|
||
# ======================
|
||
# 电机定义
|
||
# ======================
|
||
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)
|
||
|
||
# ======================
|
||
# 单电机控制
|
||
# ======================
|
||
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:
|
||
# 刹车
|
||
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 rotate_left(speed=0.6):
|
||
motor_drive("M1", -speed)
|
||
motor_drive("M2", -speed)
|
||
motor_drive("M3", -speed)
|
||
motor_drive("M4", -speed)
|
||
|
||
def rotate_right(speed=0.6):
|
||
motor_drive("M1", speed)
|
||
motor_drive("M2", speed)
|
||
motor_drive("M3", speed)
|
||
motor_drive("M4", speed)
|
||
|
||
# ======================
|
||
# 斜向移动
|
||
# ======================
|
||
def move_left_forward(speed=0.6):
|
||
motor_drive("M1", 0)
|
||
motor_drive("M2", speed)
|
||
motor_drive("M3", 0)
|
||
motor_drive("M4", -speed)
|
||
|
||
def move_right_forward(speed=0.6):
|
||
motor_drive("M1", speed)
|
||
motor_drive("M2", 0)
|
||
motor_drive("M3", -speed)
|
||
motor_drive("M4", 0)
|
||
|
||
def move_left_backward(speed=0.6):
|
||
motor_drive("M1", -speed)
|
||
motor_drive("M2", 0)
|
||
motor_drive("M3", speed)
|
||
motor_drive("M4", 0)
|
||
|
||
def move_right_backward(speed=0.6):
|
||
motor_drive("M1", 0)
|
||
motor_drive("M2", -speed)
|
||
motor_drive("M3", 0)
|
||
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__":
|
||
|
||
actions = [
|
||
("前进", forward),
|
||
("后退", backward),
|
||
("左移", move_left),
|
||
("右移", move_right),
|
||
("左前", move_left_forward),
|
||
("右前", move_right_forward),
|
||
("左后", move_left_backward),
|
||
("右后", move_right_backward),
|
||
("左旋", rotate_left),
|
||
("右旋", rotate_right),
|
||
]
|
||
|
||
for name, func in actions:
|
||
print(name)
|
||
func()
|
||
time.sleep(1)
|
||
stop()
|
||
time.sleep(2)
|
||
|
||
print("完成")
|