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

增加左右偏移设置

master
Lizongdi 3 days ago
parent
eddfa045ba
commit
b3cab48906
6 changed files with 150 additions and 34 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      .vscode/c_cpp_properties.json
  2. 24
      .vscode/launch.json
  3. 59
      .vscode/settings.json
  4. 34
      kelaifen/Kelaifen_V6.0/config.json
  5. 16
      kelaifen/Kelaifen_V6.0/serial_handler.py
  6. 33
      kelaifen/Kelaifen_V6.0/trajectory_program.py

18
.vscode/c_cpp_properties.json
View File

@ -0,0 +1,18 @@
{
"configurations": [
{
"name": "windows-gcc-x64",
"includePath": [
"${workspaceFolder}/**"
],
"compilerPath": "gcc",
"cStandard": "${default}",
"cppStandard": "${default}",
"intelliSenseMode": "windows-gcc-x64",
"compilerArgs": [
""
]
}
],
"version": 4
}

24
.vscode/launch.json
View File

@ -0,0 +1,24 @@
{
"version": "0.2.0",
"configurations": [
{
"name": "C/C++ Runner: Debug Session",
"type": "cppdbg",
"request": "launch",
"args": [],
"stopAtEntry": false,
"externalConsole": true,
"cwd": "d:/music/huangjiabao_backup/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src",
"program": "d:/music/huangjiabao_backup/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/build/Debug/outDebug",
"MIMode": "gdb",
"miDebuggerPath": "gdb",
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
]
}
]
}

59
.vscode/settings.json
View File

@ -0,0 +1,59 @@
{
"C_Cpp_Runner.cCompilerPath": "gcc",
"C_Cpp_Runner.cppCompilerPath": "g++",
"C_Cpp_Runner.debuggerPath": "gdb",
"C_Cpp_Runner.cStandard": "",
"C_Cpp_Runner.cppStandard": "",
"C_Cpp_Runner.msvcBatchPath": "C:/Program Files/Microsoft Visual Studio/VR_NR/Community/VC/Auxiliary/Build/vcvarsall.bat",
"C_Cpp_Runner.useMsvc": false,
"C_Cpp_Runner.warnings": [
"-Wall",
"-Wextra",
"-Wpedantic",
"-Wshadow",
"-Wformat=2",
"-Wcast-align",
"-Wconversion",
"-Wsign-conversion",
"-Wnull-dereference"
],
"C_Cpp_Runner.msvcWarnings": [
"/W4",
"/permissive-",
"/w14242",
"/w14287",
"/w14296",
"/w14311",
"/w14826",
"/w44062",
"/w44242",
"/w14905",
"/w14906",
"/w14263",
"/w44265",
"/w14928"
],
"C_Cpp_Runner.enableWarnings": true,
"C_Cpp_Runner.warningsAsError": false,
"C_Cpp_Runner.compilerArgs": [],
"C_Cpp_Runner.linkerArgs": [],
"C_Cpp_Runner.includePaths": [],
"C_Cpp_Runner.includeSearch": [
"*",
"**/*"
],
"C_Cpp_Runner.excludeSearch": [
"**/build",
"**/build/**",
"**/.*",
"**/.*/**",
"**/.vscode",
"**/.vscode/**"
],
"C_Cpp_Runner.useAddressSanitizer": false,
"C_Cpp_Runner.useUndefinedSanitizer": false,
"C_Cpp_Runner.useLeakSanitizer": false,
"C_Cpp_Runner.showCompilationTime": false,
"C_Cpp_Runner.useLinkTimeOptimization": false,
"C_Cpp_Runner.msvcSecureNoWarnings": false
}

34
kelaifen/Kelaifen_V6.0/config.json
View File

@ -3,25 +3,25 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
"300": 0,
"6": 0,
"700": 0
"700": 1.5
},
"relay_config": {
"turn_on_relay_start": 10,
"turn_off_relay_start": 144,
"ex_turn_on_relay_start": 10,
"ex_turn_off_relay_start": 144
"turn_on_relay_start": 6,
"turn_off_relay_start": 140,
"ex_turn_on_relay_start": 6,
"ex_turn_off_relay_start": 140
}
},
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -39,7 +39,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -57,7 +57,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -75,7 +75,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -93,7 +93,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -111,7 +111,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -129,7 +129,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -147,7 +147,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -165,7 +165,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -183,7 +183,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,
@ -201,7 +201,7 @@
{
"params": [
80,
0
0, 0, 0, 0
],
"program_delay_map": {
"2": 0,

16
kelaifen/Kelaifen_V6.0/serial_handler.py
View File

@ -7,6 +7,9 @@ from transform import transform_data
# 声明需要用到的全局变量（与主程序保持一致）
speed_adjustment = 100 # 初始值设为100，在50-100范围内
Trajectory_angle = 0
Trajectory_angle1 = 0
Trajectory_angle2 = 0
Trajectory_angle3 = 0
compensate_queue = None # 后续由主程序赋值
# 初始化全局共享数据实例
@ -99,7 +102,7 @@ def read_cmd_from_shared(timeout=1.0):
# 处理特殊指令0xBB，更新全局参数并打印清晰信息
def handle_special_commands(PRESET_PARAMS):
global speed_adjustment, Trajectory_angle
global speed_adjustment, Trajectory_angle, Trajectory_angle1, Trajectory_angle2, Trajectory_angle3
bb_header = 0xBB
bb_frame_length = 2 # 0xBB + 1字节配置选择值(0-11)
@ -124,15 +127,24 @@ def handle_special_commands(PRESET_PARAMS):
# 匹配预设参数项（索引 = 配置值 - 1）
selected_preset = PRESET_PARAMS[config_select - 1]
# 提取params并更新全局变量
speed_val, angle_val = selected_preset["params"]
speed_val, angle_val, angle_val1, angle_val2, angle_val3 = selected_preset["params"]
if speed_val < 50:
speed_val = 50
if speed_val > 150:
speed_val = 150
if angle_val > 200:
angle_val = 200
if angle_val1 > 200:
angle_val1 = 200
if angle_val2 > 200:
angle_val2 = 200
if angle_val3 > 200:
angle_val3 = 200
speed_adjustment = speed_val
Trajectory_angle = angle_val
Trajectory_angle1 = angle_val1
Trajectory_angle2 = angle_val2
Trajectory_angle3 = angle_val3
# 提取当前配置项对应的delay和relay配置（赋值给返回变量）
selected_delay_map = selected_preset["program_delay_map"]
selected_relay_config = selected_preset["relay_config"]

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

@ -1091,20 +1091,22 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_
# 1.假设 Off_Set_Value 已由外部传入（示例值）
Actual_value_mm=133*sleep_time*80
Comp_value_mm = 204.75 - Actual_value_mm
Off_Set_Value = serial_handler.Trajectory_angle + Comp_value_mm# 示例值，实际由外部传入
# 2.深拷贝原始轨迹（避免修改原数据）
Pose_3_M = copy.deepcopy(Pose_3)
Pose_4_M = copy.deepcopy(Pose_4)
# 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143）
# 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点
Off_Set_Value_1=serial_handler.Trajectory_angle + Comp_value_mm
Off_Set_Value_2=serial_handler.Trajectory_angle1 + Comp_value_mm
for i in range(11, 144): # i = 11 ~ 143 对应 MATLAB 第12~144行
factor = (i - 11) / 133.0 # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0
Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value * factor
Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value * factor
Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value_1 * factor
Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value_2 * factor
# 修改第145行到末尾（MATLAB索引145:end => Python索引144:）
for i in range(144, len(Pose_3_M)):
Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value
Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value
Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value_1
Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value_2
# 4.处理 Pose_3_4 和 Pose_4_3
num = len(Pose_3_4) # 假设两者长度相同
Pose_3_4_M = copy.deepcopy(Pose_3_4)
@ -1113,8 +1115,8 @@ def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_
# MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value
# Python 中 i 从 0 开始，所以因子为 (i+1)
factor = (i + 1) / num
Pose_3_4_M[i][2] = Pose_3_4[i][2] - Off_Set_Value * factor + Off_Set_Value
Pose_4_3_M[i][2] = Pose_4_3[i][2] - Off_Set_Value * factor + Off_Set_Value
Pose_3_4_M[i][2] = Pose_3_4[i][2] - Off_Set_Value_1 * factor + Off_Set_Value_1
Pose_4_3_M[i][2] = Pose_4_3[i][2] - Off_Set_Value_2 * factor + Off_Set_Value_2
Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M]
Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M]
@ -1230,22 +1232,23 @@ def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_
# 1.假设 Off_Set_Value 已由外部传入（示例值）
Actual_value_mm=133*sleep_time*80
Comp_value_mm = 204.75 - Actual_value_mm
Off_Set_Value = serial_handler.Trajectory_angle + Comp_value_mm # 示例值，实际由外部传入
# 这里的喷涂方向与program100相反
Off_Set_Value = -Off_Set_Value
# 2.深拷贝原始轨迹（避免修改原数据）
Pose_3_M = copy.deepcopy(Pose_1)
Pose_4_M = copy.deepcopy(Pose_2)
# 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143）
# 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点
Off_Set_Value_1=-(serial_handler.Trajectory_angle2 + Comp_value_mm)
Off_Set_Value_2=-(serial_handler.Trajectory_angle3 + Comp_value_mm)
for i in range(11, 144): # i = 11 ~ 143 对应 MATLAB 第12~144行
factor = (i - 11) / 133.0 # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0
Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value * factor
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value * factor
Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value_1 * factor
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value_2 * factor
# 修改第145行到末尾（MATLAB索引145:end => Python索引144:）
for i in range(144, len(Pose_3_M)):
Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value
Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value_1
Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value_2
# 4.处理 Pose_1_2 和 Pose_2_1
num = len(Pose_1_2) # 假设两者长度相同
Pose_3_4_M = copy.deepcopy(Pose_1_2)
@ -1254,8 +1257,8 @@ def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_
# MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value
# Python 中 i 从 0 开始，所以因子为 (i+1)
factor = (i + 1) / num
Pose_3_4_M[i][2] = Pose_1_2[i][2] - Off_Set_Value * factor + Off_Set_Value
Pose_4_3_M[i][2] = Pose_2_1[i][2] - Off_Set_Value * factor + Off_Set_Value
Pose_3_4_M[i][2] = Pose_1_2[i][2] - Off_Set_Value_1 * factor + Off_Set_Value_1
Pose_4_3_M[i][2] = Pose_2_1[i][2] - Off_Set_Value_2 * factor + Off_Set_Value_2
Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M]
Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M]

Write Preview
Loading…
Cancel
Save