【存在问题】把补偿去掉，当前无法正常工作需要退回8d4d735a3791da35454a7b3a023a8c0c042d2fe8

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})