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

pref: 优化动作后处理

Browse Source
This commit is contained in:
bmy
2024-06-09 16:48:31 +08:00
parent eb0b01c8af
commit 188127b419
2 changed files with 98 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
main.py
View File

@@ -18,18 +18,19 @@ logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logge
# 向任务队列添加任务
task_queue = queue.Queue()
task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
# task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
# task_queue.put(sb.task(sb.get_block, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
# task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'] and cfg_main['task']['PutBlock_enable']))
task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'] and cfg_main['task']['PutBlock_enable']))
task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable']))
# task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable']))
# task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable']))
# TODO 添加一个空任务用于提前降 z 轴
task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable']))
task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable']))
task_queue.put(sb.task(sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_main['task']['GetBBall_enable'] and cfg_main['task']['PutBBall_enable']))
task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True)) # 无论是否进行任务，检测标识并转向都是必须进行的
task_queue.put(sb.task(sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable']))
# task_queue.put(sb.task(sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable']))
# task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable']))
# task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable']))
task_queue.put(sb.task(sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable']))
task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable']))
task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable']))
# 将任务队列传入调度模块中
task_queuem_t = sb.task_queuem(task_queue)
@@ -44,7 +45,11 @@ worker = threading.Thread(target=worker_thread, daemon=True)
worker.start()
# if (cmd_py_obj.send_angle_camera(180) == -1):
# cmd_py_obj.send_angle_camera(180)
cmd_py_obj.send_distance_axis_x(1, 140)
time.sleep(0.5)
cmd_py_obj.send_angle_storage(20)
time.sleep(0.5)
cmd_py_obj.send_angle_scoop(25)
time.sleep(2)
# 创建主任务

130
subtask.py
View File

@@ -114,16 +114,18 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret:
if abs(error) <= 5:
if abs(error) <= 10: # 5
car_stop()
logger.info("可以停车了")
ret, box = filter.get(label)
while not ret:
ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
if abs(error) > 8:
error = error * 3
error = error * 1.5 # 3
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
@@ -239,6 +241,8 @@ class task_queuem(task):
class get_block():
def init(self):
logger.info("人员施救初始化")
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
filter.switch_camera(1)
if cfg['get_block']['first_block'] == "blue":
self.target_label = tlabel.BBLOCK
@@ -255,8 +259,24 @@ class get_block():
return False
def exec(self):
car_stop()
calibrate_new(self.target_label, offset = 12)
calibrate_new(self.target_label, offset = 12, run = True)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_position_axis_z(15, 70)
by_cmd.send_position_axis_x(1, 140)
time.sleep(4)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20)
time.sleep(2)
by_cmd.send_angle_claw(30)
by_cmd.send_light(1)
by_cmd.send_beep(1)
time.sleep(0.5)
by_cmd.send_light(0)
by_cmd.send_beep(0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 140)
time.sleep(3)
pass
@@ -298,6 +318,8 @@ class get_block():
# 紧急转移
class put_block():
def init(self):
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
logger.info("紧急转移初始化")
socket.send_string("1")
socket.recv()
@@ -313,17 +335,35 @@ class put_block():
logger.info("找到医院")
car_stop()
calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
time.sleep(3)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
by_cmd.send_position_axis_z(15, 0)
time.sleep(2)
by_cmd.send_angle_claw(63)
# time.sleep(2)
# by_cmd.send_position_axis_z(20, 130)
# while True:
# pass
# by_cmd.send_position_axis_z(10, 150)
# time.sleep(3)
# # TODO 切换爪子方向
# by_cmd.send_position_axis_x(2, 140)
# time.sleep(2)
# by_cmd.send_position_axis_z(10, 170)
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 130)
time.sleep(3)
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(36)
pass
def nexec(self):
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 130)
time.sleep(1.5)
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(36)
time.sleep(4)
pass
# 整装上阵
@@ -331,7 +371,7 @@ class get_bball():
def init(self):
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
by_cmd.send_position_axis_z(10, 130)
by_cmd.send_position_axis_z(20, 140)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
@@ -341,7 +381,7 @@ class get_bball():
socket.send_string("1")
socket.recv()
logger.info("整装上阵初始化")
time.sleep(0.5)
# time.sleep(0.5)
def find(self):
# 目标检测蓝球
@@ -354,54 +394,40 @@ class get_bball():
car_stop()
time.sleep(0.5)
for _ in range(3):
calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
explore_calibrate_new(tlabel.BBALL, offset = 27, run_direc = 1, run_speed = 4)
# calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
logger.info("抓蓝色球")
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw(54)
by_cmd.send_position_axis_x(1, 140)
by_cmd.send_position_axis_x(1, 150)
time.sleep(1)
by_cmd.send_angle_claw(70)
time.sleep(1)
by_cmd.send_angle_claw(30)
time.sleep(1)
by_cmd.send_distance_axis_z(10, 20)
by_cmd.send_distance_axis_z(20, 20)
time.sleep(2)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
by_cmd.send_angle_claw_arm(82)
time.sleep(1)
by_cmd.send_angle_claw_arm(67)
time.sleep(0.5)
by_cmd.send_angle_claw(54)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
time.sleep(1)
by_cmd.send_distance_axis_z(10, -20)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(1)
# by_cmd.send_position_axis_z(20, 160)
# time.sleep(2)
# by_cmd.send_position_axis_x(2, 70)
# time.sleep(2)
# by_cmd.send_angle_claw(90)
# time.sleep(0.2)
# by_cmd.send_position_axis_x(2, 0)
# time.sleep(2)
# by_cmd.send_angle_claw(27)
# time.sleep(1)
# by_cmd.send_position_axis_z(20, 180)
# time.sleep(1)
# by_cmd.send_position_axis_x(4, 45)
# time.sleep(1)
# by_cmd.send_position_axis_z(20, 140)
# time.sleep(3)
# by_cmd.send_position_axis_x(2, 140)
# time.sleep(2)
# by_cmd.send_angle_claw(90)
# 下一动作预备位置
by_cmd.send_angle_claw(30)
by_cmd.send_position_axis_z(20, 0)
time.sleep(2)
pass
def nexec(self):
by_cmd.send_angle_claw(30)
by_cmd.send_positioin_axis_z(20, 0)
time.sleep(2)
pass
# 通信抢修
@@ -420,13 +446,19 @@ class up_tower():
logger.info("找到塔")
car_stop()
calibrate_new(tlabel.TOWER, offset = 27, run = True)
by_cmd.send_light(1)
time.sleep(0.5)
by_cmd.send_light(0)
# calibrate(tlabel.TOWER, 27, False, 6)
time.sleep(3)
while True:
pass
# while True:
# pass
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 0)
def nexec(self):
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 0)
time.sleep(4)
pass
@@ -440,21 +472,31 @@ class get_rball():
# 目标检测红球
ret = filter.find(tlabel.RBALL)
if ret > 0:
utils.task_speed = 5
return True
else:
return False
def exec(self):
logger.info("找到红球")
utils.task_speed = 0
car_stop()
calibrate_new(tlabel.RBALL,offset = -4, run = True)
time.sleep(1)
time.sleep(0.5)
# 靠近塔
by_cmd.send_angle_scoop(20)
by_cmd.send_distance_y(-10, 65)
time.sleep(1)
by_cmd.send_distance_x(-10, 80)
calibrate_new(tlabel.RBALL,offset = 50, run = True)
time.sleep(1)
logger.info("抓红球")
while True:
pass
# by_cmd.send_angle_scoop(15)
time.sleep(0.5)
by_cmd.send_position_axis_z(15, 170)
time.sleep(5)
by_cmd.send_angle_scoop(7)
by_cmd.send_distance_y(10, 65)
time.sleep(0.5)
# while True:
# pass
pass
def nexec(self):
pass