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

暫存版本（物資盤點切換模型完成，需要重調偏移值）

Browse Source
This commit is contained in:
bmy
2024-07-13 00:06:37 +08:00
parent 6d9fb5b42a
commit ac6e055c42
5 changed files with 41 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cfg_subtask.toml
View File

@@ -35,7 +35,7 @@ pid_ki = 0
pid_kd = 0 pid_kd = 0
[put_hanoi2] [put_hanoi2]
pid_kp = 1 pid_kp = 1.0
pid_ki = 0 pid_ki = 0
pid_kd = 0 pid_kd = 0
pos_gap = 160 pos_gap = 160

1
main_upper.py
View File

@@ -45,6 +45,7 @@ def main_func(_queue, _skip_queue):
act.axis.x2(140) act.axis.x2(140)
act.axis.storage(20) act.axis.storage(20)
act.axis.scoop(25) act.axis.scoop(25)
act.axis.claw_arm(225)
act.axis.exec() act.axis.exec()
logger.info(cfg_main) logger.info(cfg_main)

6
majtask.py
View File

@@ -96,7 +96,11 @@ class main_task():
# 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)
if var.switch_lane_model:
self.socket.send_string("1")
resp = self.socket.recv_pyobj()
var.switch_lane_model = False
return
self.socket.send_string("") self.socket.send_string("")
resp = self.socket.recv_pyobj() resp = self.socket.recv_pyobj()
self.parse_data(resp) self.parse_data(resp)

54
subtask.py
View File

@@ -431,10 +431,10 @@ class get_block1():
by_cmd.send_position_axis_z(30, 130) by_cmd.send_position_axis_z(30, 130)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_x(1, 140) by_cmd.send_position_axis_x(1, 140)
by_cmd.send_angle_claw_arm(220) by_cmd.send_angle_claw_arm(225)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_storage(55) # by_cmd.send_angle_storage(55)
time.sleep(1) # time.sleep(1)
pass pass
def nexec(self): def nexec(self):
@@ -466,7 +466,7 @@ class get_block2():
calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5) calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
logger.info("抓取块") logger.info("抓取块")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(220) by_cmd.send_angle_claw_arm(225)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_z(30, 60) by_cmd.send_position_axis_z(30, 60)
time.sleep(1) time.sleep(1)
@@ -535,7 +535,7 @@ class put_block():
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 110) by_cmd.send_position_axis_z(30, 110)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(220) by_cmd.send_angle_claw_arm(225)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
@@ -554,7 +554,7 @@ class put_block():
time.sleep(1) time.sleep(1)
while by_cmd.send_position_axis_x(1, 0) == -1: while by_cmd.send_position_axis_x(1, 0) == -1:
pass pass
while by_cmd.send_angle_claw_arm(36) == -1: while by_cmd.send_angle_claw_arm(45) == -1:
pass pass
# 任务检查间隔 # 任务检查间隔
# time.sleep(2) # time.sleep(2)
@@ -569,7 +569,7 @@ class get_bball():
# time.sleep(0.5) # time.sleep(0.5)
# by_cmd.send_position_axis_x(1, 0) # by_cmd.send_position_axis_x(1, 0)
# time.sleep(2) # time.sleep(2)
# by_cmd.send_angle_claw_arm(36) # by_cmd.send_angle_claw_arm(45)
while (by_cmd.send_angle_storage(0) == -1): while (by_cmd.send_angle_storage(0) == -1):
by_cmd.send_angle_storage(0) by_cmd.send_angle_storage(0)
@@ -582,13 +582,14 @@ class get_bball():
self.record = CountRecord(5) self.record = CountRecord(5)
def find(self): def find(self):
# 目标检测蓝球 # 目标检测蓝球
ret = filter.find(tlabel.BBALL) # ret = filter.find(tlabel.BBALL)
ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL])
# ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
# TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关 # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
# if ret: # if ret:
# if self.record(tlabel.BBALL): # if self.record(tlabel.BBALL):
# return True # return True
if ret: if ret[0] or ret[1]:
return True return True
return False return False
def exec(self): def exec(self):
@@ -599,7 +600,7 @@ class get_bball():
calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5) calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5)
logger.info("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(45)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
by_cmd.send_position_axis_x(1, 160) by_cmd.send_position_axis_x(1, 160)
time.sleep(1.2) time.sleep(1.2)
@@ -615,7 +616,7 @@ class get_bball():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(45)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 135) by_cmd.send_position_axis_z(30, 135)
# 继续向前走 # 继续向前走
@@ -845,19 +846,19 @@ class put_hanoi1():
# by_cmd.send_distance_x(10, 200) # by_cmd.send_distance_x(10, 200)
# by_cmd.send_distance_x(10, 180) # by_cmd.send_distance_x(10, 180)
by_cmd.send_distance_x(10, 250) by_cmd.send_distance_x(10, 180)
time.sleep(1) time.sleep(1.5)
car_stop() car_stop()
# 根据方向初始化执行器位置 # 根据方向初始化执行器位置
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# FIXME 右侧的爪子会被 storage 挡住 # FIXME 右侧的爪子会被 storage 挡住
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(45)
by_cmd.send_angle_storage(0) by_cmd.send_angle_storage(0)
else: else:
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw_arm(220) by_cmd.send_angle_claw_arm(225)
by_cmd.send_angle_storage(55) by_cmd.send_angle_storage(55)
time.sleep(1) time.sleep(1)
@@ -884,11 +885,16 @@ class put_hanoi1():
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) var.switch_lane_model = True
if utils.direction == tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass pass
class put_hanoi2(): class put_hanoi2():
def __init__(self): def __init__(self):
if cfg['put_hanoi2']['first_target'] == "lp": if cfg['put_hanoi2']['first_target'] == "lp":
self.target_label = tlabel.LPILLER self.target_label = tlabel.LPILLER
elif cfg['put_hanoi2']['first_target'] == "mp": elif cfg['put_hanoi2']['first_target'] == "mp":
@@ -897,6 +903,7 @@ class put_hanoi2():
self.target_label = tlabel.SPILLER self.target_label = tlabel.SPILLER
def init(self): def init(self):
logger.info("物资盘点 2 初始化") logger.info("物资盘点 2 初始化")
var.task_speed = 8.5
if utils.direction == tlabel.RMARK: if utils.direction == tlabel.RMARK:
# 15 # 15
self.offset = 19 self.offset = 19
@@ -910,7 +917,6 @@ class put_hanoi2():
# ret, box = filter.get(self.target_label) # ret, box = filter.get(self.target_label)
ret, box = filter.get(tlabel.TPLATFORM) ret, box = filter.get(tlabel.TPLATFORM)
if ret: if ret:
var.task_speed = 8.5
error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
if error > 0: if error > 0:
return True return True
@@ -1094,9 +1100,9 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10) by_cmd.send_position_axis_x(1, 10)
time.sleep(1) time.sleep(1)
by_cmd.send_speed_y(15) # by_cmd.send_speed_y(15)
time.sleep(0.1) # time.sleep(0.1)
car_stop() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 170) by_cmd.send_position_axis_z(30, 170)
@@ -1116,6 +1122,7 @@ class put_hanoi2():
pass pass
def after(self): def after(self):
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
var.switch_lane_model = True
pass pass
class put_hanoi3(): class put_hanoi3():
@@ -1130,7 +1137,7 @@ class put_hanoi3():
while by_cmd.send_position_axis_x(1, 0) == -1: while by_cmd.send_position_axis_x(1, 0) == -1:
pass pass
time.sleep(1) time.sleep(1)
# while by_cmd.send_angle_claw_arm(220) == -1: # while by_cmd.send_angle_claw_arm(225) == -1:
# pass # pass
# while by_cmd.send_angle_claw(90) == -1: # while by_cmd.send_angle_claw(90) == -1:
# pass # pass
@@ -1209,7 +1216,7 @@ class move_area1():
# 任务检查间隔 # 任务检查间隔
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
# time.sleep(1) # time.sleep(1)
by_cmd.send_angle_claw_arm(220) by_cmd.send_angle_claw_arm(225)
pass pass
@@ -1387,11 +1394,12 @@ class kick_ass():
else: else:
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10 target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
by_cmd.send_distance_x(10, target_distance) by_cmd.send_distance_x(10, target_distance)
logger.info(f"target distance {target_distance}") logger.info(f"target distance {target_distance}")
time.sleep(1.5 + (self.target_person - 1) * 0.7 ) time.sleep(1.5 + (self.target_person - 1) * 0.7 )
car_stop() car_stop()
# by_cmd.send_angle_claw_arm(220) # by_cmd.send_angle_claw_arm(225)
# time.sleep(0.5) # time.sleep(0.5)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(3) time.sleep(3)

4
variable.py
View File

@@ -16,4 +16,6 @@ llm_text = ''
skip_llm_task_flag = False skip_llm_task_flag = False
first_block = None first_block = None
second_block = None second_block = None
switch_lane_model = False