From 219d7e6a49ec79d1565f22d4872a463a7d7a6bb1 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Thu, 28 May 2026 08:51:04 +0800 Subject: [PATCH] =?UTF-8?q?Revert=20"=E3=80=90=E5=AD=98=E5=9C=A8=E9=97=AE?= =?UTF-8?q?=E9=A2=98=E3=80=91=E6=8A=8A=E8=A1=A5=E5=81=BF=E5=8E=BB=E6=8E=89?= =?UTF-8?q?=EF=BC=8C=E5=BD=93=E5=89=8D=E6=97=A0=E6=B3=95=E6=AD=A3=E5=B8=B8?= =?UTF-8?q?=E5=B7=A5=E4=BD=9C=E9=9C=80=E8=A6=81=E9=80=80=E5=9B=9E8d4d735a3?= =?UTF-8?q?791da35454a7b3a023a8c0c042d2fe8"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 835a56cd62c9672cf0a022dcb9a5a134d11cc666. --- kelaifen/Kelaifen_V6.0/trajectory_program.py | 54 +++++--------------- 1 file changed, 12 insertions(+), 42 deletions(-) diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 78c5c28..8925e8e 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -49,8 +49,6 @@ Interpolation_points = 20 programinit_lock2 = 1 programinit_lock6 = 1 -Trajectory_angle_flag = 0 -increments_flag = 0 # 第一段程序封装为函数 # 第二段程序封装为函数(上升喷涂) @@ -71,7 +69,6 @@ def program1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global Coordinate_compensation global sample_time global sleep_time - global increments_flag sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sleep_time = sample_time*0.001 @@ -79,7 +76,7 @@ def program1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -129,8 +126,6 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star global sample_time global sleep_time global lookahead_time - global programinit_lock2 - global increments_flag sample_time = SAMP_TIME sleep_time = sample_time*0.001 @@ -139,7 +134,7 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -173,7 +168,6 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star for modified_list in interpolated_poses: # 初始化透传 if (i == 0 and programinit_lock2 == 1): - print("初始化100") programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, @@ -196,8 +190,6 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global sleep_time global lookahead_time global Pose_after_adjustment - global programinit_lock2 - global Trajectory_angle_flag lookahead_time = 400 i = 0 @@ -216,7 +208,7 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, # 核心逻辑：判断行号是否在13到143之间，修改第三列（索引为2，因为列表从0开始） if 13 <= actual_line_num <= 143: # 计算新的第三列值：360.2 + ((204.75 + Trajectory_angle)/143) * 行号 - new_third_value = 360.2 + ((204.75 + (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) + new_third_value = 360.2 + ((204.75 + serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) # 复制原列表并修改第三列（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -232,7 +224,6 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, Pose_after_adjustment = modified_list # print(Pose_after_adjustment) if (i == 0 and programinit_lock2 == 1): - print("初始化2") programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) @@ -260,7 +251,6 @@ def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global Coordinate_compensation global sample_time global sleep_time - global increments_flag sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sleep_time = sample_time*0.001 @@ -268,7 +258,7 @@ def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -316,8 +306,6 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star global sample_time global sleep_time global lookahead_time - global programinit_lock2 - global increments_flag sample_time = SAMP_TIME sleep_time = sample_time*0.001 @@ -326,7 +314,7 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -361,7 +349,6 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star for modified_list in interpolated_poses: # 初始化透传 if (i == 0 and programinit_lock2 == 1): - print("初始化3") programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, @@ -384,8 +371,6 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global sleep_time global lookahead_time global Pose_after_adjustment - global programinit_lock2 - global Trajectory_angle_flag lookahead_time = 400 i = 0 @@ -401,7 +386,7 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始） if 13 <= actual_line_num <= 143: # 计算新的第三列值 - new_third_value = 372.19 + ((204.75 + (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) + new_third_value = 372.19 + ((204.75 + serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) # 复制原列表并修改（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -417,7 +402,6 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, Pose_after_adjustment = modified_list # print(Pose_after_adjustment) if (i == 0 and programinit_lock2 == 1): - print("初始化4") programinit_lock2 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) @@ -444,14 +428,13 @@ def program5(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global Coordinate_compensation global sample_time global sleep_time - global increments_flag sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sleep_time = sample_time*0.001 if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -502,8 +485,6 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star global sample_time global sleep_time global lookahead_time - global programinit_lock6 - global increments_flag sample_time = SAMP_TIME sleep_time = sample_time*0.001 @@ -511,7 +492,7 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -547,7 +528,6 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star for modified_list in interpolated_poses: # 初始化透传 if (i == 0 and programinit_lock6 == 1): - print("初始化500") programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, @@ -569,8 +549,6 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global sleep_time global lookahead_time global Pose_after_adjustment - global programinit_lock6 - global Trajectory_angle_flag suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") lookahead_time = 400 @@ -585,7 +563,7 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始） if 13 <= actual_line_num <= 143: # 计算新的第三列值（注意是减法公式） - new_third_value = 576.94 - ((204.75 - (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) + new_third_value = 576.94 - ((204.75 - serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) # 复制原列表并修改（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -601,7 +579,6 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, Pose_after_adjustment = modified_list # print(Pose_after_adjustment) if (i == 0 and programinit_lock6 == 1): - print("初始化6") programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) @@ -626,14 +603,13 @@ def program7(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global Coordinate_compensation global sample_time global sleep_time - global increments_flag sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sleep_time = sample_time*0.001 if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -683,8 +659,6 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star global sample_time global sleep_time global lookahead_time - global programinit_lock6 - global increments_flag sample_time = SAMP_TIME sleep_time = sample_time*0.001 @@ -692,7 +666,7 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star if not compensate_queue.empty(): increments_x, increments_y, increments_z = compensate_queue.get_nowait() - increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -726,7 +700,6 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star for modified_list in interpolated_poses: # 初始化透传 if (i == 0 and programinit_lock6 == 1): - print("初始化700") programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, @@ -748,8 +721,6 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, global sleep_time global lookahead_time global Pose_after_adjustment - global programinit_lock6 - global Trajectory_angle_flag #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") lookahead_time = 400 @@ -766,7 +737,7 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始） if 13 <= actual_line_num <= 143: # 计算新的第三列值（注意是减法公式） - new_third_value = 564.95 - ((204.75 - (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) + new_third_value = 564.95 - ((204.75 - serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) # 复制原列表并修改（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -782,7 +753,6 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, Pose_after_adjustment = modified_list # print(Pose_after_adjustment) if (i == 0 and programinit_lock6 == 1): - print("初始化8") programinit_lock6 = 0 suc, result, id = sendCMD(sock, "transparent_transmission_init", { "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})