feat: 初步增加抓取第二个物块和放置动作

cfg_main.toml

@@ -3,28 +3,28 @@ logger_filename = "log/file_{time}.log"
 logger_format = "{time} {level} {message}"
 
 [task]
-GetBlock_enable = false # 人员施救使能
+GetBlock_enable = true # 人员施救使能
 PutBlock_enable = false # 紧急转移使能
 GetBBall_enable = false # 整装上阵使能
 UpTower_enable = false  # 通信抢修使能
-GetRBall_enable = false # 高空排险使能
+GetRBall_enable = true # 高空排险使能
 PutBBall_enable = false # 派发物资使能
 PutHanoi_enable = false # 物资盘点使能
 MoveArea_enable = false # 应急避险使能
-KickAss_enable = true  # 扫黑除暴使能
+KickAss_enable = false  # 扫黑除暴使能
 
 [find_counts]
-GetBlock_counts = 20 # 人员施救计数
+GetBlock_counts = 5 # 人员施救计数
-PutBlock_counts = 20 # 紧急转移计数
+PutBlock_counts = 5 # 紧急转移计数
 GetBBall_counts = 5 # 整装上阵计数
-UpTower_counts = 10  # 通信抢修计数
+UpTower_counts = 3  # 通信抢修计数
 GetRBall_counts = 10 # 高空排险计数
 
 
 PutBBall_counts = 5 # 派发物资计数
-PutHanoi1_counts = 18 # 物资盘点计数
+PutHanoi1_counts = 10 # 物资盘点计数
 PutHanoi2_counts = 2 # 物资盘点计数
 PutHanoi3_counts = 2 # 物资盘点计数
-MoveArea1_counts = 10 # 应急避险计数
+MoveArea1_counts = 6 # 应急避险计数
 MoveArea2_counts = 1700 # 应急避险第二阶段计数
 KickAss_counts = 20  # 扫黑除暴计数

cfg_subtask.toml

@@ -1,5 +1,6 @@
 [get_block]
 first_block = "red"
+# first_block = "blue"
 [put_block]
 
 [get_bball]
@@ -23,4 +24,4 @@ llm_enable = false # 大模型机器人
 [kick_ass]
 pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
 pos_gap2 = 80 # person 之间的距离
-target_person = 1
+target_person = 3

main.py

@@ -18,9 +18,9 @@ logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logge
 
 # 向任务队列添加任务
 task_queue = queue.Queue()
-task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) 
+task_queue.put(sb.task(sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) 
-# task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) 
+task_queue.put(sb.task(sb.get_block2, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) 
-task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'] and cfg_main['task']['PutBlock_enable'])) 
+task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'])) 
 task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) 
 # TODO 添加一个空任务用于提前降 z 轴
 task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable'])) 

majtask.py

@@ -33,7 +33,8 @@ class main_task():
         self.by_cmd = by_cmd
 
         # 转向 pid
-        self.pid1 = PidWrap(1, 0, 0, output_limits=40)
+        # self.pid1 = PidWrap(0.5, 0, 0, output_limits=55) # 1.2
+        self.pid1 = PidWrap(1.2, 0, 0, output_limits=55) # 1.2
         self.pid1.set_target(0)
 
     def parse_data(self,data):
@@ -69,26 +70,33 @@ class main_task():
             self.y = 0
             error_abs = abs(self.lane_error)
 
-            if error_abs > 45:
+            if error_abs > 50:
-                speed = 8
+                speed = 10
-                # pid_out = self.lane_error * 1.1
+            elif error_abs > 45:
-                # self.pid1.set(1, 0, 0)
+                speed = 11
             elif error_abs > 35:
                 speed = 13
-                # pid_out = self.lane_error * 1
-                # self.pid1.set(1, 0, 0)
             elif error_abs > 25:
                 speed = 15
-                # pid_out = self.lane_error * 1
-                # self.pid1.set(1.8, 0, 0)
             elif error_abs > 15:
                 speed = 15
-                # pid_out = self.lane_error * 0.8
-                # self.pid1.set(1.8, 0, 0)
             else:
-                speed = 15
+                speed = 18
-                # pid_out = self.lane_error * 0.4
+
-                # self.pid1.set(0.7, 0, 0)
+            # lane_model initial
+            # if error_abs > 50:
+            #     speed = 10
+            # elif error_abs > 45:
+            #     speed = 11
+            # elif error_abs > 35:
+            #     speed = 13
+            # elif error_abs > 25:
+            #     speed = 15
+            # elif error_abs > 15:
+            #     speed = 15
+            # else:
+            #     speed = 18
+
             if utils.task_speed != 0:
                 speed = utils.task_speed
             self.by_cmd.send_speed_x(speed)

subtask.py

@@ -3,6 +3,7 @@ from loguru import logger
 from utils import label_filter
 from utils import tlabel
 from utils import LLM
+from utils import CountRecord
 import utils
 import toml
 import zmq
@@ -37,6 +38,7 @@ def car_stop():
         by_cmd.send_speed_x(0)
         time.sleep(0.2)
         by_cmd.send_speed_omega(0)
+# 蓝色球使用
 def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
     not_found_counts = 0
     ret, error = filter.aim_right(label)
@@ -45,7 +47,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
         if not_found_counts >= 20:
             not_found_counts = 0
             error = -320 # error > 0 front run
-            logger.info("找不到 直接向前")
+            logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
             break
         ret, error = filter.aim_right(label)
     
@@ -58,11 +60,12 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
         pass
     # 停的位置已经很接近目标，可以直接使用 distance 校准
     else:
-        error = error * 3
+        # logger.info("停车时误差就小于")
-        if error > 0:
+        # error = error * 3
-            by_cmd.send_distance_x(-10, int(error))
+        # if error > 0:
-        else:
+        #     by_cmd.send_distance_x(-10, int(error))
-            by_cmd.send_distance_x(10, int(-error))
+        # else:
+        #     by_cmd.send_distance_x(10, int(-error))
         return
     while True:
         ret, error = filter.aim_right(label)
@@ -72,13 +75,15 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
         if ret:
             if abs(error) <= 8:
                 car_stop()
-                logger.info("可以停车了")
+                logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
 
                 ret, error = filter.aim_right(label)
                 while not ret:
                     ret, error = filter.aim_right(label)
                 error += offset
+                logger.info(f"calibrate_right_new:停车后的误差是{error}")
                 if abs(error) > 8:
+                    logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
                     error = error * 3
                     if error > 0:
                         by_cmd.send_distance_x(-10, int(error))
@@ -97,8 +102,14 @@ param {*} run_speed
 return {*}
 '''     
 def calibrate_new(label, offset, run = True, run_speed = 3.5):
+    not_found_counts = 0
     ret, box = filter.get(label)
     while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
         ret, box = filter.get(label)
     
     error = (box[0][2] + box[0][0] - 320) / 2 + offset
@@ -109,12 +120,16 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
             by_cmd.send_speed_x(run_speed)
     # 停的位置已经很接近目标，可以直接使用 distance 校准
     else:
-        error = error * 3
+        if abs(error) > 8:
+            logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
+            # error = error # 3
             if error > 0:
                 by_cmd.send_distance_x(-10, int(error))
             else:
                 by_cmd.send_distance_x(10, int(-error))
             return
+        logger.info(f"calibrate_new:停车后误差{error}小于 8  不校准")
+        return 
     while True:
         ret, box = filter.get(label)
         while not ret:
@@ -124,16 +139,18 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
         if ret:
             if abs(error) <= 10: # 5
                 car_stop()
-                logger.info("可以停车了")
+                logger.info("calibrate_new:行进时 误差小于 10 直接停车")
 
                 ret, box = filter.get(label)
                 while not ret:
                     ret, box = filter.get(label)
 
                 error = (box[0][2] + box[0][0] - 320) / 2 + offset
-                logger.info(f"停车后像素误差:{error}")
+                logger.info(f"calibrate_new:停车后的误差是{error}")
                 if abs(error) > 8:
-                    error = error * 1.5 # 3
+                    logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                    error = error * 3
+                    logger.error(f"error * 3{error}")
                     if error > 0:
                         by_cmd.send_distance_x(-10, int(error))
                     else:
@@ -155,9 +172,9 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
         error = (box[0][2] + box[0][0] - 320) / 2 + offset
         if ret:
             # 校准速度越大 停车的条件越宽泛
-            if abs(error) <= 15:
+            if abs(error) <= 10:
                 car_stop()
-                logger.info("可以停车了")
+                logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车")
                 
                 ret, box = filter.get(label)
                 while not ret:
@@ -165,6 +182,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 error = (box[0][2] + box[0][0] - 320) / 2 + offset
                 logger.info(f"停车后像素误差:{error}")
                 if abs(error) > 8:
+                    logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
                     error = error * 3
                     if error > 0:
                         by_cmd.send_distance_x(-10, int(error))
@@ -246,16 +264,73 @@ class task_queuem(task):
         return True
 
 # 人员施救
-class get_block():
+class get_block1():
     def init(self):
-        logger.info("人员施救初始化")
+        logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
         while (by_cmd.send_angle_camera(180) == -1):
             by_cmd.send_angle_camera(180)
         filter.switch_camera(1)
         if cfg['get_block']['first_block'] == "blue":
             self.target_label = tlabel.BBLOCK
             self.another_label = tlabel.RBLOCK
-            cfg['get_block']['first_block'] = ''
+        else:
+            self.target_label = tlabel.RBLOCK
+            self.another_label = tlabel.BBLOCK
+            cfg['get_block']['first_block'] = "red"
+    def find(self):
+        # 目标检测红/蓝方块
+        ret = filter.find(self.target_label)
+        if ret > 0:
+            return True
+        return False
+    def exec(self):
+        car_stop()
+        calibrate_new(self.target_label, offset = 15, run = True)
+        logger.info("抓取块")
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(20, 60)
+        by_cmd.send_position_axis_x(1, 100)
+        time.sleep(1)
+        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_angle_claw(63)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(2)
+        by_cmd.send_angle_claw(30)
+        by_cmd.send_beep(1)
+        time.sleep(0.5)
+        by_cmd.send_beep(0)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 80)
+        by_cmd.send_position_axis_z(20, 130)
+        time.sleep(2)
+        by_cmd.send_angle_claw_arm(36)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(20, 20)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 40)
+        time.sleep(2)
+        by_cmd.send_angle_claw(45)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(20, 80)
+        time.sleep(2)
+        by_cmd.send_angle_claw_arm(220)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 100)
+        time.sleep(1)
+        pass
+    def nexec(self):
+        # TODO 完成不执行任务的空动作
+        pass
+
+class get_block2():
+    def init(self):
+        logger.info(f"人员施救 2 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
+        while (by_cmd.send_angle_camera(180) == -1):
+            by_cmd.send_angle_camera(180)
+        filter.switch_camera(1)
+        if cfg['get_block']['first_block'] == "red":
+            self.target_label = tlabel.BBLOCK
+            self.another_label = tlabel.RBLOCK
         else:
             self.target_label = tlabel.RBLOCK
             self.another_label = tlabel.BBLOCK
@@ -267,57 +342,23 @@ class get_block():
         return False
     def exec(self):
         car_stop()
-        calibrate_new(self.target_label, offset = 12, run = True)
+        calibrate_new(self.target_label, offset = 15, run = True)
         logger.info("抓取块")
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(20, 60)
-        by_cmd.send_position_axis_x(1, 140)
-        time.sleep(4)
         by_cmd.send_angle_claw_arm(220)
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
-        time.sleep(2)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(20, 60)
+        time.sleep(1)
         by_cmd.send_angle_claw(30)
-        by_cmd.send_light(1)
         by_cmd.send_beep(1)
         time.sleep(0.5)
-        by_cmd.send_light(0)
         by_cmd.send_beep(0)
         time.sleep(0.5)
-        by_cmd.send_position_axis_x(1, 140)
+        by_cmd.send_position_axis_x(1, 100)
-        time.sleep(3)
+        time.sleep(2)
-        
         pass
-        # 调试 临时注释掉
-        # calibrate(tlabel.RBLOCK,15)
-        # time.sleep(2)
-        
-        # by_cmd.send_position_axis_z(10, 150)
-        # time.sleep(5)
-        # by_cmd.send_angle_claw_arm(127)
-        # time.sleep(1)
-        # by_cmd.send_position_axis_x(4, 140)
-        # time.sleep(4)
-        # by_cmd.send_angle_claw_arm(220)
-        # by_cmd.send_angle_claw(90)
-        # time.sleep(1)
-        # by_cmd.send_distance_axis_z(10, -70)
-        # time.sleep(3)
-        # by_cmd.send_angle_claw(27)
-        # by_cmd.send_distance_axis_z(10, 10)
-        # time.sleep(2)
-        # by_cmd.send_distance_axis_x(4, -100)
-        # time.sleep(1)
-        # by_cmd.send_distance_axis_z(10, -40)
-        # time.sleep(3)
-        # by_cmd.send_angle_claw(35)
-        # time.sleep(1)
-        # by_cmd.send_position_axis_z(10, 150)
-        # time.sleep(3)
-        # by_cmd.send_position_axis_x(2, 140)
-        # #  抓取第二个块后 收爪 
-        # time.sleep(3)
-        # by_cmd.send_position_axis_x(4, 0)
     def nexec(self):
         # TODO 完成不执行任务的空动作
         pass
@@ -343,22 +384,34 @@ class put_block():
         logger.info("找到医院")
         car_stop()
         calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
-        time.sleep(1)
-        by_cmd.send_position_axis_x(1, 0)
-        time.sleep(0.5)
         by_cmd.send_position_axis_z(20, 0)
         time.sleep(3)
+        by_cmd.send_position_axis_x(1, 0)
+        time.sleep(1)
         by_cmd.send_angle_claw(63)
-        # time.sleep(2)
+        time.sleep(1)
-        # by_cmd.send_position_axis_z(20, 130)
+        by_cmd.send_position_axis_x(1, 80)
-        # while True:
+        by_cmd.send_position_axis_z(20, 130)
-        #     pass
+        time.sleep(2)
-        # by_cmd.send_position_axis_z(10, 150)
+        by_cmd.send_angle_claw_arm(36)
-        # time.sleep(3)
+        time.sleep(1)
-        # # TODO 切换爪子方向
+        by_cmd.send_position_axis_z(20, 20)
-        # by_cmd.send_position_axis_x(2, 140)
+        time.sleep(0.5)
-        # time.sleep(2)
+        by_cmd.send_position_axis_x(1, 40)
-        # by_cmd.send_position_axis_z(10, 170)
+        time.sleep(2)
+        by_cmd.send_angle_claw(30)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(20, 130)
+        time.sleep(3)
+        by_cmd.send_angle_claw_arm(220)
+        by_cmd.send_distance_x(-10, 120)
+        by_cmd.send_position_axis_z(20, 0)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(1, 0)
+        time.sleep(2)
+        by_cmd.send_angle_claw(63)
+        time.sleep(3)
+
         # 下一动作预备位置
         by_cmd.send_position_axis_z(20, 130)
         time.sleep(4)
@@ -396,7 +449,7 @@ class get_bball():
         
     def find(self):
         # 目标检测蓝球
-        ret = filter.find(tlabel.BBALL)
+        ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
         if ret:
             return True
         return False
@@ -447,6 +500,7 @@ class up_tower():
         logger.info("通信抢修初始化")
         while (by_cmd.send_angle_camera(90) == -1):
             by_cmd.send_angle_camera(90)
+        filter.switch_camera(1)
     def find(self):
         # 目标检测通信塔
         ret = filter.find(tlabel.TOWER)
@@ -476,13 +530,18 @@ class up_tower():
 # 高空排险
 class get_rball():
     def init(self):
+        socket.send_string("1")
+        socket.recv()
         logger.info("高空排险初始化")
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
+        self.record = CountRecord(3)
     def find(self):
         # 目标检测红球
         ret = filter.find(tlabel.RBALL)
         if ret > 0:
+            # TODO 连续两帧才开始减速
+            if self.record(tlabel.RBALL):
                 utils.task_speed = 5
             return True
         else:
@@ -494,8 +553,8 @@ class get_rball():
         time.sleep(0.5)
         # 靠近塔
         by_cmd.send_angle_scoop(20)
-        by_cmd.send_distance_y(-10, 65)
+        by_cmd.send_distance_y(-15, 50)
-        time.sleep(1)
+        time.sleep(2)
         calibrate_new(tlabel.RBALL,offset = 50, run = True)
         time.sleep(1)
         logger.info("抓红球")
@@ -504,8 +563,8 @@ class get_rball():
         by_cmd.send_position_axis_z(20, 170)
         time.sleep(5)
         by_cmd.send_angle_scoop(7)
-        by_cmd.send_distance_y(10, 65)
+        by_cmd.send_distance_y(15, 70)
-        time.sleep(0.5)
+        time.sleep(1)
         # while True:
         #     pass
         pass
@@ -609,12 +668,13 @@ class put_hanoi1():
         
         # 根据方向初始化执行器位置
         if utils.direction is tlabel.RMARK:
+            # FIXME 右侧的爪子会被 storage 挡住
             by_cmd.send_position_axis_x(1, 0)
             by_cmd.send_angle_claw_arm(36)
             by_cmd.send_angle_storage(0)
         else:
             by_cmd.send_position_axis_x(1, 150)
-            by_cmd.send_angle_claw_arm(220)
+            by_cmd.send_angle_claw_arm(217)
             by_cmd.send_angle_storage(55)
         time.sleep(1.5)
 
@@ -622,13 +682,17 @@ class put_hanoi1():
 
         if utils.direction_right > utils.direction_left:
             utils.direction = tlabel.RMARK
-            by_cmd.send_angle_omega(-25,430)
+            # by_cmd.send_angle_omega(-25,430)
+            # by_cmd.send_angle_omega(-45,238)
+            by_cmd.send_angle_omega(-55,194)
             time.sleep(2)
             while (by_cmd.send_angle_camera(90) == -1):
                 by_cmd.send_angle_camera(90)
         else:
             utils.direction = tlabel.LMARK
-            by_cmd.send_angle_omega(25,430)
+            # by_cmd.send_angle_omega(25,430)
+            # by_cmd.send_angle_omega(45,238)
+            by_cmd.send_angle_omega(55,194)
             time.sleep(2)
             while (by_cmd.send_angle_camera(180) == -1):
                 by_cmd.send_angle_camera(180)
@@ -641,10 +705,7 @@ class put_hanoi1():
 class put_hanoi2():
     def __init__(self):
         
-        if utils.direction_right > utils.direction_left:
+        
-            self.offset = 25
-        else:
-            self.offset = 15
         self.pos_lp = cfg['put_hanoi2']['pos_lp']
         self.pos_mp = cfg['put_hanoi2']['pos_mp']
         self.pos_sp = 6 - self.pos_lp - self.pos_mp
@@ -660,6 +721,10 @@ class put_hanoi2():
         logger.info(f"setting hanoi pos_lp[{self.pos_lp}] pos_mp[{self.pos_mp}] pos_sp[{self.pos_sp}]") 
     def init(self):
         logger.info("物资盘点 2 初始化")
+        if utils.direction == tlabel.RMARK:
+            self.offset = 25
+        else:
+            self.offset = 10
     def find(self):
         ret, box = filter.get(self.target_label)
         if ret:
@@ -670,33 +735,35 @@ class put_hanoi2():
         return False
         
     def exec(self):
+        
         logger.info(f"direction:{utils.direction.name}")
-        logger.info(f"offset:{self.offset}")
         utils.task_speed = 0
         car_stop()
         time.sleep(0.5)
         # by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
         time.sleep(1)
-        explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
+        explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
         logger.info("抓大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(20, 40)
+            # by_cmd.send_position_axis_z(20, 40)
-            by_cmd.send_position_axis_x(1, 160)
+            # by_cmd.send_position_axis_x(1, 160)
-            by_cmd.send_angle_claw(63)
+            # by_cmd.send_angle_claw(63)
-            time.sleep(1)
+            # time.sleep(1)
-            by_cmd.send_angle_claw(85)
+            # by_cmd.send_angle_claw(85)
-            time.sleep(4)
+            # time.sleep(4)
-            by_cmd.send_angle_claw(50)
+            # by_cmd.send_angle_claw(50)
-            time.sleep(4)
+            # time.sleep(4)
-            by_cmd.send_position_axis_x(2, 80)
+            # by_cmd.send_position_axis_x(2, 80)
-            by_cmd.send_distance_axis_z(20, 10)
+            # # FIXME 多往回收一些 会挡住识别
+            # by_cmd.send_distance_axis_z(20, 10)
+            pass
         else:
             by_cmd.send_position_axis_z(20, 30)
-            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_position_axis_x(1, 50)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
             by_cmd.send_angle_claw(90)
-            time.sleep(4)
+            time.sleep(2)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
             by_cmd.send_distance_axis_z(20, 20)
@@ -704,48 +771,122 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(2, 160)
             time.sleep(4)
             pass
-        explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
+        explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
         logger.info("放大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_distance_axis_z(20, -10)
+            # by_cmd.send_distance_axis_z(20, -10)
+            # by_cmd.send_position_axis_x(2, 160)
+            # time.sleep(4)
+            # by_cmd.send_angle_claw(90)
+            # by_cmd.send_position_axis_x(1, 0)
+            pass
+        else:
+            by_cmd.send_position_axis_x(2, 60)
+            time.sleep(4)
+            by_cmd.send_distance_axis_z(20, -20)
+            time.sleep(1)
+            by_cmd.send_angle_claw(90)
+            time.sleep(1)
+            by_cmd.send_position_axis_x(1, 160)
+
+        explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
+        logger.info("抓中平台")
+        if utils.direction is tlabel.RMARK:
+            # by_cmd.send_position_axis_z(20, 40)
+            # by_cmd.send_position_axis_x(1, 160)
+            # by_cmd.send_angle_claw(63)
+            # time.sleep(1)
+            # by_cmd.send_angle_claw(85)
+            # time.sleep(4)
+            # by_cmd.send_angle_claw(37)
+            # time.sleep(4)
+            # by_cmd.send_position_axis_x(2, 40)
+            # # FIXME 多往回收一些 会挡住识别
+            # by_cmd.send_distance_axis_z(20, 10)
+            pass
+        else:
+            by_cmd.send_position_axis_z(20, 30)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(63)
+            time.sleep(1)
+            by_cmd.send_angle_claw(85)
+            time.sleep(2)
+            by_cmd.send_angle_claw(40)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(20, 20)
+            time.sleep(2)
             by_cmd.send_position_axis_x(2, 160)
             time.sleep(4)
-            by_cmd.send_angle_claw(90)
+            pass
-            by_cmd.send_position_axis_x(1, 0)
+        explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed =  5)
+        logger.info("放中平台")
+        if utils.direction is tlabel.RMARK:
+            # by_cmd.send_distance_axis_z(20, -10)
+            # by_cmd.send_position_axis_x(2, 160)
+            # time.sleep(4)
+            # by_cmd.send_angle_claw(90)
+            # by_cmd.send_position_axis_x(1, 0)
+            pass
         else:
+            by_cmd.send_distance_axis_z(20, 60)
+            time.sleep(4)
             by_cmd.send_position_axis_x(2, 40)
             time.sleep(4)
             by_cmd.send_distance_axis_z(20, -20)
-            time.sleep(2)
+            time.sleep(1)
             by_cmd.send_angle_claw(90)
-            time.sleep(2)
+            time.sleep(1)
             by_cmd.send_position_axis_x(1, 160)
 
-        explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  6)
+        explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
-        logger.info("抓中平台")
-        if utils.direction is tlabel.RMARK:
-            pass
-        else:
-            pass
-        explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed =  6)
-        logger.info("放中平台")
-        if utils.direction is tlabel.RMARK:
-            pass
-        else:
-            pass
-
-        explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed =  6)
         logger.info("抓小平台")
         if utils.direction is tlabel.RMARK:
+            # by_cmd.send_position_axis_z(20, 40)
+            # by_cmd.send_position_axis_x(1, 160)
+            # by_cmd.send_angle_claw(63)
+            # time.sleep(1)
+            # by_cmd.send_angle_claw(85)
+            # time.sleep(4)
+            # by_cmd.send_angle_claw(37)
+            # time.sleep(4)
+            # by_cmd.send_position_axis_x(2, 40)
+            # # FIXME 多往回收一些 会挡住识别
+            # by_cmd.send_distance_axis_z(20, 10)
             pass
         else:
+            by_cmd.send_position_axis_z(20, 30)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(63)
+            time.sleep(1)
+            by_cmd.send_angle_claw(85)
+            time.sleep(2)
+            by_cmd.send_angle_claw(30)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(20, 20)
+            time.sleep(2)
+            by_cmd.send_position_axis_x(2, 160)
+            time.sleep(4)
             pass
-        explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed =  6)
+        explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed =  5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
+            # by_cmd.send_distance_axis_z(20, -10)
+            # by_cmd.send_position_axis_x(2, 160)
+            # time.sleep(4)
+            # by_cmd.send_angle_claw(90)
+            # by_cmd.send_position_axis_x(1, 0)
             pass
         else:
-            pass
+            by_cmd.send_distance_axis_z(20, 155)
+            time.sleep(4)
+            by_cmd.send_position_axis_x(2, 40)
+            time.sleep(4)
+            by_cmd.send_distance_axis_z(20, -20)
+            time.sleep(1)
+            by_cmd.send_angle_claw(90)
+            time.sleep(1)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(2)
         # while True:
         #     pass
         
@@ -754,6 +895,8 @@ class put_hanoi2():
 
 class put_hanoi3():
     def init(self):
+        by_cmd.send_position_axis_z(20, 130)
+        time.sleep(3)
         logger.info("完成任务，爪回左侧")
         by_cmd.send_angle_claw_arm(128)
         time.sleep(0.5)
@@ -799,7 +942,7 @@ class move_area2():
         ret, box = filter.get(tlabel.SHELTER)
         if ret:
             error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
-            if abs(error) < 10:
+            if abs(error) < 20:
                 return 5000
         return False
     def exec(self):

utils.py

@@ -276,7 +276,31 @@ class LLM:
         self.messages.append(self.resp.to_message())
         return self.resp.get_result()
 
+class CountRecord:
+    def __init__(self, stop_count=2) -> None:
+        self.last_record = None
+        self.count = 0
+        self.stop_cout = stop_count
 
+    def get_count(self, val):
+        try:
+            if val == self.last_record:
+                self.count += 1
+            else:
+                self.count=0
+                self.last_record = val
+            return self.count
+        except Exception as e:
+            print(e)
+    
+    def __call__(self, val):
+        self.get_count(val)
+        if self.count >= self.stop_cout:
+            if type(val) == bool:
+                return val
+            return True
+        else:
+            return False