国赛出发前版本

fix: 补充 action 列表检查
pref: 修改 BBALL offset 值 (未同步到其他备份 subtask.py 中)
套用8822参数

action.py

@@ -13,15 +13,19 @@ move = None
 axis = None
 cmd = None
 
+action_run_flag = None
+
 cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml')
 
-def import_obj(_bycmd):
+def import_obj(_run_flag,_bycmd):
     global bycmd
     global move
     global axis
     global cmd
+    global action_run_flag
 
     bycmd = _bycmd
+    action_run_flag = _run_flag
     move = move_cls()
     axis = axis_cls()
     cmd = cmd_cls()
@@ -118,7 +122,7 @@ class cmd_cls():
         else:
             time_via = _time_via
         logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}")
-        while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1:
+        while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet():
             pass  
         time.sleep(time_via)
     def z2(self, _speed, _pos, _time_via = -1):
@@ -131,7 +135,7 @@ class cmd_cls():
         else:
             time_via = _time_via
         logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}")
-        while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1:
+        while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet():
             pass  
         time.sleep(time_via)
     def x(self, _speed, _dis, _time_via = -1):
@@ -143,7 +147,7 @@ class cmd_cls():
         else:
             time_via = _time_via
         logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}")
-        while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1:
+        while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet():
             pass  
         time.sleep(time_via)
     def x2(self, _speed, _pos, _time_via = -1):
@@ -156,30 +160,30 @@ class cmd_cls():
         else:
             time_via = _time_via
         logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}")
-        while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1:
+        while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet():
             pass  
         time.sleep(time_via)
     def camera(self, angle):
-        while bycmd.send_angle_camera(angle) == -1:
+        while bycmd.send_angle_camera(angle) == -1 and action_run_flag.isSet():
             pass
     def claw(self, angle):
-        while bycmd.send_angle_claw(angle) == -1:
+        while bycmd.send_angle_claw(angle) == -1 and action_run_flag.isSet():
             pass
         time.sleep(0.5)
     def claw_arm(self, angle):
-        while bycmd.send_angle_claw_arm(angle) == -1:
+        while bycmd.send_angle_claw_arm(angle) == -1 and action_run_flag.isSet():
             pass
         time.sleep(1)
     def scoop(self, angle):
-        while bycmd.send_angle_scoop(angle) == -1:
+        while bycmd.send_angle_scoop(angle) == -1 and action_run_flag.isSet():
             pass
         time.sleep(0.5)
     def storage(self, angle):
-        while bycmd.send_angle_storage(angle) == -1:
+        while bycmd.send_angle_storage(angle) == -1 and action_run_flag.isSet():
             pass
         time.sleep(0.5)
     def zhuan(self, angle):
-        while bycmd.send_angle_zhuan(angle) == -1:
+        while bycmd.send_angle_zhuan(angle) == -1 and action_run_flag.isSet():
             pass
         time.sleep(0.5)
     def wait(self, _time):

app.py

@@ -23,7 +23,7 @@ processes = []
 
 time_record = None
 
-task_run_flag = False
+
 
 # 日志队列
 queue = Queue()
@@ -37,7 +37,9 @@ app.config['SECRET_KEY'] = 'secret!'
 socketio = SocketIO(app, allow_unsafe_werkzeug=True)
 
 server_process = None
+# FIXME 在列表里存所有的 task_process  
 task_process = None
+task_run_flag = threading.Event()
 
 class WebSocketHandler(logging.Handler):
     def emit(self, record):
@@ -124,23 +126,23 @@ def operate_handle(data):
     elif data['type'] == 'operate_task':
         # 任务函数
         if data['content'] == 'run':
-            task_run_flag = True
+            task_run_flag.set()
+            # 开启 task 进程前先关闭所有历史进程
             if task_process != None:
                 task_process.terminate()
+
             time_record = time.perf_counter()
-            task_process = Process(target=main_func, args=(queue,skip_task_queue))
+            task_process = Process(target=main_func, args=(task_run_flag,queue,skip_task_queue))
             task_process.start()
             logger.info("开启 task")
         elif data['content'] == 'stop':
-            task_run_flag = False
-            task_process.terminate()
+            task_run_flag.clear()
+            if task_process != None:
+                task_process.terminate()
             logger.info(f"任务结束 用时{time.perf_counter() - time_record}s")
             logger.info("关闭  task")
         elif data['content'] == 'restart':
-            if task_process != None:
-                task_process.terminate()
-            task_process = Process(target=main_func, args=(queue,skip_task_queue))
-            task_process.start()
+            pass
     elif data['type'] == 'show_server_log':
         content = ''
         try:
@@ -167,7 +169,7 @@ def test_connect():
     for item in fileOptions_list:
         config_data[item] = toml.load(os.path.join(fileOptions_path,item))
     socketio.emit('config_data', {'type': 'config_data', 'content': config_data})
-    socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag)})
+    socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())})
 def thread_function():
     global queue
     while True:

cfg_subtask.toml.891.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.5
+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.8
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.2
+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 = true
+
+[kick_ass]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

main_upper.py

@@ -6,12 +6,12 @@ import subtask as sb
 import majtask as mj
 from by_cmd_py import by_cmd_py
 import time
-import action as act
+# import action as act
 import logging
 import signal
 
-running = True
-def main_func(_queue, _skip_queue):
+# running = True
+def main_func(_run_flag,_queue, _skip_queue):
     if _queue != None:
         # 日志重定向
         class Handler(logging.Handler):
@@ -23,14 +23,13 @@ def main_func(_queue, _skip_queue):
         logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
         
     def signal_handler(sig, frame):
-        global running
-        running = False
+        _run_flag.clear()
     signal.signal(signal.SIGTERM, signal_handler)
 
 
     cmd_py_obj = by_cmd_py(_queue)
     sb.import_obj(cmd_py_obj, _skip_queue)
-    act.import_obj(cmd_py_obj)
+    # act.import_obj(_run_flag, cmd_py_obj)
 
     # 读取配置
     cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
@@ -42,12 +41,20 @@ def main_func(_queue, _skip_queue):
     logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
     logger.info(cfg_args)
 
-    act.axis.camera(0)
-    act.axis.x2(140)
-    act.axis.storage(20)
-    act.axis.scoop(25)
-    act.axis.claw_arm(225)
-    act.axis.exec()
+    cmd_py_obj.send_angle_camera(0)
+    cmd_py_obj.send_position_axis_x(1, 140)
+    cmd_py_obj.send_angle_storage(20)
+    cmd_py_obj.send_angle_scoop(25)
+    # cmd_py_obj.send_angle_claw_arm(225)
+    time.sleep(1)
+    
+    # 弃用 action
+    # act.axis.camera(0)
+    # act.axis.x2(140)
+    # act.axis.storage(20)
+    # act.axis.scoop(25)
+    # act.axis.claw_arm(225)
+    # act.axis.exec()
 
     logger.info(cfg_main)
     
@@ -72,7 +79,7 @@ def main_func(_queue, _skip_queue):
     task_queuem_t = sb.task_queuem(task_queue)
     # 创建任务队列的工作线程
     def worker_thread():
-        while task_queuem_t.exec(_skip_queue) is True:
+        while task_queuem_t.exec(_skip_queue) and _run_flag.isSet():
             pass
 
     # 启动工作线程
@@ -83,7 +90,7 @@ def main_func(_queue, _skip_queue):
     # 创建主任务
     main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
     try:
-        while running:
+        while _run_flag.isSet():
             if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
                 pass
             else:

subtask.py

@@ -10,7 +10,6 @@ import toml
 import zmq
 import time
 import variable as var
-import action as act
 import re
 import math
 import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             ret, error = filter.aim_right(label)
         error += offset
         # if ret:
-        if abs(error) <= 8:
+        if abs(error) <= 10:
             car_stop()
             logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
             ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 car_stop()
                 logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
                 
-                ret, box = filter.get(label)
-                while not ret:
-                    ret, box = filter.get(label)
-                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                # 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"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
                 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
+                # 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
@@ -426,8 +425,7 @@ class task_queuem(task):
 class get_block1():
     def init(self):
         var.task_speed = 15
-        act.cmd.camera(0)
-        act.cmd.z2(20, 60, 0)
+        by_cmd.send_angle_camera(0)
         filter.switch_camera(1)
         # if cfg['get_block']['first_block'] == "blue":
         #     self.target_label = tlabel.BBLOCK
@@ -464,7 +462,7 @@ class get_block1():
         logger.info("抓取块")
 
         by_cmd.send_position_axis_z(30, 80)
-        time.sleep(1)
+        time.sleep(1.5)
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
@@ -557,7 +555,7 @@ class put_block():
         ret, box = filter.get(tlabel.HOSPITAL)
         if ret > 0:
             width = box[0][2] - box[0][0]
-            if width > 135:
+            if width > 125: #FIXME maybe 125 batter
                 return True
         return False
     def exec(self):
@@ -651,7 +649,8 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         for _ in range(3):
-            calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
+            by_cmd.send_position_axis_z(30, 155)
+            calibrate_right_new(tlabel.BBALL, offset = 10, run = True, run_speed = 5)
             logger.info("抓蓝色球")
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
@@ -672,7 +671,7 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
             time.sleep(1)
-            by_cmd.send_position_axis_z(30, 155)
+            # by_cmd.send_position_axis_z(30, 155)
             # 继续向前走
             # by_cmd.send_speed_x(4)
         pass
@@ -710,10 +709,10 @@ class up_tower():
         calibrate_new(tlabel.TOWER, offset = 20, run = True)
         time.sleep(1)
         # calibrate(tlabel.TOWER, 27, False, 6)
-        by_cmd.send_distance_x(-10, 120)
+        by_cmd.send_distance_x(-10, 100) # used to be 120
         time.sleep(1)
         # 上古參數
-        by_cmd.send_distance_y(-10, 50) # 80
+        # by_cmd.send_distance_y(-10, 50) # 80
         # 6_9 模型參數
         # by_cmd.send_distance_y(-10, 40)
         # 7_12_3 模型參數
@@ -724,6 +723,12 @@ class up_tower():
         # time.sleep(3)
         # by_cmd.send_speed_y(-10)
         # time.sleep(0.15)
+        # 8822
+        by_cmd.send_distance_y(-10, 50)
+        time.sleep(0.3)
+        # 891
+        # by_cmd.send_distance_y(-10, 35)
+        # time.sleep(0.3)
         
         by_cmd.send_angle_zhuan(10)
         time.sleep(12)
@@ -781,7 +786,10 @@ class get_rball():
         # car_stop()
         # 8822 参数
         by_cmd.send_distance_y(-15, 40)
-        time.sleep(1.5)
+        time.sleep(0.5)
+        # 891 参数
+        # by_cmd.send_distance_y(-15, 40)
+        # time.sleep(0.3)
         calibrate_new(tlabel.RBALL,offset = 44, run = True)
         time.sleep(1)
         logger.info("抓红球")
@@ -1105,14 +1113,14 @@ class put_hanoi2():
             time.sleep(1)
             pass
         # 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)
+        ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 140)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1124,7 +1132,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 140)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1166,14 +1174,14 @@ class put_hanoi2():
             time.sleep(2)
             pass
         # 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)
+        ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 200) # 170 #190(new)
+            by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1189,7 +1197,7 @@ class put_hanoi2():
             # car_stop()
             pass
         else:
-            by_cmd.send_position_axis_z(30, 200)
+            by_cmd.send_position_axis_z(30, 210)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1540,6 +1548,16 @@ class move_area2():
                 if len(resp_commands) == 0:
                     return
                 action_list = resp_commands
+
+                # 先检查一下 action 是否生成正确，如果不正确直接跳过
+                actions_keys = self.action_dict.keys()
+                try:
+                    for action in action_list:
+                        if not (action.get('action') in actions_keys):
+                            return
+                except:
+                    return
+                
                 # 进入停车区域
                 by_cmd.send_distance_y(10, 450)
                 time.sleep((450 * 5 / 1000) + 0.5)
@@ -1610,44 +1628,26 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 150)
 
         # 移动到中间
-        by_cmd.send_distance_x(10, 295)
         time.sleep(1)
         by_cmd.send_angle_claw(15)
         by_cmd.send_angle_claw_arm(225)
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 80)
         time.sleep(1)
+
+        # 先移动到第一个人的地方
+        by_cmd.send_distance_x(10, self.pos_gap1)
+        time.sleep(1.5)
         if self.target_person == 1:
-            by_cmd.send_distance_x(-10, 150)
-            time.sleep(1.5)
-            car_stop()
-        elif self.target_person == 2:
-            by_cmd.send_distance_x(-10, 50)
-            time.sleep(1.5)
-            car_stop()
-        elif self.target_person == 3:
-            by_cmd.send_distance_x(10, 50)
-            time.sleep(1.5)
-            car_stop()
+            # target_distance = self.pos_gap1
+            pass
         else:
-            by_cmd.send_distance_x(10, 150)
-            time.sleep(1.5)
-            car_stop()
-
-        # 先移动到第一个人的地方 假动作
-        # by_cmd.send_distance_x(10, self.pos_gap1)
-        # time.sleep(1.5)
-
-        # if self.target_person == 1:
-        #     # target_distance = self.pos_gap1
-        #     pass
-        # else:
-        #     # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        #     target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        #     by_cmd.send_distance_x(10, target_distance)
-        #     logger.info(f"target distance {target_distance}")
-        #     time.sleep(1.5 + (self.target_person - 1) * 0.7 )
-        # car_stop()
+            # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+            target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10
+            by_cmd.send_distance_x(10, target_distance)
+            logger.info(f"target distance {target_distance}")
+            time.sleep(1.5 + (self.target_person - 1) * 0.7 )
+        car_stop()
         
         # by_cmd.send_angle_claw_arm(225)
         # time.sleep(0.5)
@@ -1669,6 +1669,8 @@ class kick_ass():
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass
+        
+       
     def nexec(self):
         pass
     def after(self):

subtask_8822.py

@@ -10,7 +10,6 @@ import toml
 import zmq
 import time
 import variable as var
-import action as act
 import re
 import math
 import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             ret, error = filter.aim_right(label)
         error += offset
         # if ret:
-        if abs(error) <= 8:
+        if abs(error) <= 10:
             car_stop()
             logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
             ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 car_stop()
                 logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
                 
-                ret, box = filter.get(label)
-                while not ret:
-                    ret, box = filter.get(label)
-                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                # 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"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
                 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
+                # 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
@@ -426,8 +425,7 @@ class task_queuem(task):
 class get_block1():
     def init(self):
         var.task_speed = 15
-        act.cmd.camera(0)
-        act.cmd.z2(20, 60, 0)
+        by_cmd.send_angle_camera(0)
         filter.switch_camera(1)
         # if cfg['get_block']['first_block'] == "blue":
         #     self.target_label = tlabel.BBLOCK
@@ -464,7 +462,7 @@ class get_block1():
         logger.info("抓取块")
 
         by_cmd.send_position_axis_z(30, 80)
-        time.sleep(1)
+        time.sleep(1.5)
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
@@ -557,7 +555,7 @@ class put_block():
         ret, box = filter.get(tlabel.HOSPITAL)
         if ret > 0:
             width = box[0][2] - box[0][0]
-            if width > 135:
+            if width > 130: #FIXME maybe 125 batter
                 return True
         return False
     def exec(self):
@@ -651,6 +649,7 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         for _ in range(3):
+            by_cmd.send_position_axis_z(30, 155)
             calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
             logger.info("抓蓝色球")
             time.sleep(0.5)
@@ -672,7 +671,7 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
             time.sleep(1)
-            by_cmd.send_position_axis_z(30, 155)
+            # by_cmd.send_position_axis_z(30, 155)
             # 继续向前走
             # by_cmd.send_speed_x(4)
         pass
@@ -710,10 +709,10 @@ class up_tower():
         calibrate_new(tlabel.TOWER, offset = 20, run = True)
         time.sleep(1)
         # calibrate(tlabel.TOWER, 27, False, 6)
-        by_cmd.send_distance_x(-10, 120)
+        by_cmd.send_distance_x(-10, 100) # used to be 120
         time.sleep(1)
         # 上古參數
-        by_cmd.send_distance_y(-10, 50) # 80
+        # by_cmd.send_distance_y(-10, 50) # 80
         # 6_9 模型參數
         # by_cmd.send_distance_y(-10, 40)
         # 7_12_3 模型參數
@@ -724,6 +723,12 @@ class up_tower():
         # time.sleep(3)
         # by_cmd.send_speed_y(-10)
         # time.sleep(0.15)
+        # 8822
+        by_cmd.send_distance_y(-10, 50)
+        time.sleep(0.3)
+        # 891
+        # by_cmd.send_distance_y(-10, 35)
+        # time.sleep(0.3)
         
         by_cmd.send_angle_zhuan(10)
         time.sleep(12)
@@ -781,7 +786,10 @@ class get_rball():
         # car_stop()
         # 8822 参数
         by_cmd.send_distance_y(-15, 40)
-        time.sleep(1.5)
+        time.sleep(0.5)
+        # 891 参数
+        # by_cmd.send_distance_y(-15, 40)
+        # time.sleep(0.3)
         calibrate_new(tlabel.RBALL,offset = 44, run = True)
         time.sleep(1)
         logger.info("抓红球")
@@ -1105,14 +1113,14 @@ class put_hanoi2():
             time.sleep(1)
             pass
         # 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)
+        ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 140)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1124,7 +1132,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 140)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1166,14 +1174,14 @@ class put_hanoi2():
             time.sleep(2)
             pass
         # 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)
+        ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 200) # 170 #190(new)
+            by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1189,7 +1197,7 @@ class put_hanoi2():
             # car_stop()
             pass
         else:
-            by_cmd.send_position_axis_z(30, 200)
+            by_cmd.send_position_axis_z(30, 210)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1540,6 +1548,16 @@ class move_area2():
                 if len(resp_commands) == 0:
                     return
                 action_list = resp_commands
+
+                # 先检查一下 action 是否生成正确，如果不正确直接跳过
+                actions_keys = self.action_dict.keys()
+                try:
+                    for action in action_list:
+                        if not (action.get('action') in actions_keys):
+                            return
+                except:
+                    return
+                
                 # 进入停车区域
                 by_cmd.send_distance_y(10, 450)
                 time.sleep((450 * 5 / 1000) + 0.5)
@@ -1610,44 +1628,26 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 150)
 
         # 移动到中间
-        by_cmd.send_distance_x(10, 295)
         time.sleep(1)
         by_cmd.send_angle_claw(15)
         by_cmd.send_angle_claw_arm(225)
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 80)
         time.sleep(1)
+
+        # 先移动到第一个人的地方
+        by_cmd.send_distance_x(10, self.pos_gap1)
+        time.sleep(1.5)
         if self.target_person == 1:
-            by_cmd.send_distance_x(-10, 150)
-            time.sleep(1.5)
-            car_stop()
-        elif self.target_person == 2:
-            by_cmd.send_distance_x(-10, 50)
-            time.sleep(1.5)
-            car_stop()
-        elif self.target_person == 3:
-            by_cmd.send_distance_x(10, 50)
-            time.sleep(1.5)
-            car_stop()
+            # target_distance = self.pos_gap1
+            pass
         else:
-            by_cmd.send_distance_x(10, 150)
-            time.sleep(1.5)
-            car_stop()
-
-        # 先移动到第一个人的地方 假动作
-        # by_cmd.send_distance_x(10, self.pos_gap1)
-        # time.sleep(1.5)
-
-        # if self.target_person == 1:
-        #     # target_distance = self.pos_gap1
-        #     pass
-        # else:
-        #     # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        #     target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        #     by_cmd.send_distance_x(10, target_distance)
-        #     logger.info(f"target distance {target_distance}")
-        #     time.sleep(1.5 + (self.target_person - 1) * 0.7 )
-        # car_stop()
+            # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+            target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10
+            by_cmd.send_distance_x(10, target_distance)
+            logger.info(f"target distance {target_distance}")
+            time.sleep(1.5 + (self.target_person - 1) * 0.7 )
+        car_stop()
         
         # by_cmd.send_angle_claw_arm(225)
         # time.sleep(0.5)
@@ -1669,6 +1669,8 @@ class kick_ass():
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass
+        
+       
     def nexec(self):
         pass
     def after(self):

subtask_raw.py

@@ -10,7 +10,6 @@ import toml
 import zmq
 import time
 import variable as var
-import action as act
 import re
 import math
 import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             ret, error = filter.aim_right(label)
         error += offset
         # if ret:
-        if abs(error) <= 8:
+        if abs(error) <= 10:
             car_stop()
             logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
             ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 car_stop()
                 logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
                 
-                ret, box = filter.get(label)
-                while not ret:
-                    ret, box = filter.get(label)
-                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                # 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"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
                 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
+                # 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
@@ -426,8 +425,7 @@ class task_queuem(task):
 class get_block1():
     def init(self):
         var.task_speed = 15
-        act.cmd.camera(0)
-        act.cmd.z2(20, 60, 0)
+        by_cmd.send_angle_camera(0)
         filter.switch_camera(1)
         # if cfg['get_block']['first_block'] == "blue":
         #     self.target_label = tlabel.BBLOCK
@@ -464,7 +462,7 @@ class get_block1():
         logger.info("抓取块")
 
         by_cmd.send_position_axis_z(30, 80)
-        time.sleep(1)
+        time.sleep(1.5)
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
@@ -557,7 +555,7 @@ class put_block():
         ret, box = filter.get(tlabel.HOSPITAL)
         if ret > 0:
             width = box[0][2] - box[0][0]
-            if width > 135:
+            if width > 125: #135
                 return True
         return False
     def exec(self):
@@ -568,7 +566,7 @@ class put_block():
         by_cmd.send_distance_x(10, 100)
         by_cmd.send_position_axis_z(30, 0)
         time.sleep(0.5)
-        by_cmd.send_position_axis_x(1, 50) # 20
+        by_cmd.send_position_axis_x(1, 30) # 20
         time.sleep(1)
         by_cmd.send_angle_claw(63)
         time.sleep(1)
@@ -593,7 +591,7 @@ class put_block():
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 0)
         time.sleep(0.5)
-        by_cmd.send_position_axis_x(1, 50)
+        by_cmd.send_position_axis_x(1, 30)
         time.sleep(1.5)
         by_cmd.send_angle_claw(45)
         time.sleep(1)
@@ -651,7 +649,8 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         for _ in range(3):
-            calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
+            by_cmd.send_position_axis_z(30, 155)
+            calibrate_right_new(tlabel.BBALL, offset = 10, run = True, run_speed = 5)
             logger.info("抓蓝色球")
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
@@ -666,13 +665,13 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_distance_axis_z(30, -40)
             time.sleep(0.5)
-            by_cmd.send_angle_claw_arm(80)
+            by_cmd.send_angle_claw_arm(90)
             time.sleep(0.5)
             by_cmd.send_angle_claw(54)
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
             time.sleep(1)
-            by_cmd.send_position_axis_z(30, 155)
+            # by_cmd.send_position_axis_z(30, 155)
             # 继续向前走
             # by_cmd.send_speed_x(4)
         pass
@@ -710,10 +709,11 @@ class up_tower():
         calibrate_new(tlabel.TOWER, offset = 20, run = True)
         time.sleep(1)
         # calibrate(tlabel.TOWER, 27, False, 6)
-        by_cmd.send_distance_x(-10, 120)
+        by_cmd.send_distance_x(-10, 100)
         time.sleep(1)
         # 上古參數
         by_cmd.send_distance_y(-10, 50) # 80
+        time.sleep(0.5)
         # 6_9 模型參數
         # by_cmd.send_distance_y(-10, 40)
         # 7_12_3 模型參數
@@ -724,6 +724,11 @@ class up_tower():
         # time.sleep(3)
         # by_cmd.send_speed_y(-10)
         # time.sleep(0.15)
+        # 8822
+        # by_cmd.send_distance_y(-10, 50)
+        # 891
+        # by_cmd.send_distance_y(-10, 35)
+        # time.sleep(0.3)
         
         by_cmd.send_angle_zhuan(10)
         time.sleep(12)
@@ -771,7 +776,7 @@ class get_rball():
         # 上古參數
         # by_cmd.send_distance_y(-15, 50) # 50 # 70
         by_cmd.send_distance_y(-15, 40) # 50 # 70
-        time.sleep(1.5)
+        time.sleep(0.5)
         # 6_9 參數
         # by_cmd.send_distance_y(-15, 35)
         # time.sleep(2)
@@ -779,6 +784,12 @@ class get_rball():
         # by_cmd.send_distance_y(-15, 45)
         # time.sleep(2)
         # car_stop()
+        # 8822 参数
+        # by_cmd.send_distance_y(-15, 40)
+        # time.sleep(1.5)
+        # 891 参数
+        # by_cmd.send_distance_y(-15, 40)
+        # time.sleep(0.3)
         calibrate_new(tlabel.RBALL,offset = 44, run = True)
         time.sleep(1)
         logger.info("抓红球")
@@ -826,14 +837,14 @@ class put_bball():
         calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
         # by_cmd.send_distance_x(10, 10)
         # 向左运动
-        by_cmd.send_distance_y(-10, 35)
+        # by_cmd.send_distance_y(-10, 35)
         # by_cmd.send_angle_storage(10)
         # time.sleep(1)
 
         by_cmd.send_angle_storage(50)
         logger.info("把球放篮筐里")
 
-        time.sleep(1)
+        time.sleep(2)
         # by_cmd.send_distance_y(10, 55)
         by_cmd.send_angle_storage(20)
         # time.sleep(1)
@@ -1018,7 +1029,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1030,7 +1041,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1078,7 +1089,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1090,7 +1101,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1102,14 +1113,14 @@ class put_hanoi2():
             time.sleep(1)
             pass
         # 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)
+        ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 140)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1121,7 +1132,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 140)
+            by_cmd.send_position_axis_z(30, 150)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1139,7 +1150,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1151,7 +1162,7 @@ class put_hanoi2():
             time.sleep(2)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1163,14 +1174,14 @@ class put_hanoi2():
             time.sleep(2)
             pass
         # 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)
+        ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
         if not ret:
             logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
             return
         time.sleep(0.5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 200) # 170 #190(new)
+            by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1186,7 +1197,7 @@ class put_hanoi2():
             # car_stop()
             pass
         else:
-            by_cmd.send_position_axis_z(30, 200)
+            by_cmd.send_position_axis_z(30, 210)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1537,6 +1548,16 @@ class move_area2():
                 if len(resp_commands) == 0:
                     return
                 action_list = resp_commands
+
+                # 先检查一下 action 是否生成正确，如果不正确直接跳过
+                actions_keys = self.action_dict.keys()
+                try:
+                    for action in action_list:
+                        if not (action.get('action') in actions_keys):
+                            return
+                except:
+                    return
+                
                 # 进入停车区域
                 by_cmd.send_distance_y(10, 450)
                 time.sleep((450 * 5 / 1000) + 0.5)
@@ -1607,44 +1628,26 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 150)
 
         # 移动到中间
-        by_cmd.send_distance_x(10, 295)
         time.sleep(1)
         by_cmd.send_angle_claw(15)
         by_cmd.send_angle_claw_arm(225)
         time.sleep(1)
         by_cmd.send_position_axis_z(30, 80)
         time.sleep(1)
+
+        # 先移动到第一个人的地方
+        by_cmd.send_distance_x(10, self.pos_gap1)
+        time.sleep(1.5)
         if self.target_person == 1:
-            by_cmd.send_distance_x(-10, 150)
-            time.sleep(1.5)
-            car_stop()
-        elif self.target_person == 2:
-            by_cmd.send_distance_x(-10, 50)
-            time.sleep(1.5)
-            car_stop()
-        elif self.target_person == 3:
-            by_cmd.send_distance_x(10, 50)
-            time.sleep(1.5)
-            car_stop()
+            # target_distance = self.pos_gap1
+            pass
         else:
-            by_cmd.send_distance_x(10, 150)
-            time.sleep(1.5)
-            car_stop()
-
-        # 先移动到第一个人的地方 假动作
-        # by_cmd.send_distance_x(10, self.pos_gap1)
-        # time.sleep(1.5)
-
-        # if self.target_person == 1:
-        #     # target_distance = self.pos_gap1
-        #     pass
-        # else:
-        #     # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        #     target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        #     by_cmd.send_distance_x(10, target_distance)
-        #     logger.info(f"target distance {target_distance}")
-        #     time.sleep(1.5 + (self.target_person - 1) * 0.7 )
-        # car_stop()
+            # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+            target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10
+            by_cmd.send_distance_x(10, target_distance)
+            logger.info(f"target distance {target_distance}")
+            time.sleep(1.5 + (self.target_person - 1) * 0.7 )
+        car_stop()
         
         # by_cmd.send_angle_claw_arm(225)
         # time.sleep(0.5)
@@ -1666,6 +1669,8 @@ class kick_ass():
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass
+        
+       
     def nexec(self):
         pass
     def after(self):

test/test_filter.py

@@ -25,10 +25,11 @@ filter.switch_camera(1)
 # label = tlabel.TPLATFORM
 while True:
     time.sleep(0.2)
-    ret, box = filter.get(tlabel.SIGN)
+    ret, box = filter.get(tlabel.HOSPITAL)
     if ret:
+        width = box[0][2] - box[0][0]
         error = (box[0][2] + box[0][0] - 320) / 2
-        logger.error(error)
+        logger.error(width)
         # if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180:
         #     # height = box[0][3] - box[0][1]
         #     logger.error(111)

utils.py

@@ -285,7 +285,11 @@ class label_filter:
                 else:
                     target_bool = False
                     target_box = None
-                expect_boxes = (results[:, 0] == label.value)
+                if len(label) == 1:
+                    expect_boxes = (results[:, 0] == label[0].value)
+                elif len(label) == 2:
+                    expect_boxes = np.logical_or(results[:, 0] == label[0].value, results[:, 0] == label[1].value)
+
                 boxes = results[expect_boxes, :]
                 # 在此处过滤
                 if len(boxes) != 0: