feat: 增加任务段pid参数设置

cfg_subtask.toml

@@ -1,27 +1,92 @@
+################################################
 [get_block]
-# first_block = "red"
+# pid 参数值
-first_block = "blue"
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+# *第一个抓取的方块
+first_block = "red"
+# first_block = "blue"
+
+################################################
 [put_block]
+# pid 参数值
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
 
+################################################
 [get_bball]
+# pid 参数值
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
 
+################################################
 [up_tower]
+# pid 参数值
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
 
+################################################
 [get_rball]
+# pid 参数值
+pid_kp = 0.6
+pid_ki = 0
+pid_kd = 0
 
+################################################
 [put_bball]
+# pid 参数值
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
 
+################################################
 [put_hanoi1]
+# pid 参数值
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
 
+################################################
 [put_hanoi2]
+# pid 参数值
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+# 距离标定值
 pos_gap = 160 # 标定值，两个放置位置的标定距离 
 pos_lp = 2    # 1\2\3 数字越小越靠近红色置物区
 pos_mp = 1
 
+################################################
+[put_hanoi3]
+# pid 参数值
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+################################################
 [move_area]
+# pid 参数值
+pid_kp = 1.4
+pid_ki = 0
+pid_kd = 0
+
 llm_enable = false # 大模型机器人
 
+################################################
 [kick_ass]
-pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
+# pid 参数值
-pos_gap2 = 80 # person 之间的距离
+pid_kp = 1.2
-target_person = 1
+pid_ki = 0
+pid_kd = 0
+
+pos_gap1 = 150    # 目标牌和第一个 person 之间的距离
+pos_gap2 = 80     # person 之间的距离
+target_person = 4 # 击打的人 - 最靠近标识牌的为 1
+################################################

majtask.py

@@ -4,7 +4,7 @@ import time
 import numpy as np
 from loguru import logger
 import utils
-from utils import PidWrap
+import variable as var
     
 class main_task():
     def __init__(self,by_cmd):
@@ -22,9 +22,9 @@ class main_task():
         self.by_cmd = by_cmd
 
         # 转向 pid
-        # self.pid1 = PidWrap(0.5, 0, 0, output_limits=55) # 1.2
+        # var.pid_turning = PidWrap(0.5, 0, 0, output_limits=55) # 1.2
-        self.pid1 = PidWrap(1.2, 0, 0, output_limits=50) # 1.2
+        # pid 参数在
-        self.pid1.set_target(0)
+        var.pid_turning.set_target(0)
 
     def parse_data(self,data):
         if data.get('code') == 0:
@@ -55,7 +55,7 @@ class main_task():
             self.x = self.x / 3
             self.y = self.y / 3
             self.lane_error = self.x - 160
-            utils.lane_error = self.lane_error # 赋全局变量
+            var.lane_error = self.lane_error # 赋全局变量
             self.error_counts = 0
             self.x = 0
             self.y = 0
@@ -88,11 +88,11 @@ class main_task():
             # else:
             #     speed = 18
 
-            if utils.task_speed != 0:
+            if var.task_speed != 0:
-                speed = utils.task_speed
+                speed = var.task_speed
             self.by_cmd.send_speed_x(speed)
-            # pid_out = self.pid1.get(self.lane_error*0.65)
+            # pid_out = var.pid_turning.get(self.lane_error*0.65)
-            pid_out = self.pid1.get(self.lane_error)
+            pid_out = var.pid_turning.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)

subtask.py

@@ -1,3 +1,9 @@
+'''
+待办事项：
+- nexec 中添加延时，保证重试时不会立即跳入下个任务
+- 确认 nexec 还是直接添加一个任务结束后执行的方法更好，或者两者都保留
+- 医院第二个方块自由落体
+'''
 from enum import Enum
 from loguru import logger
 from utils import label_filter
@@ -8,6 +14,7 @@ import utils
 import toml
 import zmq
 import time
+import variable as var
 
 context = zmq.Context()
 socket = context.socket(zmq.REQ)
@@ -224,8 +231,8 @@ class task_queuem_status(Enum):
     IDEL      = 0
     SEARCHING = 1
     EXECUTING = 2
-
 # 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
+
 class task_queuem(task):
     # task_now = task(None, False)
     def __init__(self, queue):
@@ -275,7 +282,7 @@ class task_queuem(task):
 # 人员施救
 class get_block1():
     def init(self):
-        utils.task_speed = 12
+        var.task_speed = 12
         logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
@@ -370,16 +377,19 @@ class get_block2():
         
         by_cmd.send_distance_x(15, 100)
         time.sleep(2)
+
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
         pass
     def nexec(self):
         # TODO 完成不执行任务的空动作
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
         pass
 
 
 # 紧急转移
 class put_block():
     def init(self):
-        utils.task_speed = 0
+        var.task_speed = 0
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
         logger.info("紧急转移初始化")
@@ -434,6 +444,7 @@ class put_block():
         by_cmd.send_position_axis_x(1, 0)
         time.sleep(1)
         by_cmd.send_angle_claw_arm(36)
+        var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
         pass
     def nexec(self):
         # 下一动作预备位置
@@ -442,6 +453,7 @@ class put_block():
         by_cmd.send_position_axis_x(1, 0)
         by_cmd.send_angle_claw_arm(36)
         time.sleep(4)
+        var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
         pass
 
 # 整装上阵
@@ -507,11 +519,13 @@ class get_bball():
         by_cmd.send_angle_claw(30)
         by_cmd.send_position_axis_z(20, 0)
         time.sleep(2)
+        var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
         pass
     def nexec(self):
         by_cmd.send_angle_claw(30)
         by_cmd.send_position_axis_z(20, 0)
         time.sleep(2)
+        var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
         pass
 
 # 通信抢修
@@ -543,15 +557,17 @@ class up_tower():
         by_cmd.send_distance_y(10, 50)
         time.sleep(1)
         by_cmd.send_angle_zhuan(0)
-        time.sleep(1)
+        time.sleep(0.5)
         # while True:
         #     pass
         # 下一动作预备位置
         by_cmd.send_position_axis_z(20, 0)
+        var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
     def nexec(self):
         # 下一动作预备位置
         by_cmd.send_position_axis_z(20, 0)
         time.sleep(4)
+        var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
         pass
 
 
@@ -570,18 +586,18 @@ class get_rball():
         if ret > 0:
             # TODO 连续两帧才开始减速
             if self.record(tlabel.RBALL):
-                utils.task_speed = 5
+                var.task_speed = 5
             return True
         else:
             return False
     def exec(self):
         logger.info("找到红球")
-        utils.task_speed = 0
+        var.task_speed = 0
         car_stop()
         time.sleep(0.5)
         # 靠近塔
         by_cmd.send_angle_scoop(20)
-        by_cmd.send_distance_y(-15, 42) # 50
+        by_cmd.send_distance_y(-15, 50) # 50
         time.sleep(2)
         calibrate_new(tlabel.RBALL,offset = 44, run = True)
         time.sleep(1)
@@ -596,8 +612,10 @@ class get_rball():
         by_cmd.send_angle_omega(-55,30)
         # while True:
         #     pass
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
         pass
     def nexec(self):
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
         pass
 
 # 派发物资
@@ -631,8 +649,10 @@ class put_bball():
 
         by_cmd.send_angle_storage(20)
 
+        var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
         pass
     def nexec(self):
+        var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
         pass
 
 
@@ -685,9 +705,9 @@ class put_hanoi1():
 
         # 校准 omega
         if error > 0:
-            by_cmd.send_angle_omega(-20,abs(utils.lane_error))
+            by_cmd.send_angle_omega(-20,abs(var.lane_error))
         else:
-            by_cmd.send_angle_omega(20,abs(utils.lane_error))
+            by_cmd.send_angle_omega(20,abs(var.lane_error))
         time.sleep(0.5)
         car_stop()
         time.sleep(0.5)
@@ -757,7 +777,7 @@ class put_hanoi2():
     def find(self):
         ret, box = filter.get(self.target_label)
         if ret:
-            utils.task_speed = 8.5
+            var.task_speed = 8.5
             error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
             if abs(error) < 40:
                 return True
@@ -766,7 +786,7 @@ class put_hanoi2():
     def exec(self):
         
         logger.info(f"direction:{utils.direction.name}")
-        utils.task_speed = 0
+        var.task_speed = 0
         car_stop()
         time.sleep(0.5)
         # by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
@@ -943,8 +963,10 @@ class put_hanoi3():
         return True
     def exec(self):
         by_cmd.send_position_axis_z(20, 100)
+        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
         pass
     def nexec(self):
+        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
         pass
 
 # 应急避险 第一阶段 找目标牌
@@ -963,7 +985,7 @@ class move_area1():
         # 停车 ocr 识别文字 调用大模型
         car_stop()
         time.sleep(2)
-        utils.task_speed = 8
+        var.task_speed = 8
         pass
     def nexec(self):
         pass
@@ -981,7 +1003,7 @@ class move_area2():
                 return 5000
         return False
     def exec(self):
-        utils.task_speed = 0
+        var.task_speed = 0
         logger.info("开始寻找停车区域")
         car_stop()
         calibrate_new(tlabel.SHELTER, offset = 15, run = True)
@@ -998,9 +1020,11 @@ class move_area2():
         by_cmd.send_distance_y(-10, 450)
         time.sleep(3)
         by_cmd.send_position_axis_z(20, 0)
+        var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
         pass
     def nexec(self):
         by_cmd.send_position_axis_z(20, 0)
+        var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
         pass
 
 # 扫黑除暴
@@ -1040,7 +1064,9 @@ class kick_ass():
         time.sleep(4)
         by_cmd.send_position_axis_x(1, 160)
         time.sleep(3)
+        var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
         pass
     def nexec(self):
+        var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
         pass
 

utils.py

@@ -3,6 +3,7 @@ from enum import Enum
 import numpy as np
 import erniebot
 from simple_pid import PID
+from loguru import logger
 
 # 巡线误差
 lane_error = 0
@@ -313,6 +314,7 @@ class PidWrap:
         self.pid_t.Kp = kp
         self.pid_t.Ki = ki
         self.pid_t.Kd = kd
+        logger.info(f"[PID]# 更新 PID 参数：Kp({kp:.2f}) Ki({ki:.2f}) Kd({kd:.2f})")
     def get(self, val_in):
         return self.pid_t(val_in)
 

variable.py

@@ -0,0 +1,12 @@
+from utils import PidWrap
+
+# 巡线误差
+lane_error = 0
+# 进入任务时可以通过修改 task_speed 控制巡线速度
+task_speed = 0
+
+# pid 参数
+pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0}
+
+# 转向 pid 对象
+pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=50)