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("串口已关闭")
|