LiZongdi
/
Huangjiabao_backup
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

This reverts commit 835a56cd62.
master
Lizongdi 3 weeks ago
parent
835a56cd62
commit
219d7e6a49
1 changed files with 12 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 54
      kelaifen/Kelaifen_V6.0/trajectory_program.py

54
kelaifen/Kelaifen_V6.0/trajectory_program.py
View File

@ -49,8 +49,6 @@ Interpolation_points = 20
programinit_lock2 = 1 programinit_lock2 = 1
programinit_lock6 = 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 Coordinate_compensation
global sample_time global sample_time
global sleep_time global sleep_time
global increments_flag
sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
sleep_time = sample_time*0.001 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(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 Coordinate_compensation = increments
reference_pos = None reference_pos = None
new_joint_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 sample_time
global sleep_time global sleep_time
global lookahead_time global lookahead_time
global programinit_lock2
global increments_flag
sample_time = SAMP_TIME sample_time = SAMP_TIME
sleep_time = sample_time*0.001 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(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 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: for modified_list in interpolated_poses:
# 初始化透传 # 初始化透传
if (i == 0 and programinit_lock2 == 1): if (i == 0 and programinit_lock2 == 1):
print("初始化100")
programinit_lock2 = 0 programinit_lock2 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "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 sleep_time
global lookahead_time global lookahead_time
global Pose_after_adjustment global Pose_after_adjustment
global programinit_lock2
global Trajectory_angle_flag
lookahead_time = 400 lookahead_time = 400
i = 0 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开始） # 核心逻辑：判断行号是否在13到143之间，修改第三列（索引为2，因为列表从0开始）
if 13 <= actual_line_num <= 143: if 13 <= actual_line_num <= 143:
# 计算新的第三列值：360.2 + ((204.75 + Trajectory_angle)/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 = line_list.copy()
line_list[2] = new_third_value # 第三列对应索引2 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 Pose_after_adjustment = modified_list
# print(Pose_after_adjustment) # print(Pose_after_adjustment)
if (i == 0 and programinit_lock2 == 1): if (i == 0 and programinit_lock2 == 1):
print("初始化2")
programinit_lock2 = 0 programinit_lock2 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "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 Coordinate_compensation
global sample_time global sample_time
global sleep_time global sleep_time
global increments_flag
sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
sleep_time = sample_time*0.001 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(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 Coordinate_compensation = increments
reference_pos = None reference_pos = None
new_joint_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 sample_time
global sleep_time global sleep_time
global lookahead_time global lookahead_time
global programinit_lock2
global increments_flag
sample_time = SAMP_TIME sample_time = SAMP_TIME
sleep_time = sample_time*0.001 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(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 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: for modified_list in interpolated_poses:
# 初始化透传 # 初始化透传
if (i == 0 and programinit_lock2 == 1): if (i == 0 and programinit_lock2 == 1):
print("初始化3")
programinit_lock2 = 0 programinit_lock2 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "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 sleep_time
global lookahead_time global lookahead_time
global Pose_after_adjustment global Pose_after_adjustment
global programinit_lock2
global Trajectory_angle_flag
lookahead_time = 400 lookahead_time = 400
i = 0 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开始） # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始）
if 13 <= actual_line_num <= 143: 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 = line_list.copy()
line_list[2] = new_third_value # 第三列对应索引2 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 Pose_after_adjustment = modified_list
# print(Pose_after_adjustment) # print(Pose_after_adjustment)
if (i == 0 and programinit_lock2 == 1): if (i == 0 and programinit_lock2 == 1):
print("初始化4")
programinit_lock2 = 0 programinit_lock2 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "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 Coordinate_compensation
global sample_time global sample_time
global sleep_time global sleep_time
global increments_flag
sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
sleep_time = sample_time*0.001 sleep_time = sample_time*0.001
if not compensate_queue.empty(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 Coordinate_compensation = increments
reference_pos = None reference_pos = None
new_joint_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 sample_time
global sleep_time global sleep_time
global lookahead_time global lookahead_time
global programinit_lock6
global increments_flag
sample_time = SAMP_TIME sample_time = SAMP_TIME
sleep_time = sample_time*0.001 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(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 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: for modified_list in interpolated_poses:
# 初始化透传 # 初始化透传
if (i == 0 and programinit_lock6 == 1): if (i == 0 and programinit_lock6 == 1):
print("初始化500")
programinit_lock6 = 0 programinit_lock6 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "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 sleep_time
global lookahead_time global lookahead_time
global Pose_after_adjustment global Pose_after_adjustment
global programinit_lock6
global Trajectory_angle_flag
suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
lookahead_time = 400 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开始） # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始）
if 13 <= actual_line_num <= 143: 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 = line_list.copy()
line_list[2] = new_third_value # 第三列对应索引2 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 Pose_after_adjustment = modified_list
# print(Pose_after_adjustment) # print(Pose_after_adjustment)
if (i == 0 and programinit_lock6 == 1): if (i == 0 and programinit_lock6 == 1):
print("初始化6")
programinit_lock6 = 0 programinit_lock6 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "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 Coordinate_compensation
global sample_time global sample_time
global sleep_time global sleep_time
global increments_flag
sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
sleep_time = sample_time*0.001 sleep_time = sample_time*0.001
if not compensate_queue.empty(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 Coordinate_compensation = increments
reference_pos = None reference_pos = None
new_joint_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 sample_time
global sleep_time global sleep_time
global lookahead_time global lookahead_time
global programinit_lock6
global increments_flag
sample_time = SAMP_TIME sample_time = SAMP_TIME
sleep_time = sample_time*0.001 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(): if not compensate_queue.empty():
increments_x, increments_y, increments_z = compensate_queue.get_nowait() 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 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: for modified_list in interpolated_poses:
# 初始化透传 # 初始化透传
if (i == 0 and programinit_lock6 == 1): if (i == 0 and programinit_lock6 == 1):
print("初始化700")
programinit_lock6 = 0 programinit_lock6 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "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 sleep_time
global lookahead_time global lookahead_time
global Pose_after_adjustment global Pose_after_adjustment
global programinit_lock6
global Trajectory_angle_flag
#suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") #suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
lookahead_time = 400 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开始） # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始）
if 13 <= actual_line_num <= 143: 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 = line_list.copy()
line_list[2] = new_third_value # 第三列对应索引2 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 Pose_after_adjustment = modified_list
# print(Pose_after_adjustment) # print(Pose_after_adjustment)
if (i == 0 and programinit_lock6 == 1): if (i == 0 and programinit_lock6 == 1):
print("初始化8")
programinit_lock6 = 0 programinit_lock6 = 0
suc, result, id = sendCMD(sock, "transparent_transmission_init", { suc, result, id = sendCMD(sock, "transparent_transmission_init", {
"lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})

Write Preview
Loading…
Cancel
Save