|
|
|
@ -12,7 +12,8 @@ from serial_init import SerialSharedData |
|
|
|
from trajectory_data import ( |
|
|
|
Pose_1, Pose_2, Pose_3, Pose_4, Pose_3_4, Pose_4_3, |
|
|
|
joint_positions_assembled_1, joint_positions_assembled_2, |
|
|
|
joint_positions_assembled_3, joint_positions_assembled_4 |
|
|
|
joint_positions_assembled_3, joint_positions_assembled_4, |
|
|
|
Pose_1_2, Pose_2_1, |
|
|
|
) |
|
|
|
import time |
|
|
|
# 导入串口处理模块 |
|
|
|
@ -1013,6 +1014,11 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ |
|
|
|
Pose_3_4_M[i][2] = Pose_3_4[i][2] - Off_Set_Value * factor + Off_Set_Value |
|
|
|
Pose_4_3_M[i][2] = Pose_4_3[i][2] - Off_Set_Value * factor + Off_Set_Value |
|
|
|
|
|
|
|
Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M] |
|
|
|
Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M] |
|
|
|
Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M] |
|
|
|
Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M] |
|
|
|
|
|
|
|
#0.延时操作，点位延时 |
|
|
|
#1.添加点位延时 |
|
|
|
NUM = int(delay * 1000 / sample_time) |
|
|
|
@ -1102,4 +1108,135 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_ |
|
|
|
|
|
|
|
return False |
|
|
|
|
|
|
|
def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay): |
|
|
|
global sample_time, lookahead_time |
|
|
|
lookahead_time = 300 # 前瞻时间 (ms) |
|
|
|
|
|
|
|
sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) |
|
|
|
sleep_time = sample_time*0.001 |
|
|
|
|
|
|
|
#0.点位平移操作 |
|
|
|
# 1.假设 Off_Set_Value 已由外部传入（示例值） |
|
|
|
Off_Set_Value = serial_handler.Trajectory_angle # 示例值，实际由外部传入 |
|
|
|
# 2.深拷贝原始轨迹（避免修改原数据） |
|
|
|
Pose_3_M = copy.deepcopy(Pose_1) |
|
|
|
Pose_4_M = copy.deepcopy(Pose_2) |
|
|
|
# 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143） |
|
|
|
# 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点 |
|
|
|
for i in range(11, 144): # i = 11 ~ 143 对应 MATLAB 第12~144行 |
|
|
|
factor = (i - 11) / 133.0 # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0 |
|
|
|
Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value * factor |
|
|
|
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value * factor |
|
|
|
# 修改第145行到末尾（MATLAB索引145:end => Python索引144:） |
|
|
|
for i in range(144, len(Pose_3_M)): |
|
|
|
Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value |
|
|
|
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value |
|
|
|
# 4.处理 Pose_1_2 和 Pose_2_1 |
|
|
|
num = len(Pose_1_2) # 假设两者长度相同 |
|
|
|
Pose_3_4_M = copy.deepcopy(Pose_1_2) |
|
|
|
Pose_4_3_M = copy.deepcopy(Pose_2_1) |
|
|
|
for i in range(num): |
|
|
|
# MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value |
|
|
|
# Python 中 i 从 0 开始，所以因子为 (i+1) |
|
|
|
factor = (i + 1) / num |
|
|
|
Pose_3_4_M[i][2] = Pose_1_2[i][2] - Off_Set_Value * factor + Off_Set_Value |
|
|
|
Pose_4_3_M[i][2] = Pose_2_1[i][2] - Off_Set_Value * factor + Off_Set_Value |
|
|
|
|
|
|
|
Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M] |
|
|
|
Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M] |
|
|
|
Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M] |
|
|
|
Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M] |
|
|
|
|
|
|
|
#0.延时操作，点位延时 |
|
|
|
#1.添加点位延时 |
|
|
|
NUM = int(delay * 1000 / sample_time) |
|
|
|
# 2.第一道喷涂延时点位 |
|
|
|
last_point_3_4 = Pose_3_4_M[-1].copy() # 先复制最后一个点位 |
|
|
|
Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)] # 每个点位独立副本 |
|
|
|
# 3.第2道喷涂延时点位 |
|
|
|
last_point_4_3 = Pose_4_3_M[-1].copy() |
|
|
|
Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM)] |
|
|
|
|
|
|
|
trajectories = [Pose_3_M, Pose_3_4_M,Pose_3_4_Stop, Pose_4_M, Pose_4_3_M,Pose_4_3_Stop] |
|
|
|
# ----- 一次性清理和初始化透传服务 ----- |
|
|
|
sendCMD(sock, "tt_clear_servo_joint_buf") |
|
|
|
time.sleep(0.05) |
|
|
|
|
|
|
|
#0.喷枪开关控制,开喷枪时间最小为0，最大为155；Pose_4_3_M及Pose_3_4_M中各有20组数据 |
|
|
|
#每道喷涂指令有155个点， |
|
|
|
# 第1道开枪时间在第10个点位（Pose_3_M的开枪点位 lookahead_time/sample_time+10），关枪时间在第130点位示例（lookahead_time/sample_time+135） |
|
|
|
# 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中） |
|
|
|
#队列，延时时间计算 |
|
|
|
off_set_Time=int(lookahead_time / sample_time) |
|
|
|
off_set_Time=0 |
|
|
|
# 1.喷枪1开关时间计算 |
|
|
|
OPEN_1=turn_on_relay_start |
|
|
|
CLOSE_1=turn_off_relay_start |
|
|
|
OPEN_1_M = off_set_Time+OPEN_1 |
|
|
|
CLOSE_1_M1S=off_set_Time+CLOSE_1 |
|
|
|
CLOSE_1_M1E = off_set_Time + CLOSE_1-155 |
|
|
|
#2.喷枪2开关时间计算 |
|
|
|
OPEN_2=ex_turn_on_relay_start |
|
|
|
CLOSE_2 = ex_turn_off_relay_start |
|
|
|
OPEN_2_M = off_set_Time+OPEN_2 |
|
|
|
CLOSE_2_M2S = off_set_Time+CLOSE_2 |
|
|
|
CLOSE_2_M2E = off_set_Time + CLOSE_2- 155 |
|
|
|
|
|
|
|
sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 }) |
|
|
|
cycles = 100 # 循环5次s |
|
|
|
|
|
|
|
for _ in range(cycles): |
|
|
|
for traj in trajectories: |
|
|
|
print("开始执行轨迹...") |
|
|
|
# 针对不同轨迹进行计数 |
|
|
|
if traj is Pose_3_M: |
|
|
|
for idx, point in enumerate(traj, start=1): |
|
|
|
if idx == OPEN_1_M: |
|
|
|
turn_on_relay() |
|
|
|
if idx == CLOSE_1_M1S: |
|
|
|
turn_off_relay() |
|
|
|
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) |
|
|
|
time.sleep(sleep_time) |
|
|
|
elif traj is Pose_3_4_M: |
|
|
|
for idx, point in enumerate(traj, start=1): |
|
|
|
if idx == CLOSE_1_M1E: |
|
|
|
turn_off_relay() |
|
|
|
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) |
|
|
|
time.sleep(sleep_time) |
|
|
|
elif traj is Pose_4_M: |
|
|
|
for idx, point in enumerate(traj, start=1): |
|
|
|
if idx == OPEN_2_M: |
|
|
|
turn_on_relay() |
|
|
|
if idx == CLOSE_2_M2S: |
|
|
|
turn_off_relay() |
|
|
|
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) |
|
|
|
time.sleep(sleep_time) |
|
|
|
elif traj is Pose_4_3_M: |
|
|
|
for idx, point in enumerate(traj, start=1): |
|
|
|
if idx == CLOSE_2_M2E: |
|
|
|
turn_off_relay() |
|
|
|
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) |
|
|
|
time.sleep(sleep_time) |
|
|
|
else: |
|
|
|
# 其他轨迹（如 Stop 轨迹）无需喷枪控制 |
|
|
|
for point in traj: |
|
|
|
send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point}) |
|
|
|
time.sleep(sleep_time) |
|
|
|
print(f"停止轨迹，NUM = {NUM}, delay = {delay}") |
|
|
|
stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) |
|
|
|
if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30': |
|
|
|
print("收到0xAA 0x30,停止循环") |
|
|
|
for i in range(3): |
|
|
|
ser.write(b'\x80\x80') |
|
|
|
ser.flush() |
|
|
|
time.sleep(0.01) |
|
|
|
time.sleep(0.5) |
|
|
|
return False |
|
|
|
print("轨迹执行完成") |
|
|
|
|
|
|
|
return False |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
