diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 8925e8e..78c5c28 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -49,6 +49,8 @@ Interpolation_points = 20 programinit_lock2 = 1 programinit_lock6 = 1 +Trajectory_angle_flag = 0 +increments_flag = 0 # 第一段程序封装为函数 # 第二段程序封装为函数(上升喷涂) @@ -69,6 +71,7 @@ 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 @@ -76,7 +79,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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -126,6 +129,8 @@ 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 @@ -134,7 +139,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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -168,6 +173,7 @@ 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, @@ -190,6 +196,8 @@ 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 @@ -208,7 +216,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) / 131) * (actual_line_num-12) + new_third_value = 360.2 + ((204.75 + (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) # 复制原列表并修改第三列（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -224,6 +232,7 @@ 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}) @@ -251,6 +260,7 @@ 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 @@ -258,7 +268,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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -306,6 +316,8 @@ 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 @@ -314,7 +326,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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -349,6 +361,7 @@ 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, @@ -371,6 +384,8 @@ 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 @@ -386,7 +401,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) / 131) * (actual_line_num-12) + new_third_value = 372.19 + ((204.75 + (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) # 复制原列表并修改（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -402,6 +417,7 @@ 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}) @@ -428,13 +444,14 @@ 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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -485,6 +502,8 @@ 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 @@ -492,7 +511,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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -528,6 +547,7 @@ 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, @@ -549,6 +569,8 @@ 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 @@ -563,7 +585,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) / 131) * (actual_line_num-12) + new_third_value = 576.94 - ((204.75 - (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) # 复制原列表并修改（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -579,6 +601,7 @@ 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}) @@ -603,13 +626,14 @@ 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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments reference_pos = None new_joint_pos = None @@ -659,6 +683,8 @@ 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 @@ -666,7 +692,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_y, increments_z+z_positionadjustment, 0, 0, 0] + increments = [increments_x * increments_flag, increments_y * increments_flag, (increments_z * increments_flag)+z_positionadjustment, 0, 0, 0] Coordinate_compensation = increments # 获取插值起点和插值终点 @@ -700,6 +726,7 @@ 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, @@ -721,6 +748,8 @@ 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 @@ -737,7 +766,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) / 131) * (actual_line_num-12) + new_third_value = 564.95 - ((204.75 - (serial_handler.Trajectory_angle * Trajectory_angle_flag)) / 131) * (actual_line_num-12) # 复制原列表并修改（避免修改原列表的引用） line_list = line_list.copy() line_list[2] = new_third_value # 第三列对应索引2 @@ -753,6 +782,7 @@ 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})