feat: 日常更新

cfg_main.toml

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

cfg_subtask.toml

@@ -1,5 +1,5 @@
 [get_block]
-first_block = "blue"
+first_block = "red"
 [put_block]
 
 [get_bball]

main.py

@@ -28,6 +28,7 @@ task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'],
 task_queue.put(sb.task(sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_main['task']['GetBBall_enable'] and cfg_main['task']['PutBBall_enable'])) 
 task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True))  # 无论是否进行任务，检测标识并转向都是必须进行的
 task_queue.put(sb.task(sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable'])) 
+task_queue.put(sb.task(sb.put_hanoi3, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable']))
 task_queue.put(sb.task(sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable'])) 
 task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable'])) 
 task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable'])) 
@@ -45,7 +46,7 @@ worker = threading.Thread(target=worker_thread, daemon=True)
 worker.start()  
 # if (cmd_py_obj.send_angle_camera(180) == -1):
 #     cmd_py_obj.send_angle_camera(180)
-cmd_py_obj.send_distance_axis_x(1, 140)
+cmd_py_obj.send_position_axis_x(1, 140)
 time.sleep(0.5)
 cmd_py_obj.send_angle_storage(20)
 time.sleep(0.5)

majtask.py

@@ -11,9 +11,9 @@ class PidWrap:
     def set_target(self, target):
         self.pid_t.setpoint = target
     def set(self, kp, ki, kd):
-        self.pid_t.kp = kp
+        self.pid_t.Kp = kp
-        self.pid_t.ki = ki
+        self.pid_t.Ki = ki
-        self.pid_t.kd = kd
+        self.pid_t.Kd = kd
     def get(self, val_in):
         return self.pid_t(val_in)
     
@@ -33,7 +33,7 @@ class main_task():
         self.by_cmd = by_cmd
 
         # 转向 pid
-        self.pid1 = PidWrap(0.7, 0, 0,output_limits=40)
+        self.pid1 = PidWrap(1, 0, 0, output_limits=40)
         self.pid1.set_target(0)
 
     def parse_data(self,data):
@@ -69,26 +69,33 @@ class main_task():
             self.y = 0
             error_abs = abs(self.lane_error)
             
-            if error_abs < 10:
+            if error_abs > 45:
-                self.pid1.set(0.7, 0, 0)
-                speed = 13
-            elif error_abs > 45:
-                speed = 6
-                self.pid1.set(1.8, 0, 0)
-            elif error_abs > 35:
-                speed = 8
-                self.pid1.set(1.5, 0, 0)
-            elif error_abs > 25:
                 speed = 10
-                self.pid1.set(1, 0, 0)
+                # pid_out = self.lane_error * 1.1
+                # self.pid1.set(1, 0, 0)
+            elif error_abs > 35:
+                speed = 10
+                # pid_out = self.lane_error * 1
+                # self.pid1.set(1, 0, 0)
+            elif error_abs > 25:
+                speed = 13
+                # pid_out = self.lane_error * 1
+                # self.pid1.set(1.8, 0, 0)
+            elif error_abs > 15:
+                speed = 13
+                # pid_out = self.lane_error * 0.8
+                # self.pid1.set(1.8, 0, 0)
             else:
-                self.pid1.set(0.8, 0, 0)
+                speed = 15
-                speed = 11
+                # pid_out = self.lane_error * 0.4
+                # self.pid1.set(0.7, 0, 0)
             if utils.task_speed != 0:
                 speed = utils.task_speed
             self.by_cmd.send_speed_x(speed)
             # pid_out = self.pid1.get(self.lane_error*0.65)
-            pid_out = self.pid1.get(self.lane_error*0.7)
+            pid_out = self.pid1.get(self.lane_error)
+            # pid_out = -pid_out
+            # logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
             self.by_cmd.send_speed_omega(pid_out)
             
         self.socket.send_string("")

subtask.py

@@ -38,8 +38,15 @@ def car_stop():
         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)
     while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("找不到 直接向前")
+            break
         ret, error = filter.aim_right(label)
     
     error += offset
@@ -48,6 +55,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
             by_cmd.send_speed_x(-run_speed)
         else:
             by_cmd.send_speed_x(run_speed)
+        pass
     # 停的位置已经很接近目标，可以直接使用 distance 校准
     else:
         error = error * 3
@@ -62,7 +70,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) <= 5:
+            if abs(error) <= 8:
                 car_stop()
                 logger.info("可以停车了")
 
@@ -262,7 +270,7 @@ class get_block():
         calibrate_new(self.target_label, offset = 12, run = True)
         logger.info("抓取块")
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(15, 70)
+        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)
@@ -338,8 +346,8 @@ class put_block():
         time.sleep(1)
         by_cmd.send_position_axis_x(1, 0)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(15, 0)
+        by_cmd.send_position_axis_z(20, 0)
-        time.sleep(2)
+        time.sleep(3)
         by_cmd.send_angle_claw(63)
         # time.sleep(2)
         # by_cmd.send_position_axis_z(20, 130)
@@ -353,8 +361,9 @@ class put_block():
         # by_cmd.send_position_axis_z(10, 170)
         # 下一动作预备位置
         by_cmd.send_position_axis_z(20, 130)
-        time.sleep(3)
+        time.sleep(4)
         by_cmd.send_position_axis_x(1, 0)
+        time.sleep(1)
         by_cmd.send_angle_claw_arm(36)
         pass
     def nexec(self):
@@ -371,12 +380,14 @@ class get_bball():
     def init(self):
         while (by_cmd.send_angle_camera(90) == -1):
             by_cmd.send_angle_camera(90)
-        by_cmd.send_position_axis_z(20, 140)
+        by_cmd.send_position_axis_z(20, 135)
         time.sleep(0.5)
         by_cmd.send_position_axis_x(1, 0)
         time.sleep(0.5)
         
+        while (by_cmd.send_angle_storage(0) == -1):
             by_cmd.send_angle_storage(0)
+
         # 调试 临时换源
         socket.send_string("1")
         socket.recv()
@@ -394,22 +405,21 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         for _ in range(3):
-            explore_calibrate_new(tlabel.BBALL, offset = 27, run_direc = 1, run_speed = 4)
+            calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
-            # calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
             logger.info("抓蓝色球")
-            
+            time.sleep(0.5)
             by_cmd.send_angle_claw_arm(36)
             by_cmd.send_angle_claw(54)
-            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
             by_cmd.send_angle_claw(70)
             time.sleep(1)
             by_cmd.send_angle_claw(30)
             time.sleep(1)
             by_cmd.send_distance_axis_z(20, 20)
-            time.sleep(2)
-            by_cmd.send_position_axis_x(1, 0)
             time.sleep(1)
+            by_cmd.send_position_axis_x(1, 0)
+            time.sleep(1.5)
             by_cmd.send_angle_claw_arm(67)
             time.sleep(0.5)
             by_cmd.send_angle_claw(54)
@@ -418,7 +428,8 @@ class get_bball():
             time.sleep(1)
             by_cmd.send_distance_axis_z(20, -20)
             time.sleep(1)
-
+            # 继续向前走
+            # by_cmd.send_speed_x(4)
         # 下一动作预备位置
         by_cmd.send_angle_claw(30)
         by_cmd.send_position_axis_z(20, 0)
@@ -426,7 +437,7 @@ class get_bball():
         pass
     def nexec(self):
         by_cmd.send_angle_claw(30)
-        by_cmd.send_positioin_axis_z(20, 0)
+        by_cmd.send_position_axis_z(20, 0)
         time.sleep(2)
         pass
 
@@ -490,7 +501,7 @@ class get_rball():
         logger.info("抓红球")
         # by_cmd.send_angle_scoop(15)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(15, 170)
+        by_cmd.send_position_axis_z(20, 170)
         time.sleep(5)
         by_cmd.send_angle_scoop(7)
         by_cmd.send_distance_y(10, 65)
@@ -507,6 +518,7 @@ class put_bball():
         logger.info("派发物资初始化")
         socket.send_string("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)
     def find(self):
@@ -525,10 +537,12 @@ class put_bball():
         by_cmd.send_angle_storage(55)
         logger.info("把球放篮筐里")
 
-        time.sleep(1)
+        time.sleep(2)
         by_cmd.send_distance_y(10, 65)
         time.sleep(1)
 
+        by_cmd.send_angle_storage(20)
+
         pass
     def nexec(self):
         pass
@@ -564,6 +578,8 @@ class put_hanoi1():
             by_cmd.send_speed_omega(0)
         time.sleep(0.2)
         
+        by_cmd.send_position_axis_z(20, 130)
+
         # 校准牌子
         if utils.direction_right > utils.direction_left:
             ret, error = filter.aim_near(tlabel.RMARK)
@@ -577,6 +593,8 @@ class put_hanoi1():
                 ret, error = filter.aim_near(tlabel.LMARK)
             utils.direction = tlabel.LMARK
             logger.info("应该向左转")
+
+
         # 校准 omega
         if error > 0:
             by_cmd.send_angle_omega(-20,abs(utils.lane_error))
@@ -587,13 +605,20 @@ class put_hanoi1():
         time.sleep(0.5)
         by_cmd.send_distance_x(10, 200)
         time.sleep(0.5)
-        # 根据岔路口切换爪子的方向
-        if utils.direction_right > utils.direction_left:
-            pass
-        else:
         
-            pass
+        # 根据方向初始化执行器位置
+        if utils.direction is tlabel.RMARK:
+            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(215)
+            by_cmd.send_angle_storage(55)
         time.sleep(1.5)
+
+        by_cmd.send_position_axis_z(20, 0)
+
         if utils.direction_right > utils.direction_left:
             utils.direction = tlabel.RMARK
             by_cmd.send_angle_omega(-25,430)
@@ -609,7 +634,6 @@ class put_hanoi1():
         time.sleep(0.5)
         socket.send_string("1")
         socket.recv()
-        pass
     def nexec(self):
         pass
 
@@ -645,6 +669,7 @@ 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()
@@ -653,53 +678,92 @@ class put_hanoi2():
         time.sleep(1)
         explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
         logger.info("抓大平台")
-        time.sleep(5)
+        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(50)
+            time.sleep(4)
+            by_cmd.send_position_axis_x(2, 80)
+            by_cmd.send_distance_axis_z(20, 10)
+        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(90)
+            time.sleep(4)
+            by_cmd.send_angle_claw(50)
+            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)
         logger.info("放大平台")
-        time.sleep(5)
+        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)
+        else:
+            by_cmd.send_position_axis_x(2, 40)
+            time.sleep(4)
+            by_cmd.send_distance_axis_z(20, -20)
+            time.sleep(2)
+            by_cmd.send_angle_claw(90)
+            time.sleep(2)
+            by_cmd.send_position_axis_x(1, 160)
 
         explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  6)
         logger.info("抓中平台")
-        time.sleep(5)
+        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("放中平台")
-        time.sleep(5)
+        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("抓小平台")
-        time.sleep(5)
+        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("放小平台")
-        time.sleep(5)
+        if utils.direction is tlabel.RMARK:
-        while True:
             pass
-        # 往回走一段 然后向前行进校准
-        
-        time.sleep(1)
-        logger.info(f"方向{direction}")
-        
-        if utils.lane_error > 0:
-            by_cmd.send_angle_omega(-20,abs(utils.lane_error))
         else:
-            by_cmd.send_angle_omega(20,abs(utils.lane_error))
+            pass
+        # while True:
+        #     pass
         
+    def nexec(self):
+        pass
+
+class put_hanoi3():
+    def init(self):
+        logger.info("完成任务，爪回左侧")
+        by_cmd.send_angle_claw_arm(128)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 150)
         time.sleep(1)
-        sub_put_hanoi2(tlabel.LPILLER, self.distance_lp, True, True)
+        by_cmd.send_angle_claw_arm(220)
-        time.sleep(5)
+    def find(self):
-        # 对准中蓝色柱体
+        time.sleep(1)
-        sub_put_hanoi2(tlabel.MPILLER, self.distance_mp, True, True)
+        return True
-        time.sleep(5)
+    def exec(self):
-        # 根据 direction 确定移动方向
+        by_cmd.send_position_axis_z(20, 100)
-        # 移动 self.distance_mp 距离
-        # 放置物块
-        # 回移相同距离
-        sub_put_hanoi2(tlabel.SPILLER, self.distance_sp, True, True)
-        time.sleep(5)
-        # 对准小红色柱体
-        # 根据 direction 确定移动方向
-        # 移动 self.distance_sp 距离
-        # 放置物块
         pass
     def nexec(self):
         pass
@@ -754,9 +818,10 @@ class move_area2():
         # 离开停车区域
         by_cmd.send_distance_y(-10, 450)
         time.sleep(3)
-        
+        by_cmd.send_position_axis_z(20, 0)
         pass
     def nexec(self):
+        by_cmd.send_position_axis_z(20, 0)
         pass
 # 扫黑除暴
 class kick_ass():

test/test_filter.py

@@ -17,7 +17,7 @@ logger.info("subtask yolo client init")
 
 filter = label_filter(socket)
 filter.switch_camera(1)
-offset = 15
+offset = 12
 while True:
     time.sleep(0.2)
     # ret1 = filter.get_mult(tlabel.RMARK)
@@ -27,7 +27,7 @@ while True:
     #     if abs(error) < 40:
     #         logger.info('yes')
 
-    label = tlabel.BASKET
+    label = tlabel.RBLOCK
     ret, box = filter.get(label)
     while not ret:
         # 注意这里一定要保证摄像头内有该目标 否则会无限循环