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

This reverts commit 835a56cd62.

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