344 lines
12 KiB
Python
344 lines
12 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
"""
|
|||
|
|
interactive_servo_test.py — 基于 Python 的 ESP32 舵机控制交互脚本
|
|||
|
|
依赖：pip install pyserial
|
|||
|
|
用法：
|
|||
|
|
python3 interactive_servo_test.py /dev/ttyUSB0 [baudrate]
|
|||
|
|
功能：
|
|||
|
|
1. 控制单个舵机
|
|||
|
|
2. 控制多个舵机
|
|||
|
|
3. 退出
|
|||
|
|
4. 自动增减模式：从 0 到 180° 每步 +6° → -6° 循环，50Hz 频率发送
|
|||
|
|
5. 设置自动模式舵机数量（通道 1~N）
|
|||
|
|
6. 眨眼模式：让五个舵机执行预设角度循环，模拟眨眼，8号舵机保持120度
|
|||
|
|
7. 打哈欠模式：模拟打哈欠动作
|
|||
|
|
8. 说话模式：模拟嘴巴开合动作
|
|||
|
|
"""
|
|||
|
|
import sys
|
|||
|
|
import time
|
|||
|
|
import serial
|
|||
|
|
import random
|
|||
|
|
|
|||
|
|
FRAME_HEADER = 0xAA # 帧头
|
|||
|
|
|
|||
|
|
def wakeup_esp32(port, baud=115200):
|
|||
|
|
"""通过 RTS/DTR 线复位并唤醒 ESP32"""
|
|||
|
|
try:
|
|||
|
|
ser = serial.Serial(port, baud, timeout=1)
|
|||
|
|
ser.setDTR(False)
|
|||
|
|
ser.setRTS(False)
|
|||
|
|
time.sleep(0.01)
|
|||
|
|
ser.setDTR(True)
|
|||
|
|
ser.setRTS(True)
|
|||
|
|
ser.write(b"\r\n")
|
|||
|
|
time.sleep(1)
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"ESP32 唤醒失败: {e}")
|
|||
|
|
finally:
|
|||
|
|
try:
|
|||
|
|
ser.close()
|
|||
|
|
except:
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
|
|||
|
|
def open_serial(port, baud):
|
|||
|
|
"""打开并配置串口"""
|
|||
|
|
try:
|
|||
|
|
ser = serial.Serial(port, baud, timeout=1)
|
|||
|
|
print(f"已打开串口: {port} @ {baud}bps")
|
|||
|
|
return ser
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"打开串口失败: {e}")
|
|||
|
|
sys.exit(1)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def calc_checksum(data: bytearray) -> int:
|
|||
|
|
"""计算 XOR 校验和"""
|
|||
|
|
cs = 0
|
|||
|
|
for b in data:
|
|||
|
|
cs ^= b
|
|||
|
|
return cs
|
|||
|
|
|
|||
|
|
|
|||
|
|
def pack_commands(cmds):
|
|||
|
|
"""
|
|||
|
|
cmds: list of (addr, channel, angle, duration_ms)
|
|||
|
|
返回：完整帧字节数组
|
|||
|
|
格式： [FRAME_HEADER][count] [addr, ch, ang, durL, durH]... [checksum]
|
|||
|
|
"""
|
|||
|
|
pkt = bytearray([FRAME_HEADER, len(cmds)])
|
|||
|
|
for addr, ch, ang, dur in cmds:
|
|||
|
|
pkt.extend([addr, ch, ang, dur & 0xFF, (dur >> 8) & 0xFF])
|
|||
|
|
pkt.append(calc_checksum(pkt))
|
|||
|
|
return pkt
|
|||
|
|
|
|||
|
|
|
|||
|
|
def print_packet(pkt):
|
|||
|
|
"""打印十六进制数据包"""
|
|||
|
|
print("发送数据包:", ' '.join(f"0x{b:02X}" for b in pkt))
|
|||
|
|
|
|||
|
|
|
|||
|
|
def input_hex(prompt):
|
|||
|
|
"""输入 0x 开头或十进制整型"""
|
|||
|
|
val = input(prompt).strip()
|
|||
|
|
try:
|
|||
|
|
return int(val, 0)
|
|||
|
|
except ValueError:
|
|||
|
|
print(f"无效数字: {val}")
|
|||
|
|
return input_hex(prompt)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def input_cmd_single():
|
|||
|
|
"""交互输入单条命令"""
|
|||
|
|
addr = input_hex("输入 addr (hex or dec): ")
|
|||
|
|
ch = input_hex("输入 channel (hex or dec): ")
|
|||
|
|
ang = input_hex("输入 angle (0-180): ")
|
|||
|
|
dur = input_hex("输入 duration(ms): ")
|
|||
|
|
return [(addr, ch, ang, dur)]
|
|||
|
|
|
|||
|
|
|
|||
|
|
def input_cmd_multiple():
|
|||
|
|
"""交互输入多条命令"""
|
|||
|
|
cnt = input_hex("输入命令数量: ")
|
|||
|
|
cmds = []
|
|||
|
|
for i in range(cnt):
|
|||
|
|
print(f"第 {i+1} 条:")
|
|||
|
|
cmds.extend(input_cmd_single())
|
|||
|
|
return cmds
|
|||
|
|
|
|||
|
|
|
|||
|
|
def auto_mode(ser, channels):
|
|||
|
|
"""自动增减模式：0→180→0，步长6，50Hz，多舵机"""
|
|||
|
|
addr = input_hex("自动模式: 输入 addr (hex或dec): ")
|
|||
|
|
interval = 1.0 / 50.0
|
|||
|
|
dur_ms = int(interval * 1000)
|
|||
|
|
angle = 0
|
|||
|
|
direction = 1
|
|||
|
|
step = 6
|
|||
|
|
|
|||
|
|
print(f"启动自动模式 @50Hz, addr=0x{addr:02X}, channels={channels}, step={step}°")
|
|||
|
|
try:
|
|||
|
|
while True:
|
|||
|
|
cmds = [(addr, ch, angle, dur_ms) for ch in channels]
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt)
|
|||
|
|
# 更新角度
|
|||
|
|
angle += direction * step
|
|||
|
|
if angle >= 180:
|
|||
|
|
angle = 180
|
|||
|
|
direction = -1
|
|||
|
|
elif angle <= 0:
|
|||
|
|
angle = 0
|
|||
|
|
direction = 1
|
|||
|
|
time.sleep(interval)
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n自动模式已停止，返回菜单")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def set_channels():
|
|||
|
|
"""自定义舵机数量，返回通道列表 1~N"""
|
|||
|
|
cnt = input_hex("输入自动模式舵机数量 N: ")
|
|||
|
|
if cnt < 1:
|
|||
|
|
print("数量必须 >= 1")
|
|||
|
|
return None
|
|||
|
|
channels = list(range(1, cnt+1))
|
|||
|
|
print(f"已设置通道列表: {channels}")
|
|||
|
|
return channels
|
|||
|
|
|
|||
|
|
|
|||
|
|
def blink_mode(ser):
|
|||
|
|
"""眨眼模式：根据输入的角度进行指定舵机的眨眼操作"""
|
|||
|
|
addr = input_hex("眨眼模式: 输入 addr (hex或dec): ")
|
|||
|
|
blink_count = 10 # 循环10次
|
|||
|
|
interval = 0.1 # 每秒眨眼一次
|
|||
|
|
|
|||
|
|
# 定义五个舵机的通道
|
|||
|
|
servo_channels = [4, 5, 6, 7, 8] # 控制五个舵机
|
|||
|
|
blink_angles = [90, 90, 90, 90, 90] # 眨眼时的角度
|
|||
|
|
open_angles = [50, 130, 130, 60, 90] # 睁开时的角度（4-7号舵机回到90度，8号保持120度）
|
|||
|
|
|
|||
|
|
print(f"启动眨眼模式，10次眨眼，每次间隔 0.1 秒")
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
for i in range(blink_count):
|
|||
|
|
print(f"第 {i+1} 次眨眼...")
|
|||
|
|
|
|||
|
|
# 眨眼动作：同步发送所有舵机到眨眼角度
|
|||
|
|
cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, blink_angles)]
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt)
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
# 睁开动作：同步发送4-7号舵机回到90度，8号保持120度
|
|||
|
|
print("睁开动作：舵机4-7回到90度")
|
|||
|
|
cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, open_angles)]
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt)
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
print("眨眼模式结束，返回菜单")
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n眨眼模式已停止，返回菜单")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def yawn_mode(ser):
|
|||
|
|
"""打哈欠模式：控制多个舵机模拟打哈欠动作"""
|
|||
|
|
addr = input_hex("打哈欠模式: 输入 addr (hex或dec): ")
|
|||
|
|
yawn_count = 5 # 循环5次
|
|||
|
|
interval = 1 # 每次打哈欠动作间隔1秒
|
|||
|
|
|
|||
|
|
# 定义控制的舵机通道
|
|||
|
|
# 打哈欠时使用舵机1, 3, 4, 5, 6, 7, 8（通道1~8）
|
|||
|
|
# 闭嘴时使用舵机8、9（65号舵机）
|
|||
|
|
yawning_channels = [1, 3, 4, 5, 6, 7, 8] # 控制通道1~8
|
|||
|
|
yawning_angles = [70, 110, 60, 130, 120, 80, 90] # 打哈欠时的角度（根据需要调整）
|
|||
|
|
closing_angles = [90, 90, 90, 90, 90, 90, 90] # 闭嘴时的角度（闭嘴动作）
|
|||
|
|
|
|||
|
|
# 65号舵机（8、9号通道）设置不同的角度，模拟控制头部动作
|
|||
|
|
yawning_65_angles = [120, 45, 60, 120] # 65号舵机（8、9号通道）在打哈欠时的角度
|
|||
|
|
closing_65_angles = [120, 45, 90, 90] # 闭嘴时65号舵机（8、9号通道）的角度
|
|||
|
|
|
|||
|
|
print(f"启动打哈欠模式，循环 {yawn_count} 次，每次间隔 {interval} 秒")
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
for i in range(yawn_count):
|
|||
|
|
print(f"第 {i+1} 次打哈欠...")
|
|||
|
|
|
|||
|
|
# 打哈欠动作：同步发送所有舵机到打哈欠角度
|
|||
|
|
cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, yawning_angles)]
|
|||
|
|
cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], yawning_65_angles)] # 控制65号舵机（8、9号通道）
|
|||
|
|
cmds.extend(cmds_65) # 合并65号舵机的指令
|
|||
|
|
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt) # 一次性发送所有指令
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
# 闭嘴动作：同步发送舵机回到闭嘴角度
|
|||
|
|
print("闭嘴动作：所有舵机回到闭嘴角度")
|
|||
|
|
cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, closing_angles)]
|
|||
|
|
cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], closing_65_angles)] # 65号舵机（8、9号通道）
|
|||
|
|
cmds.extend(cmds_65) # 合并65号舵机的指令
|
|||
|
|
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt) # 一次性发送所有指令
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
print("打哈欠模式结束，返回菜单")
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n打哈欠模式已停止，返回菜单")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def talk_mode(ser):
|
|||
|
|
"""说话模式：模拟嘴巴开合动作"""
|
|||
|
|
addr = input_hex("说话模式: 输入 addr (hex或dec): ")
|
|||
|
|
talk_count = 50 # 循环5次
|
|||
|
|
|
|||
|
|
# 定义控制的舵机通道
|
|||
|
|
mouth_channels = [0, 1, 8, 9] # 控制3个舵机（口型）
|
|||
|
|
mouth_open_angles = [120, 45, 80, 110] # 嘴巴打开时的角度
|
|||
|
|
mouth_close_angles = [120, 45, 90, 90] # 嘴巴关闭时的角度（正常闭合）
|
|||
|
|
|
|||
|
|
print(f"启动说话模式，循环 {talk_count} 次，每次间隔随机生成在0~1秒之间")
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
for i in range(talk_count):
|
|||
|
|
print(f"第 {i + 1} 次说话...")
|
|||
|
|
|
|||
|
|
# 开口动作：同步发送所有舵机到嘴巴打开的角度
|
|||
|
|
cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_open_angles)]
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt) # 一次性发送所有指令
|
|||
|
|
|
|||
|
|
# 生成一个 0 到 1 之间的随机间隔时间
|
|||
|
|
interval = random.uniform(0, 0.3)
|
|||
|
|
print(f"随机间隔时间：{interval:.2f}秒")
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
# 闭口动作：同步发送所有舵机回到嘴巴关闭的角度
|
|||
|
|
print("闭口动作：舵机回到闭口角度")
|
|||
|
|
cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_close_angles)]
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt) # 一次性发送所有指令
|
|||
|
|
|
|||
|
|
# 生成一个 0 到 1 之间的随机间隔时间
|
|||
|
|
interval = random.uniform(0, 0.3)
|
|||
|
|
print(f"随机间隔时间：{interval:.2f}秒")
|
|||
|
|
time.sleep(interval)
|
|||
|
|
|
|||
|
|
print("说话模式结束，返回菜单")
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n说话模式已停止，返回菜单")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def main():
|
|||
|
|
if len(sys.argv) < 2:
|
|||
|
|
print(f"用法: {sys.argv[0]} /dev/ttyUSB0 [baudrate]")
|
|||
|
|
sys.exit(1)
|
|||
|
|
port = sys.argv[1]
|
|||
|
|
baud = int(sys.argv[2]) if len(sys.argv) > 2 else 115200
|
|||
|
|
|
|||
|
|
print("唤醒 ESP32...")
|
|||
|
|
wakeup_esp32(port, baud)
|
|||
|
|
ser = open_serial(port, baud)
|
|||
|
|
channels = list(range(1, 10)) # 默认通道 1-9
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
while True:
|
|||
|
|
print("\n=== 操作菜单 ===")
|
|||
|
|
print("1. 控制单个舵机")
|
|||
|
|
print("2. 控制多个舵机")
|
|||
|
|
print("3. 退出")
|
|||
|
|
print("4. 自动增减模式(50Hz)")
|
|||
|
|
print("5. 设置自动模式舵机数量")
|
|||
|
|
print("6. 眨眼模式")
|
|||
|
|
print("7. 打哈欠模式")
|
|||
|
|
print("8. 说话模式") # 新增选项
|
|||
|
|
opt = input("选择: ").strip()
|
|||
|
|
if opt == '1':
|
|||
|
|
cmds = input_cmd_single()
|
|||
|
|
elif opt == '2':
|
|||
|
|
cmds = input_cmd_multiple()
|
|||
|
|
elif opt == '4':
|
|||
|
|
auto_mode(ser, channels)
|
|||
|
|
continue
|
|||
|
|
elif opt == '5':
|
|||
|
|
new_ch = set_channels()
|
|||
|
|
if new_ch:
|
|||
|
|
channels = new_ch
|
|||
|
|
continue
|
|||
|
|
elif opt == '6':
|
|||
|
|
blink_mode(ser)
|
|||
|
|
continue
|
|||
|
|
elif opt == '7': # 调用打哈欠模式
|
|||
|
|
yawn_mode(ser)
|
|||
|
|
continue
|
|||
|
|
elif opt == '8': # 调用说话模式
|
|||
|
|
talk_mode(ser)
|
|||
|
|
continue
|
|||
|
|
elif opt == '3':
|
|||
|
|
break
|
|||
|
|
else:
|
|||
|
|
print("无效选项，重试")
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
pkt = pack_commands(cmds)
|
|||
|
|
print_packet(pkt)
|
|||
|
|
ser.write(pkt)
|
|||
|
|
print("✅ 指令已发送")
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n用户中断，退出")
|
|||
|
|
finally:
|
|||
|
|
ser.close()
|
|||
|
|
print("串口已关闭")
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == '__main__':
|
|||
|
|
main()
|