55 lines
1.5 KiB
Python
55 lines
1.5 KiB
Python
from calculate.calculate_angle import main_of_5dof
|
||
|
||
# control_with_serial.py
|
||
|
||
|
||
# 📡 初始化串口连接
|
||
def init_serial(port=SERIAL_PORT, baud_rate=BAUD_RATE):
|
||
try:
|
||
ser = serial.Serial(port, baud_rate, timeout=TIMEOUT)
|
||
print(f"串口已连接: {port} @ {baud_rate}")
|
||
return ser
|
||
except Exception as e:
|
||
print(f"无法打开串口 {port}: {e}")
|
||
return None
|
||
|
||
# 📤 发送角度到串口
|
||
def send_angles(ser, theta1_deg, theta4_deg):
|
||
# 格式：theta1_deg,theta4_deg\n
|
||
data = f"{int(theta1_deg)},{int(theta4_deg)}\n"
|
||
try:
|
||
ser.write(data.encode())
|
||
print(f"已发送角度: theta1 = {theta1_deg}°, theta4 = {theta4_deg}°")
|
||
except Exception as e:
|
||
print(f"发送失败: {e}")
|
||
|
||
# 🚀 主程序入口
|
||
if __name__ == "__main__":
|
||
# 调用你的轨迹规划函数
|
||
angles_list = main_of_5dof(
|
||
trajectory_type='line',
|
||
start=(0, 0),
|
||
end=(200, 300),
|
||
show_animation=False
|
||
)
|
||
|
||
# 初始化串口
|
||
ser = init_serial()
|
||
if ser is None:
|
||
exit(1)
|
||
|
||
# 遍历角度列表并发送
|
||
for i, (theta1, theta4) in enumerate(angles_list):
|
||
theta1_deg = np.degrees(theta1)
|
||
theta4_deg = np.degrees(theta4)
|
||
|
||
print(f"第 {i} 帧角度: theta1 = {theta1_deg:.2f}°, theta4 = {theta4_deg:.2f}°")
|
||
send_angles(ser, theta1_deg, theta4_deg)
|
||
|
||
# 可选：每帧之间加延迟，控制发送速度
|
||
import time
|
||
time.sleep(0.05) # 每帧间隔 50ms
|
||
|
||
# 关闭串口
|
||
ser.close()
|
||
print("串口已关闭") |