diff --git a/subtask_69.py b/subtask_69.py index 47a4e9f..2586e3c 100644 --- a/subtask_69.py +++ b/subtask_69.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 * 3 + error = error * 1.5 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: @@ -241,6 +241,49 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): break return True +# 对准应知道是左还是右，右侧需在过滤器中进行翻转 +# flipv 为垂直翻转标志，转右侧开启 +def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5): + stop_error = 0 + error_record = CountRecord(10) + 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_hanoi(target_label, error_label, utils.direction == tlabel.RMARK) + 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_hanoi(target_label, error_label, utils.direction == tlabel.RMARK) + + 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): @@ -406,7 +449,7 @@ class get_block1(): else: var.first_block = tlabel.BBLOCK var.second_block = tlabel.RBLOCK - calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5) + calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5) logger.info("抓取块") by_cmd.send_position_axis_z(30, 60) @@ -421,20 +464,21 @@ class get_block1(): by_cmd.send_angle_claw_arm(175) time.sleep(0.1) - by_cmd.send_position_axis_x(1, 120) + by_cmd.send_position_axis_x(1, 100) time.sleep(1) by_cmd.send_position_axis_z(30, 70) - time.sleep(0.1) + time.sleep(0.5) by_cmd.send_angle_claw(63) time.sleep(0.5) 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) + by_cmd.send_position_axis_z(30, 60) pass def nexec(self): @@ -463,13 +507,12 @@ class get_block2(): return False def exec(self): car_stop() - calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5) + calibrate_new(var.second_block, offset = 16, 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) + time.sleep(0.1) by_cmd.send_position_axis_x(1, 20) time.sleep(1) by_cmd.send_angle_claw(25) @@ -521,7 +564,7 @@ class put_block(): # 放置第二個塊 by_cmd.send_angle_storage(20) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 110) by_cmd.send_position_axis_z(30, 120) time.sleep(1.5) by_cmd.send_angle_claw_arm(180) @@ -535,7 +578,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 +597,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 +612,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 +625,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): @@ -596,10 +640,10 @@ class get_bball(): car_stop() time.sleep(0.5) for _ in range(3): - calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5) + calibrate_right_new(tlabel.BBALL, offset = 18, 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 +659,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) # 继续向前走 @@ -834,30 +878,40 @@ class put_hanoi1(): logger.info("应该向左转") - # # 校准 omega - # if error > 0: - # by_cmd.send_angle_omega(-20,abs(var.lane_error)) - # else: - # by_cmd.send_angle_omega(20,abs(var.lane_error)) - # time.sleep(0.5) - # car_stop() - # time.sleep(0.5) - # by_cmd.send_distance_x(10, 200) - - # by_cmd.send_distance_x(10, 180) - by_cmd.send_distance_x(10, 250) - time.sleep(1) + # 校准 omega + for _ in range(10): + ret, box = filter.get(utils.direction) + if ret: + error = (box[0][2] + box[0][0] - 320) / 2 + by_cmd.send_speed_omega(-error * 0.8) + time.sleep(0.2) car_stop() + + + # 前进 + # by_cmd.send_distance_x(10, 200) + # by_cmd.send_distance_x(10, 180) + # by_cmd.send_distance_x(10, 180) + # time.sleep(1.5) + # car_stop() + + while True: + by_cmd.send_speed_x(8.5) + ret, box = filter.get(utils.direction) + if ret: + if abs(box[0][2] - box[0][0]) > 41: + car_stop() + break # 根据方向初始化执行器位置 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) @@ -866,16 +920,20 @@ 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(-45,238) # by_cmd.send_angle_omega(-55,194) + # by_cmd.send_angle_omega(-45,238) + # by_cmd.send_angle_omega(-45,252) + by_cmd.send_angle_omega(-45,260) 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(45,238) # by_cmd.send_angle_omega(55,194) + # by_cmd.send_angle_omega(45,238) + # by_cmd.send_angle_omega(45,252) + by_cmd.send_angle_omega(45,260) time.sleep(2) while (by_cmd.send_angle_camera(0) == -1): by_cmd.send_angle_camera(0) @@ -884,11 +942,17 @@ 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.3, 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 + # time.sleep(1.5) 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,20 +961,25 @@ 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 + self.offset = 14 # self.platform_offset = -25 - self.platform_offset = -10 - else: - self.offset = 10 - #self.platform_offset = -30 + # self.platform_offset = -19 self.platform_offset = -15 + else: + self.offset = 14 + # self.platform_offset = -30 + # self.platform_offset = -19 + self.platform_offset = -15 + # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异） + time.sleep(1.5) + def find(self): # 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 @@ -935,12 +1004,12 @@ class put_hanoi2(): logger.info("抓大平台") if utils.direction is tlabel.RMARK: by_cmd.send_position_axis_z(30, 10) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(63) time.sleep(2) by_cmd.send_angle_claw(40) time.sleep(0.5) - by_cmd.send_distance_axis_z(30, 20) + by_cmd.send_distance_axis_z(30, 30) time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) time.sleep(1) @@ -952,7 +1021,7 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_angle_claw(40) time.sleep(0.5) - by_cmd.send_distance_axis_z(30, 20) + by_cmd.send_distance_axis_z(30, 30) time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) time.sleep(1) @@ -964,15 +1033,15 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放大平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) time.sleep(1.5) by_cmd.send_distance_axis_z(30, -20) time.sleep(1) by_cmd.send_angle_claw(81) time.sleep(0.5) - by_cmd.send_angle_claw(63) - time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) + time.sleep(0.5) + by_cmd.send_angle_claw(63) time.sleep(1) pass else: @@ -982,9 +1051,9 @@ class put_hanoi2(): time.sleep(1) by_cmd.send_angle_claw(81) time.sleep(0.5) - by_cmd.send_angle_claw(63) - time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) + time.sleep(0.5) + by_cmd.send_angle_claw(63) time.sleep(1) ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5) @@ -995,8 +1064,8 @@ class put_hanoi2(): logger.info("抓中平台") if utils.direction is tlabel.RMARK: by_cmd.send_position_axis_z(30, 10) - by_cmd.send_position_axis_x(1, 130) - by_cmd.send_angle_claw(63) + by_cmd.send_position_axis_x(1, 150) + by_cmd.send_angle_claw(55) time.sleep(2) by_cmd.send_angle_claw(35) time.sleep(0.5) @@ -1008,7 +1077,7 @@ class put_hanoi2(): else: by_cmd.send_position_axis_z(30, 10) by_cmd.send_position_axis_x(1, 40) - by_cmd.send_angle_claw(63) + by_cmd.send_angle_claw(55) time.sleep(2) by_cmd.send_angle_claw(35) time.sleep(0.5) @@ -1017,32 +1086,33 @@ 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") + logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") return time.sleep(0.5) logger.info("放中平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 100) + by_cmd.send_position_axis_z(30, 120) time.sleep(2) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) - by_cmd.send_angle_claw(65) + by_cmd.send_angle_claw(55) time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 100) + by_cmd.send_position_axis_z(30, 120) time.sleep(2) by_cmd.send_position_axis_x(1, 40) time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) - by_cmd.send_angle_claw(65) + by_cmd.send_angle_claw(55) time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) time.sleep(1) @@ -1055,15 +1125,15 @@ class put_hanoi2(): logger.info("抓小平台") if utils.direction is tlabel.RMARK: by_cmd.send_position_axis_z(30, 10) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(50) time.sleep(2) by_cmd.send_angle_claw(27) time.sleep(1) by_cmd.send_distance_axis_z(30, 10) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 10) - time.sleep(1) + by_cmd.send_position_axis_x(1, 0) + time.sleep(2) pass else: by_cmd.send_position_axis_z(30, 10) @@ -1074,19 +1144,20 @@ class put_hanoi2(): time.sleep(1) by_cmd.send_distance_axis_z(30, 10) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 160) - time.sleep(1) + by_cmd.send_position_axis_x(1, 170) + time.sleep(2) 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") + logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") return time.sleep(0.5) logger.info("放小平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 170) + by_cmd.send_position_axis_z(30, 190) # 170 time.sleep(1.5) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) @@ -1094,12 +1165,13 @@ 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(10) + # time.sleep(0.12) + # car_stop() pass else: - by_cmd.send_position_axis_z(30, 170) + by_cmd.send_position_axis_z(30, 190) time.sleep(1.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1111,11 +1183,17 @@ class put_hanoi2(): time.sleep(1.5) # while True: # pass - + by_cmd.send_speed_x(12) + time.sleep(1.5) def nexec(self): 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 = False + if utils.direction is tlabel.RMARK: + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + else: + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + # time.sleep(2) pass class put_hanoi3(): @@ -1130,10 +1208,10 @@ 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: - # pass - # while by_cmd.send_angle_claw(90) == -1: + # while by_cmd.send_angle_claw_arm(225) == -1: # pass + while by_cmd.send_angle_claw(85) == -1: + pass def find(self): time.sleep(1) return True @@ -1209,7 +1287,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 +1465,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)