From ac72d1e3e62229ef485d0d93fafce00a5ed7b2e9 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Sat, 15 Jun 2024 22:04:11 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A2=9E=E5=8A=A0=E4=BB=BB=E5=8A=A1?= =?UTF-8?q?=E6=AE=B5pid=E5=8F=82=E6=95=B0=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cfg_subtask.toml | 75 ++++++++++++++++++++++++++++++++++++++++++++---- majtask.py | 18 ++++++------ subtask.py | 52 ++++++++++++++++++++++++--------- utils.py | 2 ++ variable.py | 12 ++++++++ 5 files changed, 132 insertions(+), 27 deletions(-) create mode 100644 variable.py diff --git a/cfg_subtask.toml b/cfg_subtask.toml index e018101..d5d7a0b 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -1,27 +1,92 @@ +################################################ [get_block] -# first_block = "red" -first_block = "blue" +# pid 参数值 +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 之间的距离 -pos_gap2 = 80 # person 之间的距离 -target_person = 1 +# pid 参数值 +pid_kp = 1.2 +pid_ki = 0 +pid_kd = 0 + +pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 +pos_gap2 = 80 # person 之间的距离 +target_person = 4 # 击打的人 - 最靠近标识牌的为 1 +################################################ diff --git a/majtask.py b/majtask.py index bd30810..7c512f1 100644 --- a/majtask.py +++ b/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 - self.pid1 = PidWrap(1.2, 0, 0, output_limits=50) # 1.2 - self.pid1.set_target(0) + # var.pid_turning = PidWrap(0.5, 0, 0, output_limits=55) # 1.2 + # pid 参数在 + 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: - speed = utils.task_speed + if var.task_speed != 0: + speed = var.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) + # pid_out = var.pid_turning.get(self.lane_error*0.65) + 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) diff --git a/subtask.py b/subtask.py index ff79a2c..8c5a324 100644 --- a/subtask.py +++ b/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 diff --git a/utils.py b/utils.py index aefe444..43e7002 100644 --- a/utils.py +++ b/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) diff --git a/variable.py b/variable.py new file mode 100644 index 0000000..4afc5b2 --- /dev/null +++ b/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) \ No newline at end of file