btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
bmy
2024-06-15 22:04:11 +08:00
parent db6ec06441
commit ac72d1e3e6
5 changed files with 132 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
cfg_subtask.toml
View File

@@ -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]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
pos_gap2 = 80 # person 之间的距离
target_person = 1
target_person = 4 # 击打的人 - 最靠近标识牌的为 1
################################################

18
majtask.py
View File

@@ -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)

52
subtask.py
View File

@@ -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

2
utils.py
View File

@@ -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)

12
variable.py Normal file
View File

@@ -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)