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

feat: 初步增加抓取第二个物块和放置动作

Browse Source
This commit is contained in:
bmy
2024-06-12 20:33:32 +08:00
parent 6f426d9576
commit 21ac0f773e
6 changed files with 328 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
cfg_main.toml
View File

@@ -3,28 +3,28 @@ logger_filename = "log/file_{time}.log"
logger_format = "{time} {level} {message}"
[task]
GetBlock_enable = false # 人员施救使能
GetBlock_enable = true # 人员施救使能
PutBlock_enable = false # 紧急转移使能
GetBBall_enable = false # 整装上阵使能
UpTower_enable = false # 通信抢修使能
GetRBall_enable = false # 高空排险使能
GetRBall_enable = true # 高空排险使能
PutBBall_enable = false # 派发物资使能
PutHanoi_enable = false # 物资盘点使能
MoveArea_enable = false # 应急避险使能
KickAss_enable = true # 扫黑除暴使能
KickAss_enable = false # 扫黑除暴使能
[find_counts]
GetBlock_counts = 20 # 人员施救计数
PutBlock_counts = 20 # 紧急转移计数
GetBlock_counts = 5 # 人员施救计数
PutBlock_counts = 5 # 紧急转移计数
GetBBall_counts = 5 # 整装上阵计数
UpTower_counts = 10 # 通信抢修计数
UpTower_counts = 3 # 通信抢修计数
GetRBall_counts = 10 # 高空排险计数
PutBBall_counts = 5 # 派发物资计数
PutHanoi1_counts = 18 # 物资盘点计数
PutHanoi1_counts = 10 # 物资盘点计数
PutHanoi2_counts = 2 # 物资盘点计数
PutHanoi3_counts = 2 # 物资盘点计数
MoveArea1_counts = 10 # 应急避险计数
MoveArea1_counts = 6 # 应急避险计数
MoveArea2_counts = 1700 # 应急避险第二阶段计数
KickAss_counts = 20 # 扫黑除暴计数

3
cfg_subtask.toml
View File

@@ -1,5 +1,6 @@
[get_block]
first_block = "red"
# first_block = "blue"
[put_block]
[get_bball]
@@ -23,4 +24,4 @@ llm_enable = false # 大模型机器人
[kick_ass]
pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
pos_gap2 = 80 # person 之间的距离
target_person = 1
target_person = 3

6
main.py
View File

@@ -18,9 +18,9 @@ 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.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_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable']))
task_queue.put(sb.task(sb.get_block2, 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']))
task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable']))
# TODO 添加一个空任务用于提前降 z 轴
task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable']))

36
majtask.py
View File

@@ -33,7 +33,8 @@ class main_task():
self.by_cmd = by_cmd
# 转向 pid
self.pid1 = PidWrap(1, 0, 0, output_limits=40)
# self.pid1 = PidWrap(0.5, 0, 0, output_limits=55) # 1.2
self.pid1 = PidWrap(1.2, 0, 0, output_limits=55) # 1.2
self.pid1.set_target(0)
def parse_data(self,data):
@@ -69,26 +70,33 @@ class main_task():
self.y = 0
error_abs = abs(self.lane_error)
if error_abs > 45:
speed = 8
# pid_out = self.lane_error * 1.1
# self.pid1.set(1, 0, 0)
if error_abs > 50:
speed = 10
elif error_abs > 45:
speed = 11
elif error_abs > 35:
speed = 13
# pid_out = self.lane_error * 1
# self.pid1.set(1, 0, 0)
elif error_abs > 25:
speed = 15
# pid_out = self.lane_error * 1
# self.pid1.set(1.8, 0, 0)
elif error_abs > 15:
speed = 15
# pid_out = self.lane_error * 0.8
# self.pid1.set(1.8, 0, 0)
else:
speed = 15
# pid_out = self.lane_error * 0.4
# self.pid1.set(0.7, 0, 0)
speed = 18
# lane_model initial
# if error_abs > 50:
# speed = 10
# elif error_abs > 45:
# speed = 11
# elif error_abs > 35:
# speed = 13
# elif error_abs > 25:
# speed = 15
# elif error_abs > 15:
# speed = 15
# else:
# speed = 18
if utils.task_speed != 0:
speed = utils.task_speed
self.by_cmd.send_speed_x(speed)

379
subtask.py
View File

@@ -3,6 +3,7 @@ from loguru import logger
from utils import label_filter
from utils import tlabel
from utils import LLM
from utils import CountRecord
import utils
import toml
import zmq
@@ -37,6 +38,7 @@ def car_stop():
by_cmd.send_speed_x(0)
time.sleep(0.2)
by_cmd.send_speed_omega(0)
# 蓝色球使用
def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
not_found_counts = 0
ret, error = filter.aim_right(label)
@@ -45,7 +47,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
if not_found_counts >= 20:
not_found_counts = 0
error = -320 # error > 0 front run
logger.info("找不到 直接向前")
logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
break
ret, error = filter.aim_right(label)
@@ -58,11 +60,12 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
pass
# 停的位置已经很接近目标，可以直接使用 distance 校准
else:
error = error * 3
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
by_cmd.send_distance_x(10, int(-error))
# logger.info("停车时误差就小于")
# error = error * 3
# if error > 0:
# by_cmd.send_distance_x(-10, int(error))
# else:
# by_cmd.send_distance_x(10, int(-error))
return
while True:
ret, error = filter.aim_right(label)
@@ -72,13 +75,15 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
if ret:
if abs(error) <= 8:
car_stop()
logger.info("可以停车了")
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label)
while not ret:
ret, error = filter.aim_right(label)
error += offset
logger.info(f"calibrate_right_new:停车后的误差是{error}")
if abs(error) > 8:
logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3
if error > 0:
by_cmd.send_distance_x(-10, int(error))
@@ -97,8 +102,14 @@ param {*} run_speed
return {*}
'''
def calibrate_new(label, offset, run = True, run_speed = 3.5):
not_found_counts = 0
ret, box = filter.get(label)
while not ret:
not_found_counts += 1
if not_found_counts >= 20:
not_found_counts = 0
error = -320 # error > 0 front run
logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
@@ -109,12 +120,16 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
by_cmd.send_speed_x(run_speed)
# 停的位置已经很接近目标，可以直接使用 distance 校准
else:
error = error * 3
if abs(error) > 8:
logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
# error = error # 3
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
by_cmd.send_distance_x(10, int(-error))
return
logger.info(f"calibrate_new:停车后误差{error}小于 8 不校准")
return
while True:
ret, box = filter.get(label)
while not ret:
@@ -124,16 +139,18 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
if ret:
if abs(error) <= 10: # 5
car_stop()
logger.info("可以停车了")
logger.info("calibrate_new:行进时 误差小于 10 直接停车")
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}")
logger.info(f"calibrate_new:停车后的误差是{error}")
if abs(error) > 8:
error = error * 1.5 # 3
logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3
logger.error(f"error * 3{error}")
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
@@ -155,9 +172,9 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret:
# 校准速度越大 停车的条件越宽泛
if abs(error) <= 15:
if abs(error) <= 10:
car_stop()
logger.info("可以停车了")
logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车")
ret, box = filter.get(label)
while not ret:
@@ -165,6 +182,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
if abs(error) > 8:
logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3
if error > 0:
by_cmd.send_distance_x(-10, int(error))
@@ -246,16 +264,73 @@ class task_queuem(task):
return True
# 人员施救
class get_block():
class get_block1():
def init(self):
logger.info("人员施救初始化")
logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
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
self.another_label = tlabel.RBLOCK
cfg['get_block']['first_block'] = ''
else:
self.target_label = tlabel.RBLOCK
self.another_label = tlabel.BBLOCK
cfg['get_block']['first_block'] = "red"
def find(self):
# 目标检测红/蓝方块
ret = filter.find(self.target_label)
if ret > 0:
return True
return False
def exec(self):
car_stop()
calibrate_new(self.target_label, offset = 15, run = True)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 60)
by_cmd.send_position_axis_x(1, 100)
time.sleep(1)
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_beep(1)
time.sleep(0.5)
by_cmd.send_beep(0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 80)
by_cmd.send_position_axis_z(20, 130)
time.sleep(2)
by_cmd.send_angle_claw_arm(36)
time.sleep(1)
by_cmd.send_position_axis_z(20, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(45)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 80)
time.sleep(2)
by_cmd.send_angle_claw_arm(220)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 100)
time.sleep(1)
pass
def nexec(self):
# TODO 完成不执行任务的空动作
pass
class get_block2():
def init(self):
logger.info(f"人员施救 2 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
filter.switch_camera(1)
if cfg['get_block']['first_block'] == "red":
self.target_label = tlabel.BBLOCK
self.another_label = tlabel.RBLOCK
else:
self.target_label = tlabel.RBLOCK
self.another_label = tlabel.BBLOCK
@@ -267,57 +342,23 @@ class get_block():
return False
def exec(self):
car_stop()
calibrate_new(self.target_label, offset = 12, run = True)
calibrate_new(self.target_label, offset = 15, run = True)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 60)
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)
time.sleep(1)
by_cmd.send_position_axis_z(20, 60)
time.sleep(1)
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)
by_cmd.send_position_axis_x(1, 100)
time.sleep(2)
pass
# 调试 临时注释掉
# calibrate(tlabel.RBLOCK,15)
# time.sleep(2)
# by_cmd.send_position_axis_z(10, 150)
# time.sleep(5)
# by_cmd.send_angle_claw_arm(127)
# time.sleep(1)
# by_cmd.send_position_axis_x(4, 140)
# time.sleep(4)
# by_cmd.send_angle_claw_arm(220)
# by_cmd.send_angle_claw(90)
# time.sleep(1)
# by_cmd.send_distance_axis_z(10, -70)
# time.sleep(3)
# by_cmd.send_angle_claw(27)
# by_cmd.send_distance_axis_z(10, 10)
# time.sleep(2)
# by_cmd.send_distance_axis_x(4, -100)
# time.sleep(1)
# by_cmd.send_distance_axis_z(10, -40)
# time.sleep(3)
# by_cmd.send_angle_claw(35)
# time.sleep(1)
# by_cmd.send_position_axis_z(10, 150)
# time.sleep(3)
# by_cmd.send_position_axis_x(2, 140)
# # 抓取第二个块后 收爪
# time.sleep(3)
# by_cmd.send_position_axis_x(4, 0)
def nexec(self):
# TODO 完成不执行任务的空动作
pass
@@ -343,22 +384,34 @@ class put_block():
logger.info("找到医院")
car_stop()
calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 0)
time.sleep(3)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
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)
time.sleep(1)
by_cmd.send_position_axis_x(1, 80)
by_cmd.send_position_axis_z(20, 130)
time.sleep(2)
by_cmd.send_angle_claw_arm(36)
time.sleep(1)
by_cmd.send_position_axis_z(20, 20)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(30)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 130)
time.sleep(3)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_distance_x(-10, 120)
by_cmd.send_position_axis_z(20, 0)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(2)
by_cmd.send_angle_claw(63)
time.sleep(3)
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 130)
time.sleep(4)
@@ -396,7 +449,7 @@ class get_bball():
def find(self):
# 目标检测蓝球
ret = filter.find(tlabel.BBALL)
ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
if ret:
return True
return False
@@ -447,6 +500,7 @@ class up_tower():
logger.info("通信抢修初始化")
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
filter.switch_camera(1)
def find(self):
# 目标检测通信塔
ret = filter.find(tlabel.TOWER)
@@ -476,13 +530,18 @@ class up_tower():
# 高空排险
class get_rball():
def init(self):
socket.send_string("1")
socket.recv()
logger.info("高空排险初始化")
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
self.record = CountRecord(3)
def find(self):
# 目标检测红球
ret = filter.find(tlabel.RBALL)
if ret > 0:
# TODO 连续两帧才开始减速
if self.record(tlabel.RBALL):
utils.task_speed = 5
return True
else:
@@ -494,8 +553,8 @@ class get_rball():
time.sleep(0.5)
# 靠近塔
by_cmd.send_angle_scoop(20)
by_cmd.send_distance_y(-10, 65)
time.sleep(1)
by_cmd.send_distance_y(-15, 50)
time.sleep(2)
calibrate_new(tlabel.RBALL,offset = 50, run = True)
time.sleep(1)
logger.info("抓红球")
@@ -504,8 +563,8 @@ class get_rball():
by_cmd.send_position_axis_z(20, 170)
time.sleep(5)
by_cmd.send_angle_scoop(7)
by_cmd.send_distance_y(10, 65)
time.sleep(0.5)
by_cmd.send_distance_y(15, 70)
time.sleep(1)
# while True:
# pass
pass
@@ -609,12 +668,13 @@ class put_hanoi1():
# 根据方向初始化执行器位置
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_storage(0)
else:
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_angle_claw_arm(217)
by_cmd.send_angle_storage(55)
time.sleep(1.5)
@@ -622,13 +682,17 @@ class put_hanoi1():
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
by_cmd.send_angle_omega(-25,430)
# by_cmd.send_angle_omega(-25,430)
# by_cmd.send_angle_omega(-45,238)
by_cmd.send_angle_omega(-55,194)
time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
else:
utils.direction = tlabel.LMARK
by_cmd.send_angle_omega(25,430)
# by_cmd.send_angle_omega(25,430)
# by_cmd.send_angle_omega(45,238)
by_cmd.send_angle_omega(55,194)
time.sleep(2)
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
@@ -641,10 +705,7 @@ class put_hanoi1():
class put_hanoi2():
def __init__(self):
if utils.direction_right > utils.direction_left:
self.offset = 25
else:
self.offset = 15
self.pos_lp = cfg['put_hanoi2']['pos_lp']
self.pos_mp = cfg['put_hanoi2']['pos_mp']
self.pos_sp = 6 - self.pos_lp - self.pos_mp
@@ -660,6 +721,10 @@ class put_hanoi2():
logger.info(f"setting hanoi pos_lp[{self.pos_lp}] pos_mp[{self.pos_mp}] pos_sp[{self.pos_sp}]")
def init(self):
logger.info("物资盘点 2 初始化")
if utils.direction == tlabel.RMARK:
self.offset = 25
else:
self.offset = 10
def find(self):
ret, box = filter.get(self.target_label)
if ret:
@@ -670,33 +735,35 @@ class put_hanoi2():
return False
def exec(self):
logger.info(f"direction:{utils.direction.name}")
logger.info(f"offset:{self.offset}")
utils.task_speed = 0
car_stop()
time.sleep(0.5)
# by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
time.sleep(1)
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
logger.info("抓大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(20, 40)
by_cmd.send_position_axis_x(1, 160)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(4)
by_cmd.send_angle_claw(50)
time.sleep(4)
by_cmd.send_position_axis_x(2, 80)
by_cmd.send_distance_axis_z(20, 10)
# by_cmd.send_position_axis_z(20, 40)
# by_cmd.send_position_axis_x(1, 160)
# by_cmd.send_angle_claw(63)
# time.sleep(1)
# by_cmd.send_angle_claw(85)
# time.sleep(4)
# by_cmd.send_angle_claw(50)
# time.sleep(4)
# by_cmd.send_position_axis_x(2, 80)
# # FIXME 多往回收一些 会挡住识别
# by_cmd.send_distance_axis_z(20, 10)
pass
else:
by_cmd.send_position_axis_z(20, 30)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_position_axis_x(1, 50)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(4)
time.sleep(2)
by_cmd.send_angle_claw(50)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 20)
@@ -704,48 +771,122 @@ class put_hanoi2():
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
logger.info("放大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_position_axis_x(2, 160)
# time.sleep(4)
# by_cmd.send_angle_claw(90)
# by_cmd.send_position_axis_x(1, 0)
pass
else:
by_cmd.send_position_axis_x(2, 60)
time.sleep(4)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(1)
by_cmd.send_position_axis_x(1, 160)
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
logger.info("抓中平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40)
# by_cmd.send_position_axis_x(1, 160)
# by_cmd.send_angle_claw(63)
# time.sleep(1)
# by_cmd.send_angle_claw(85)
# time.sleep(4)
# by_cmd.send_angle_claw(37)
# time.sleep(4)
# by_cmd.send_position_axis_x(2, 40)
# # FIXME 多往回收一些 会挡住识别
# by_cmd.send_distance_axis_z(20, 10)
pass
else:
by_cmd.send_position_axis_z(20, 30)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(2)
by_cmd.send_angle_claw(40)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 20)
time.sleep(2)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
by_cmd.send_angle_claw(90)
by_cmd.send_position_axis_x(1, 0)
pass
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_position_axis_x(2, 160)
# time.sleep(4)
# by_cmd.send_angle_claw(90)
# by_cmd.send_position_axis_x(1, 0)
pass
else:
by_cmd.send_distance_axis_z(20, 60)
time.sleep(4)
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(2)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(2)
time.sleep(1)
by_cmd.send_position_axis_x(1, 160)
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
logger.info("抓中平台")
if utils.direction is tlabel.RMARK:
pass
else:
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
pass
else:
pass
explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
logger.info("抓小平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40)
# by_cmd.send_position_axis_x(1, 160)
# by_cmd.send_angle_claw(63)
# time.sleep(1)
# by_cmd.send_angle_claw(85)
# time.sleep(4)
# by_cmd.send_angle_claw(37)
# time.sleep(4)
# by_cmd.send_position_axis_x(2, 40)
# # FIXME 多往回收一些 会挡住识别
# by_cmd.send_distance_axis_z(20, 10)
pass
else:
by_cmd.send_position_axis_z(20, 30)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(2)
by_cmd.send_angle_claw(30)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 20)
time.sleep(2)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
logger.info("放小平台")
if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10)
# by_cmd.send_position_axis_x(2, 160)
# time.sleep(4)
# by_cmd.send_angle_claw(90)
# by_cmd.send_position_axis_x(1, 0)
pass
else:
pass
by_cmd.send_distance_axis_z(20, 155)
time.sleep(4)
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(1)
by_cmd.send_position_axis_x(1, 160)
time.sleep(2)
# while True:
# pass
@@ -754,6 +895,8 @@ class put_hanoi2():
class put_hanoi3():
def init(self):
by_cmd.send_position_axis_z(20, 130)
time.sleep(3)
logger.info("完成任务，爪回左侧")
by_cmd.send_angle_claw_arm(128)
time.sleep(0.5)
@@ -799,7 +942,7 @@ class move_area2():
ret, box = filter.get(tlabel.SHELTER)
if ret:
error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
if abs(error) < 10:
if abs(error) < 20:
return 5000
return False
def exec(self):

24
utils.py
View File

@@ -276,7 +276,31 @@ class LLM:
self.messages.append(self.resp.to_message())
return self.resp.get_result()
class CountRecord:
def __init__(self, stop_count=2) -> None:
self.last_record = None
self.count = 0
self.stop_cout = stop_count
def get_count(self, val):
try:
if val == self.last_record:
self.count += 1
else:
self.count=0
self.last_record = val
return self.count
except Exception as e:
print(e)
def __call__(self, val):
self.get_count(val)
if self.count >= self.stop_cout:
if type(val) == bool:
return val
return True
else:
return False