增加左右偏移设置

.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
+}

.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
+        }
+      ]
+    }
+  ]
+}

.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
+}

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_on_relay_start": 6,
-                "turn_off_relay_start": 144,
+                "turn_off_relay_start": 140,
-                "ex_turn_on_relay_start": 10,
+                "ex_turn_on_relay_start": 6,
-                "ex_turn_off_relay_start": 144
+                "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,

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"]

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_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 * 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_3_M[i][2] = Pose_3[i][2] + Off_Set_Value_1
-        Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value
+        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_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 * factor + Off_Set_Value
+        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_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 * 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_3_M[i][2] = Pose_1[i][2] + Off_Set_Value_1
-        Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value
+        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_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 * factor + Off_Set_Value
+        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]