feat: 增加动作类方法

action.py

@@ -182,33 +182,64 @@ class cmd_cls():
         while bycmd.send_angle_zhuan(angle) == -1:
             pass
         time.sleep(0.5)
+    def wait(self, _time):
+        time.sleep(_time)
 
-#  TODO 轴控制指令类，增加指令后加入动作队列，非阻塞
+# TODO 增加任务队列中非阻塞控制
 class axis_cls():
     def __init__(self) -> None:
         self.axis_queue = queue.Queue()
         self.busy = False
         pass
-    def axis_z(self, _distance, _time_via = -1):
+    def z(self, _distance, _time_via = -1):
         while self.busy is True:
             pass
         self.axis_queue.put(lambda: cmd.z(20, _distance, _time_via))
         pass
-    def axis_z2(self, _position, _time_via = -1):
+    def z2(self, _position, _time_via = -1):
         while self.busy is True:
             pass
         self.axis_queue.put(lambda: cmd.z2(20, _position, _time_via))
         pass
-    def axis_x(self, _distance, _time_via = -1):
+    def x(self, _distance, _time_via = -1):
         while self.busy is True:
             pass
         self.axis_queue.put(lambda: cmd.x(1, _distance, _time_via))
         pass
-    def axis_x2(self, _position, _time_via = -1):
+    def x2(self, _position, _time_via = -1):
         while self.busy is True:
             pass
         self.axis_queue.put(lambda: cmd.x2(1, _position, _time_via))
         pass
+    def camera(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.camera(_angle))
+        pass
+    def claw(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.claw(_angle))
+        pass
+    def claw_arm(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.claw_arm(_angle))
+        pass
+    def scoop(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.scoop(_angle))
+        pass
+    def storage(self, _angle):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.storage(_angle))
+        pass
+    def wait(self, _time):
+        while self.busy is True:
+            pass
+        self.axis_queue.put(lambda: cmd.wait(_time))
     def pop(self):
         self.busy = True
         while self.axis_queue.qsize() > 0:

main.py

@@ -18,11 +18,11 @@ cfg_main = toml.load('cfg_main.toml')
 # 配置日志输出
 logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
 
-act.cmd.camera(180)
+act.axis.camera(180)
-act.cmd.x2(1, 140)
+act.axis.x2(140)
-act.cmd.storage(20)
+act.axis.storage(20)
-act.cmd.scoop(25)
+act.axis.scoop(25)
-
+act.axis.exec()
 
 # 向任务队列添加任务
 task_queue = queue.Queue()

subtask.py

@@ -15,6 +15,7 @@ import toml
 import zmq
 import time
 import variable as var
+import action as act
 
 context = zmq.Context()
 socket = context.socket(zmq.REQ)
@@ -184,6 +185,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 ret, box = filter.get(label)
                 while not ret:
                     ret, box = filter.get(label)
+                    print(box)
                 error = (box[0][2] + box[0][0] - 320) / 2 + offset
                 logger.info(f"停车后像素误差:{error}")
                 
@@ -303,8 +305,7 @@ class get_block1():
     def init(self):
         var.task_speed = 12
         logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
-        while (by_cmd.send_angle_camera(0) == -1):
+        act.cmd.camera(0)
-            by_cmd.send_angle_camera(0)
         filter.switch_camera(1)
         if cfg['get_block']['first_block'] == "blue":
             self.target_label = tlabel.BBLOCK
@@ -323,7 +324,25 @@ class get_block1():
         car_stop()
         calibrate_new(self.target_label, offset = 15, run = True)
         logger.info("抓取块")
-        time.sleep(0.5)
+        # act.axis.wait(0.5)
+        # act.axis.z2(60)
+        # act.axis.x2(100)
+        # act.axis.claw_arm(220)
+        # act.axis.claw(63)
+        # act.axis.x2(20)
+        # act.axis.wait(2)
+        # act.axis.claw(30)
+        # act.axis.x2(80)
+        # act.axis.z2(130)
+        # act.axis.claw_arm(36)
+        # act.axis.z2(20)
+        # act.axis.x2(40)
+        # act.axis.claw(45)
+        # act.axis.z2(80)
+        # act.axis.claw_arm(220)
+        # act.axis.x2(100)
+        # act.axis.exec()
+
         by_cmd.send_position_axis_z(20, 60)
         by_cmd.send_position_axis_x(1, 100)
         time.sleep(1)
@@ -345,7 +364,7 @@ class get_block1():
         time.sleep(0.5)
         by_cmd.send_position_axis_x(1, 40)
         time.sleep(2)
-        by_cmd.send_angle_claw(45)
+        by_cmd.send_angle_claw(30)
         time.sleep(0.5)
         by_cmd.send_position_axis_z(20, 80)
         time.sleep(2)
@@ -416,8 +435,7 @@ class put_block():
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
         logger.info("紧急转移初始化")
-        socket.send_string("1")
+        filter.switch_camera(1)
-        socket.recv()
     def find(self):
         # 目标检测医院
         ret, box = filter.get(tlabel.HOSPITAL)
@@ -450,7 +468,7 @@ class put_block():
         time.sleep(2)
         by_cmd.send_angle_claw(30)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(20, 130)
+        by_cmd.send_position_axis_z(20, 140)
         time.sleep(3)
         by_cmd.send_angle_claw_arm(220)
         by_cmd.send_distance_x(-10, 110)
@@ -491,8 +509,7 @@ class get_bball():
             by_cmd.send_angle_storage(0)
 
         # 调试 临时换源
-        socket.send_string("1")
+        filter.switch_camera(1)
-        socket.recv()
         logger.info("整装上阵初始化")
         # time.sleep(0.5)
         
@@ -518,18 +535,18 @@ class get_bball():
             by_cmd.send_angle_claw(54)
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
-            by_cmd.send_angle_claw(70)
+            by_cmd.send_angle_claw(60)
             time.sleep(1)
             by_cmd.send_angle_claw(30)
             time.sleep(1)
             by_cmd.send_distance_axis_z(20, 20)
             time.sleep(1)
             by_cmd.send_position_axis_x(1, 0)
-            time.sleep(1)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(60)
+            time.sleep(0.5)
             by_cmd.send_distance_axis_z(20, -20)
-            time.sleep(0.5)
+            time.sleep(1)
-            by_cmd.send_angle_claw_arm(67)
-            time.sleep(0.5)
             by_cmd.send_angle_claw(54)
             time.sleep(1)
             by_cmd.send_angle_claw_arm(36)
@@ -594,8 +611,7 @@ class up_tower():
 # 高空排险
 class get_rball():
     def init(self):
-        socket.send_string("1")
+        filter.switch_camera(1)
-        socket.recv()
         logger.info("高空排险初始化")
         while (by_cmd.send_angle_camera(180) == -1):
             by_cmd.send_angle_camera(180)
@@ -645,8 +661,7 @@ class get_rball():
 class put_bball():
     def init(self):
         logger.info("派发物资初始化")
-        socket.send_string("1")
+        filter.switch_camera(1)
-        socket.recv()
         by_cmd.send_position_axis_z(20, 0)
         while (by_cmd.send_angle_camera(90) == -1):
             by_cmd.send_angle_camera(90)
@@ -684,8 +699,7 @@ class put_bball():
 class put_hanoi1():
     def init(self):
         logger.info("物资盘点 1 初始化")
-        socket.send_string("2")
+        filter.switch_camera(2)
-        socket.recv()
     def find(self):
         ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
         if label == tlabel.RMARK:
@@ -770,8 +784,7 @@ class put_hanoi1():
             while (by_cmd.send_angle_camera(0) == -1):
                 by_cmd.send_angle_camera(0)
         time.sleep(0.5)
-        socket.send_string("1")
+        filter.switch_camera(1)
-        socket.recv()
     def nexec(self):
         pass
     def after(self):
@@ -1047,6 +1060,7 @@ class move_area2():
         by_cmd.send_light(0)
         # 离开停车区域
         by_cmd.send_distance_y(-10, 450)
+        time.sleep(4)
         pass
     def nexec(self):
         pass