95 lines
2.6 KiB
Python
95 lines
2.6 KiB
Python
import time
|
|
import numpy as np
|
|
|
|
# 导入运动学函数
|
|
from calculate.ik import inverseF
|
|
|
|
# 导入串口控制函数
|
|
from duoji_test.motor2 import send_hex_command, generate_position_command
|
|
|
|
# 机械臂参数（和 qt_main.py 保持一致）
|
|
L1 = 250
|
|
L2 = 300
|
|
L3 = 300
|
|
L4 = 250
|
|
L0 = 250
|
|
|
|
# 串口配置
|
|
MOTOR1_PORT = "/dev/ttyACM1" # 电机1使用的串口
|
|
MOTOR2_PORT = "/dev/ttyACM0" # 电机2使用的串口
|
|
BAUD_RATE = 115200
|
|
|
|
# 控制参数
|
|
SPEED_MOTOR1 = 10 # Rad/s
|
|
SPEED_MOTOR2 = 10 # Rad/s
|
|
DIRECTION_MOTOR1 = 1 # 0: 逆时针, 1: 顺时针
|
|
DIRECTION_MOTOR2 = 1
|
|
|
|
# 初始化电机角度为0
|
|
current_angle_motor1 = 0
|
|
current_angle_motor2 = 0
|
|
|
|
|
|
def control_two_motors(theta1, theta4):
|
|
"""
|
|
根据 theta1 和 theta4 控制两个电机转动指定角度
|
|
:param theta1: 左臂角度（弧度）
|
|
:param theta4: 右臂角度（弧度）
|
|
"""
|
|
global current_angle_motor1, current_angle_motor2
|
|
|
|
# 弧度转角度
|
|
angle1_target = np.degrees(theta1)
|
|
angle4_target = np.degrees(theta4)
|
|
|
|
# 计算相对角度
|
|
relative_angle1 = angle1_target - current_angle_motor1
|
|
relative_angle4 = angle4_target - current_angle_motor2
|
|
|
|
print(f"发送相对角度: 电机1={relative_angle1:.2f}°, 电机2={relative_angle4:.2f}°")
|
|
|
|
# 更新当前角度
|
|
current_angle_motor1 += relative_angle1
|
|
current_angle_motor2 += relative_angle4
|
|
|
|
# 生成指令
|
|
cmd1 = generate_position_command(angle=relative_angle1, speed=SPEED_MOTOR1, direction=DIRECTION_MOTOR1)
|
|
cmd2 = generate_position_command(angle=relative_angle4, speed=SPEED_MOTOR2, direction=DIRECTION_MOTOR2)
|
|
|
|
# 发送指令到两个电机
|
|
send_hex_command(MOTOR1_PORT, BAUD_RATE, cmd1)
|
|
send_hex_command(MOTOR2_PORT, BAUD_RATE, cmd2)
|
|
|
|
|
|
def run_trajectory():
|
|
"""
|
|
主函数：模拟轨迹点并控制电机
|
|
"""
|
|
|
|
# 模拟一个圆形轨迹（你可以替换成从文件读取或用户输入）
|
|
center = (150, 250)
|
|
radius = 60
|
|
num_points = 100
|
|
angles = np.linspace(0, 2 * np.pi, num_points)
|
|
x_list = center[0] + radius * np.cos(angles)
|
|
y_list = center[1] + radius * np.sin(angles)
|
|
|
|
print("开始执行轨迹控制...")
|
|
|
|
for i in range(len(x_list)):
|
|
x = x_list[i]
|
|
y = y_list[i]
|
|
|
|
try:
|
|
theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
|
control_two_motors(theta1, theta4)
|
|
except Exception as e:
|
|
print(f"第 {i} 点计算失败: {e}")
|
|
|
|
time.sleep(0.1) # 控制发送频率（可根据实际需求调整）
|
|
|
|
print("轨迹执行完成。")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
run_trajectory() |