diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..cea4d3f --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "windows-gcc-x64", + "includePath": [ + "${workspaceFolder}/**" + ], + "compilerPath": "gcc", + "cStandard": "${default}", + "cppStandard": "${default}", + "intelliSenseMode": "windows-gcc-x64", + "compilerArgs": [ + "" + ] + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..01e2c9f --- /dev/null +++ b/.vscode/launch.json @@ -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 + } + ] + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..bb879da --- /dev/null +++ b/.vscode/settings.json @@ -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 +} \ No newline at end of file diff --git a/kelaifen/Kelaifen_V6.0/config.json b/kelaifen/Kelaifen_V6.0/config.json index 3f153a2..13b6c0a 100644 --- a/kelaifen/Kelaifen_V6.0/config.json +++ b/kelaifen/Kelaifen_V6.0/config.json @@ -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, diff --git a/kelaifen/Kelaifen_V6.0/serial_handler.py b/kelaifen/Kelaifen_V6.0/serial_handler.py index 1929532..0eb885e 100644 --- a/kelaifen/Kelaifen_V6.0/serial_handler.py +++ b/kelaifen/Kelaifen_V6.0/serial_handler.py @@ -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"] diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py index 9787044..3310418 100644 --- a/kelaifen/Kelaifen_V6.0/trajectory_program.py +++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py @@ -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]