From 21ac0f773e53e28f52fce9d707235b6899484883 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Wed, 12 Jun 2024 20:33:32 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=88=9D=E6=AD=A5=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E6=8A=93=E5=8F=96=E7=AC=AC=E4=BA=8C=E4=B8=AA=E7=89=A9=E5=9D=97?= =?UTF-8?q?=E5=92=8C=E6=94=BE=E7=BD=AE=E5=8A=A8=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cfg_main.toml | 16 +- cfg_subtask.toml | 3 +- main.py | 6 +- majtask.py | 38 +++-- subtask.py | 393 ++++++++++++++++++++++++++++++++--------------- utils.py | 24 +++ 6 files changed, 328 insertions(+), 152 deletions(-) diff --git a/cfg_main.toml b/cfg_main.toml index 4c7f958..2845c04 100644 --- a/cfg_main.toml +++ b/cfg_main.toml @@ -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 # 扫黑除暴计数 diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 9e950c0..60cc669 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -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 diff --git a/main.py b/main.py index e7999f5..0b9a47c 100644 --- a/main.py +++ b/main.py @@ -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'])) diff --git a/majtask.py b/majtask.py index 9ed03dd..e961e44 100644 --- a/majtask.py +++ b/majtask.py @@ -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): @@ -68,27 +69,34 @@ class main_task(): self.x = 0 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) diff --git a/subtask.py b/subtask.py index 7ebf4ad..e9f1799 100644 --- a/subtask.py +++ b/subtask.py @@ -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 error > 0: - by_cmd.send_distance_x(-10, int(error)) - else: - by_cmd.send_distance_x(10, int(-error)) - return + 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,14 +530,19 @@ 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) + ret = filter.find(tlabel.RBALL) if ret > 0: - utils.task_speed = 5 + # TODO 连续两帧才开始减速 + if self.record(tlabel.RBALL): + utils.task_speed = 5 return True else: return False @@ -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): diff --git a/utils.py b/utils.py index 3a891a5..ea869bf 100644 --- a/utils.py +++ b/utils.py @@ -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