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
[put_hanoi2]
pid_kp = 1
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
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.storage(20)
act.axis.scoop(25)
act.axis.claw_arm(225)
act.axis.exec()
logger.info(cfg_main)

6
majtask.py
View File

@@ -96,7 +96,11 @@ class main_task():
# pid_out = -pid_out
# logger.debug(f"err={self.lane_error}, pwm out={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("")
resp = self.socket.recv_pyobj()
self.parse_data(resp)

52
subtask.py
View File

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

2
variable.py
View File

@@ -17,3 +17,5 @@ skip_llm_task_flag = False
first_block = None
second_block = None
switch_lane_model = False