feat: 增加最後對準基地運行

feat: 增加原始模型參數和任務版本

cfg_args.toml

@@ -1,5 +1,5 @@
-[kick_ass]
+[lane_mode]
-target_person = 3
+mode_index = 3
 
 [task]
 Subtask_enable = true

cfg_subtask.toml.7154.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 2
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 2.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

cfg_subtask.toml.raw.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.8
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

majtask.py

@@ -67,6 +67,7 @@ class main_task():
             error_abs = abs(self.lane_error)
 
             if error_abs > 50:
+                # speed = 11
                 speed = 13
             elif error_abs > 45:
                 speed = 15

subtask.py

@@ -1439,7 +1439,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
@@ -1477,6 +1477,16 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 120)
         time.sleep(1)
         logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass

subtask_69.py

@@ -1439,7 +1439,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
@@ -1477,6 +1477,16 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 120)
         time.sleep(1)
         logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass

subtask_7123.py

@@ -1367,7 +1367,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)

subtask_7126.py

@@ -1360,7 +1360,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)

test/test_cali.py

@@ -34,15 +34,16 @@ def signal_handler(sig, frame):
 
 offset = 0
 signal.signal(signal.SIGTERM, signal_handler)
+cmd_py_obj.send_speed_x(15)
 while True:
-    time.sleep(0.2)
-    ret, box = filter.get(tlabel.LMARK)
-    if ret:
-        # 宽度大于 41 停车
-        print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
-    # if ret:
-    #     error = (box[0][2] + box[0][0] - 320) / 2 + offset
-    #     print(error)
 
-    #     cmd_py_obj.send_speed_omega(-error * 0.8)
+    time.sleep(0.02)
+    ret, box = filter.get(tlabel.BASE)
+    # if ret:
+        # 宽度大于 41 停车
+        # print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
+    if ret:
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        print(error)
+        cmd_py_obj.send_speed_omega(-error * 0.8)
 car_stop()

test/test_filter.py

@@ -16,18 +16,18 @@ socket.connect("tcp://localhost:6667")
 logger.info("subtask yolo client init")
 
 filter = label_filter(socket)
-filter.switch_camera(1)
+filter.switch_camera(2)
 
 
-find_counts = 0
+# find_counts = 0
-offset = 19
+# offset = 19
-label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
+# label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
-label = tlabel.TPLATFORM
+# label = tlabel.TPLATFORM
 while True:
     time.sleep(0.2)
-    ret, box = filter.get(tlabel.TPLATFORM)
+    ret, box = filter.get(tlabel.BASE)
     if ret:
-        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        error = (box[0][2] + box[0][0] - 320) / 2
         logger.error(error)
 
     # label = tlabel.HOSPITAL

variable.py

@@ -9,7 +9,8 @@ task_speed = 0
 pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0} # 1.2
 
 # 转向 pid 对象
-pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 50
+# pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=70) # FIXME 6_9 模型为 60
+pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 60
 
 llm_text = ''