feat: 添加hanoi2物块掉落处理，添加hanoi2 停车延时，修改蓝球计数

cfg_main.toml

@@ -7,7 +7,7 @@ logger_format = "{time} {level} {message}"
 [find_counts]
 GetBlock_counts = 5
 PutBlock_counts = 8
-GetBBall_counts = 1
+GetBBall_counts = 5
 UpTower_counts = 3
 GetRBall_counts = 10
 PutBBall_counts = 15

subtask.py

@@ -118,7 +118,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             logger.info(f"calibrate_right_new:停车后的误差是{error}")
             if abs(error) > 8:
                 logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
-                error = error * 2.3
+                error = error * 1.5
                 if error > 0:
                     by_cmd.send_distance_x(-10, int(error))
                 else:
@@ -240,7 +240,44 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 
                 break
     return True
+def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
+    stop_error = 0
+    error_record = CountRecord(3)
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
         
+    if target_label == tlabel.TPLATFORM:
+        stop_error = 8
+    else:
+        stop_error = 15
+    while True:
+        ret1, ret2, box = filter.get_two(target_label, error_label)
+        while not ret1:
+            if not global_skip_queue.empty():
+                _ = global_skip_queue.get()
+                logger.error("跳过 hanoi_calibrate")
+                return False
+            if ret2:
+                if error_record(ret2):
+                    return False
+            ret1, ret2, box = filter.get_two(target_label, error_label)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret1:
+            # 校准速度越大 停车的条件越宽泛 20 15
+            if abs(error) <= stop_error:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
+                
+                ret, box = filter.get(target_label)
+                while not ret:
+                    ret, box = filter.get(target_label)
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"停车后像素误差:{error}")
+                break
+    return True
 # 任务类
 class task:  
     def __init__(self, name, task_template, find_counts=10, enable=True):  
@@ -903,6 +940,7 @@ class put_hanoi1():
         else:
             var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
         pass
+        time.sleep(1.5)
 
 class put_hanoi2():
     def __init__(self):
@@ -915,7 +953,7 @@ class put_hanoi2():
             self.target_label = tlabel.SPILLER
     def init(self):
         logger.info("物资盘点 2 初始化")
-        var.task_speed = 10
+        var.task_speed = 8.5
         if utils.direction == tlabel.RMARK:
             # 15
             self.offset = 14
@@ -925,6 +963,9 @@ class put_hanoi2():
             self.offset = 14
             #self.platform_offset = -30
             self.platform_offset = -19
+        # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异）
+        time.sleep(0.5)
+
     def find(self):
         # ret, box = filter.get(self.target_label)
         ret, box = filter.get(tlabel.TPLATFORM)
@@ -1035,7 +1076,8 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
             pass
-        ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("跳过物资盘点 2 exec")
             return
@@ -1095,7 +1137,8 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
             pass
-        ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("跳过物资盘点 2 exec")
             return
@@ -1156,8 +1199,8 @@ class put_hanoi3():
         time.sleep(1)
         # while by_cmd.send_angle_claw_arm(225) == -1:
         #     pass
-        # while by_cmd.send_angle_claw(90) == -1:
-        #     pass
+        while by_cmd.send_angle_claw(85) == -1:
+            pass
     def find(self):
         time.sleep(1)
         return True

utils.py

@@ -190,6 +190,33 @@ class label_filter:
                     return True 
         return False
     '''
+    description: 查询两个目标 只有 target_label 返回 box
+    param {*} self
+    param {*} tlabel
+    return {[bool]}
+    ''' 
+    def get_two(self, target_label, label):
+        response = self.get_resp()
+        if response['code'] == 0:
+            ret, results = self.filter_box(response['data'])
+            if ret:
+                expect_boxes = (results[:, 0] == target_label.value)
+                boxes = results[expect_boxes, :]
+                if len(boxes) != 0:
+                    target_bool = True
+                    target_box = boxes[:, 2:]
+                else:
+                    target_bool = False
+                    target_box = None
+                expect_boxes = (results[:, 0] == label.value)
+                boxes = results[expect_boxes, :]
+                if len(boxes) != 0:
+                    label_bool = True
+                else:
+                    label_bool = False
+                return (target_bool, label_bool, target_box)
+        return (False, False, None)
+    '''
     description: 判断传入的多目标标签是否存在，存在返回 True
     param {*} self
     param {*} tlabel