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] [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] [put_block]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
################################################
[get_bball] [get_bball]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
################################################
[up_tower] [up_tower]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
################################################
[get_rball] [get_rball]
# pid 参数值
pid_kp = 0.6
pid_ki = 0
pid_kd = 0
################################################
[put_bball] [put_bball]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
################################################
[put_hanoi1] [put_hanoi1]
# pid 参数值
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
################################################
[put_hanoi2] [put_hanoi2]
# pid 参数值
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
# 距离标定值
pos_gap = 160 # 标定值，两个放置位置的标定距离 pos_gap = 160 # 标定值，两个放置位置的标定距离
pos_lp = 2 # 1\2\3 数字越小越靠近红色置物区 pos_lp = 2 # 1\2\3 数字越小越靠近红色置物区
pos_mp = 1 pos_mp = 1
################################################
[put_hanoi3]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
################################################
[move_area] [move_area]
# pid 参数值
pid_kp = 1.4
pid_ki = 0
pid_kd = 0
llm_enable = false # 大模型机器人 llm_enable = false # 大模型机器人
################################################
[kick_ass] [kick_ass]
# pid 参数值
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
pos_gap2 = 80 # 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 import numpy as np
from loguru import logger from loguru import logger
import utils import utils
from utils import PidWrap import variable as var
class main_task(): class main_task():
def __init__(self,by_cmd): def __init__(self,by_cmd):
@@ -22,9 +22,9 @@ class main_task():
self.by_cmd = by_cmd self.by_cmd = by_cmd
# 转向 pid # 转向 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): def parse_data(self,data):
if data.get('code') == 0: if data.get('code') == 0:
@@ -55,7 +55,7 @@ class main_task():
self.x = self.x / 3 self.x = self.x / 3
self.y = self.y / 3 self.y = self.y / 3
self.lane_error = self.x - 160 self.lane_error = self.x - 160
utils.lane_error = self.lane_error # 赋全局变量 var.lane_error = self.lane_error # 赋全局变量
self.error_counts = 0 self.error_counts = 0
self.x = 0 self.x = 0
self.y = 0 self.y = 0
@@ -88,11 +88,11 @@ class main_task():
# else: # else:
# speed = 18 # 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) 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 # pid_out = -pid_out
# logger.debug(f"err={self.lane_error}, pwm out={pid_out}") # logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
self.by_cmd.send_speed_omega(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 enum import Enum
from loguru import logger from loguru import logger
from utils import label_filter from utils import label_filter
@@ -8,6 +14,7 @@ import utils
import toml import toml
import zmq import zmq
import time import time
import variable as var
context = zmq.Context() context = zmq.Context()
socket = context.socket(zmq.REQ) socket = context.socket(zmq.REQ)
@@ -224,8 +231,8 @@ class task_queuem_status(Enum):
IDEL = 0 IDEL = 0
SEARCHING = 1 SEARCHING = 1
EXECUTING = 2 EXECUTING = 2
# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作 # 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
class task_queuem(task): class task_queuem(task):
# task_now = task(None, False) # task_now = task(None, False)
def __init__(self, queue): def __init__(self, queue):
@@ -275,7 +282,7 @@ class task_queuem(task):
# 人员施救 # 人员施救
class get_block1(): class get_block1():
def init(self): def init(self):
utils.task_speed = 12 var.task_speed = 12
logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}") logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(0)
@@ -370,16 +377,19 @@ class get_block2():
by_cmd.send_distance_x(15, 100) by_cmd.send_distance_x(15, 100)
time.sleep(2) time.sleep(2)
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass pass
def nexec(self): def nexec(self):
# TODO 完成不执行任务的空动作 # TODO 完成不执行任务的空动作
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass pass
# 紧急转移 # 紧急转移
class put_block(): class put_block():
def init(self): def init(self):
utils.task_speed = 0 var.task_speed = 0
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(0)
logger.info("紧急转移初始化") logger.info("紧急转移初始化")
@@ -434,6 +444,7 @@ class put_block():
by_cmd.send_position_axis_x(1, 0) by_cmd.send_position_axis_x(1, 0)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(36) 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 pass
def nexec(self): def nexec(self):
# 下一动作预备位置 # 下一动作预备位置
@@ -442,6 +453,7 @@ class put_block():
by_cmd.send_position_axis_x(1, 0) by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(36)
time.sleep(4) time.sleep(4)
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
pass pass
# 整装上阵 # 整装上阵
@@ -507,11 +519,13 @@ class get_bball():
by_cmd.send_angle_claw(30) by_cmd.send_angle_claw(30)
by_cmd.send_position_axis_z(20, 0) by_cmd.send_position_axis_z(20, 0)
time.sleep(2) time.sleep(2)
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
pass pass
def nexec(self): def nexec(self):
by_cmd.send_angle_claw(30) by_cmd.send_angle_claw(30)
by_cmd.send_position_axis_z(20, 0) by_cmd.send_position_axis_z(20, 0)
time.sleep(2) time.sleep(2)
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
pass pass
# 通信抢修 # 通信抢修
@@ -543,15 +557,17 @@ class up_tower():
by_cmd.send_distance_y(10, 50) by_cmd.send_distance_y(10, 50)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_zhuan(0) by_cmd.send_angle_zhuan(0)
time.sleep(1) time.sleep(0.5)
# while True: # while True:
# pass # pass
# 下一动作预备位置 # 下一动作预备位置
by_cmd.send_position_axis_z(20, 0) 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): def nexec(self):
# 下一动作预备位置 # 下一动作预备位置
by_cmd.send_position_axis_z(20, 0) by_cmd.send_position_axis_z(20, 0)
time.sleep(4) time.sleep(4)
var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
pass pass
@@ -570,18 +586,18 @@ class get_rball():
if ret > 0: if ret > 0:
# TODO 连续两帧才开始减速 # TODO 连续两帧才开始减速
if self.record(tlabel.RBALL): if self.record(tlabel.RBALL):
utils.task_speed = 5 var.task_speed = 5
return True return True
else: else:
return False return False
def exec(self): def exec(self):
logger.info("找到红球") logger.info("找到红球")
utils.task_speed = 0 var.task_speed = 0
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
# 靠近塔 # 靠近塔
by_cmd.send_angle_scoop(20) 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) time.sleep(2)
calibrate_new(tlabel.RBALL,offset = 44, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
@@ -596,8 +612,10 @@ class get_rball():
by_cmd.send_angle_omega(-55,30) by_cmd.send_angle_omega(-55,30)
# while True: # while True:
# pass # pass
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass pass
def nexec(self): def nexec(self):
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
pass pass
# 派发物资 # 派发物资
@@ -631,8 +649,10 @@ class put_bball():
by_cmd.send_angle_storage(20) 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 pass
def nexec(self): def nexec(self):
var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
pass pass
@@ -685,9 +705,9 @@ class put_hanoi1():
# 校准 omega # 校准 omega
if error > 0: if error > 0:
by_cmd.send_angle_omega(-20,abs(utils.lane_error)) by_cmd.send_angle_omega(-20,abs(var.lane_error))
else: 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) time.sleep(0.5)
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
@@ -757,7 +777,7 @@ class put_hanoi2():
def find(self): def find(self):
ret, box = filter.get(self.target_label) ret, box = filter.get(self.target_label)
if ret: if ret:
utils.task_speed = 8.5 var.task_speed = 8.5
error = (box[0][2] + box[0][0] - 320) / 2 + self.offset error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
if abs(error) < 40: if abs(error) < 40:
return True return True
@@ -766,7 +786,7 @@ class put_hanoi2():
def exec(self): def exec(self):
logger.info(f"direction:{utils.direction.name}") logger.info(f"direction:{utils.direction.name}")
utils.task_speed = 0 var.task_speed = 0
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
# by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30) # by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
@@ -943,8 +963,10 @@ class put_hanoi3():
return True return True
def exec(self): def exec(self):
by_cmd.send_position_axis_z(20, 100) 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 pass
def nexec(self): def nexec(self):
var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
pass pass
# 应急避险 第一阶段 找目标牌 # 应急避险 第一阶段 找目标牌
@@ -963,7 +985,7 @@ class move_area1():
# 停车 ocr 识别文字 调用大模型 # 停车 ocr 识别文字 调用大模型
car_stop() car_stop()
time.sleep(2) time.sleep(2)
utils.task_speed = 8 var.task_speed = 8
pass pass
def nexec(self): def nexec(self):
pass pass
@@ -981,7 +1003,7 @@ class move_area2():
return 5000 return 5000
return False return False
def exec(self): def exec(self):
utils.task_speed = 0 var.task_speed = 0
logger.info("开始寻找停车区域") logger.info("开始寻找停车区域")
car_stop() car_stop()
calibrate_new(tlabel.SHELTER, offset = 15, run = True) calibrate_new(tlabel.SHELTER, offset = 15, run = True)
@@ -998,9 +1020,11 @@ class move_area2():
by_cmd.send_distance_y(-10, 450) by_cmd.send_distance_y(-10, 450)
time.sleep(3) time.sleep(3)
by_cmd.send_position_axis_z(20, 0) 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 pass
def nexec(self): def nexec(self):
by_cmd.send_position_axis_z(20, 0) 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 pass
# 扫黑除暴 # 扫黑除暴
@@ -1040,7 +1064,9 @@ class kick_ass():
time.sleep(4) time.sleep(4)
by_cmd.send_position_axis_x(1, 160) by_cmd.send_position_axis_x(1, 160)
time.sleep(3) time.sleep(3)
var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
pass pass
def nexec(self): def nexec(self):
var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
pass pass

2
utils.py
View File

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