暫存版本（物資盤點切換模型完成，需要重調偏移值）
This commit is contained in:
54
subtask.py
54
subtask.py
@@ -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.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
|
||||
|
||||
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)
|
||||
|
||||
Reference in New Issue
Block a user