diff --git a/cfg_main.toml b/cfg_main.toml index a845f5d..452aa97 100644 --- a/cfg_main.toml +++ b/cfg_main.toml @@ -7,7 +7,7 @@ logger_format = "{time} {level} {message}" [find_counts] GetBlock_counts = 5 PutBlock_counts = 8 -GetBBall_counts = 1 +GetBBall_counts = 5 UpTower_counts = 3 GetRBall_counts = 10 PutBBall_counts = 15 diff --git a/subtask.py b/subtask.py index 41b6f54..73927e9 100644 --- a/subtask.py +++ b/subtask.py @@ -118,7 +118,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5): logger.info(f"calibrate_right_new:停车后的误差是{error}") if abs(error) > 8: logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准") - error = error * 2.3 + error = error * 1.5 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: @@ -240,7 +240,44 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): break return True - +def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5): + stop_error = 0 + error_record = CountRecord(3) + if run_direc == 1: + by_cmd.send_speed_x(run_speed) + else: + by_cmd.send_speed_x(-run_speed) + + if target_label == tlabel.TPLATFORM: + stop_error = 8 + else: + stop_error = 15 + while True: + ret1, ret2, box = filter.get_two(target_label, error_label) + while not ret1: + if not global_skip_queue.empty(): + _ = global_skip_queue.get() + logger.error("跳过 hanoi_calibrate") + return False + if ret2: + if error_record(ret2): + return False + ret1, ret2, box = filter.get_two(target_label, error_label) + + error = (box[0][2] + box[0][0] - 320) / 2 + offset + if ret1: + # 校准速度越大 停车的条件越宽泛 20 15 + if abs(error) <= stop_error: + car_stop() + logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") + + ret, box = filter.get(target_label) + while not ret: + ret, box = filter.get(target_label) + error = (box[0][2] + box[0][0] - 320) / 2 + offset + logger.info(f"停车后像素误差:{error}") + break + return True # 任务类 class task: def __init__(self, name, task_template, find_counts=10, enable=True): @@ -903,6 +940,7 @@ class put_hanoi1(): else: var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) pass + time.sleep(1.5) class put_hanoi2(): def __init__(self): @@ -915,7 +953,7 @@ class put_hanoi2(): self.target_label = tlabel.SPILLER def init(self): logger.info("物资盘点 2 初始化") - var.task_speed = 10 + var.task_speed = 8.5 if utils.direction == tlabel.RMARK: # 15 self.offset = 14 @@ -925,6 +963,9 @@ class put_hanoi2(): self.offset = 14 #self.platform_offset = -30 self.platform_offset = -19 + # 延时,防止过早看到 tplatform(虽然此现象相当少见且诡异) + time.sleep(0.5) + def find(self): # ret, box = filter.get(self.target_label) ret, box = filter.get(tlabel.TPLATFORM) @@ -1035,7 +1076,8 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 160) time.sleep(1) pass - ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5) if not ret: logger.error("跳过物资盘点 2 exec") return @@ -1095,7 +1137,8 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 160) time.sleep(1) pass - ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) if not ret: logger.error("跳过物资盘点 2 exec") return @@ -1156,8 +1199,8 @@ class put_hanoi3(): time.sleep(1) # while by_cmd.send_angle_claw_arm(225) == -1: # pass - # while by_cmd.send_angle_claw(90) == -1: - # pass + while by_cmd.send_angle_claw(85) == -1: + pass def find(self): time.sleep(1) return True diff --git a/utils.py b/utils.py index 1334d07..9b7aa0f 100644 --- a/utils.py +++ b/utils.py @@ -190,6 +190,33 @@ class label_filter: return True return False ''' + description: 查询两个目标 只有 target_label 返回 box + param {*} self + param {*} tlabel + return {[bool]} + ''' + def get_two(self, target_label, label): + response = self.get_resp() + if response['code'] == 0: + ret, results = self.filter_box(response['data']) + if ret: + expect_boxes = (results[:, 0] == target_label.value) + boxes = results[expect_boxes, :] + if len(boxes) != 0: + target_bool = True + target_box = boxes[:, 2:] + else: + target_bool = False + target_box = None + expect_boxes = (results[:, 0] == label.value) + boxes = results[expect_boxes, :] + if len(boxes) != 0: + label_bool = True + else: + label_bool = False + return (target_bool, label_bool, target_box) + return (False, False, None) + ''' description: 判断传入的多目标标签是否存在,存在返回 True param {*} self param {*} tlabel