diff --git a/cfg_main.toml b/cfg_main.toml index 2845c04..2cdaa2d 100644 --- a/cfg_main.toml +++ b/cfg_main.toml @@ -4,19 +4,19 @@ logger_format = "{time} {level} {message}" [task] GetBlock_enable = true # 人员施救使能 -PutBlock_enable = false # 紧急转移使能 -GetBBall_enable = false # 整装上阵使能 -UpTower_enable = false # 通信抢修使能 +PutBlock_enable = true # 紧急转移使能 +GetBBall_enable = true # 整装上阵使能 +UpTower_enable = true # 通信抢修使能 GetRBall_enable = true # 高空排险使能 -PutBBall_enable = false # 派发物资使能 -PutHanoi_enable = false # 物资盘点使能 -MoveArea_enable = false # 应急避险使能 -KickAss_enable = false # 扫黑除暴使能 +PutBBall_enable = true # 派发物资使能 +PutHanoi_enable = true # 物资盘点使能 +MoveArea_enable = true # 应急避险使能 +KickAss_enable = true # 扫黑除暴使能 [find_counts] -GetBlock_counts = 5 # 人员施救计数 +GetBlock_counts = 10 # 人员施救计数 PutBlock_counts = 5 # 紧急转移计数 -GetBBall_counts = 5 # 整装上阵计数 +GetBBall_counts = 1 # 整装上阵计数 UpTower_counts = 3 # 通信抢修计数 GetRBall_counts = 10 # 高空排险计数 @@ -27,4 +27,4 @@ PutHanoi2_counts = 2 # 物资盘点计数 PutHanoi3_counts = 2 # 物资盘点计数 MoveArea1_counts = 6 # 应急避险计数 MoveArea2_counts = 1700 # 应急避险第二阶段计数 -KickAss_counts = 20 # 扫黑除暴计数 +KickAss_counts = 10 # 扫黑除暴计数 diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 60cc669..e018101 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -1,6 +1,6 @@ [get_block] -first_block = "red" -# first_block = "blue" +# first_block = "red" +first_block = "blue" [put_block] [get_bball] @@ -16,7 +16,7 @@ first_block = "red" [put_hanoi2] pos_gap = 160 # 标定值，两个放置位置的标定距离 pos_lp = 2 # 1\2\3 数字越小越靠近红色置物区 -pos_mp = 3 +pos_mp = 1 [move_area] llm_enable = false # 大模型机器人 @@ -24,4 +24,4 @@ llm_enable = false # 大模型机器人 [kick_ass] pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 pos_gap2 = 80 # person 之间的距离 -target_person = 3 +target_person = 1 diff --git a/majtask.py b/majtask.py index e961e44..a6db79e 100644 --- a/majtask.py +++ b/majtask.py @@ -34,7 +34,7 @@ class main_task(): # 转向 pid # 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 = PidWrap(1.2, 0, 0, output_limits=50) # 1.2 self.pid1.set_target(0) def parse_data(self,data): @@ -71,11 +71,11 @@ class main_task(): error_abs = abs(self.lane_error) if error_abs > 50: - speed = 10 + speed = 15 elif error_abs > 45: - speed = 11 + speed = 15 elif error_abs > 35: - speed = 13 + speed = 15 elif error_abs > 25: speed = 15 elif error_abs > 15: diff --git a/subtask.py b/subtask.py index e9f1799..f4cfc46 100644 --- a/subtask.py +++ b/subtask.py @@ -40,6 +40,7 @@ def car_stop(): 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) while not ret: @@ -50,47 +51,43 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5): logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找") break ret, error = filter.aim_right(label) - + error += offset if abs(error) > 10 and run: if error > 0: by_cmd.send_speed_x(-run_speed) else: by_cmd.send_speed_x(run_speed) - pass # 停的位置已经很接近目标，可以直接使用 distance 校准 else: - # 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) while not ret: ret, error = filter.aim_right(label) error += offset - if ret: - if abs(error) <= 8: - car_stop() - logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") - + # if ret: + if abs(error) <= 8: + car_stop() + logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") + ret, error = filter.aim_right(label) + while not ret: + not_found_counts += 1 + if not_found_counts >=30: + return 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)) - else: - by_cmd.send_distance_x(10, int(-error)) - - break + 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)) + else: + by_cmd.send_distance_x(10, int(-error)) + + break ''' description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车 如果停车后 error > 8 则使用 distance 校准 @@ -110,6 +107,7 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5): not_found_counts = 0 error = -320 # error > 0 front run logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找") + break ret, box = filter.get(label) error = (box[0][2] + box[0][0] - 320) / 2 + offset @@ -150,7 +148,7 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5): if abs(error) > 8: logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准") error = error * 3 - logger.error(f"error * 3{error}") + # logger.error(f"error * 3 {error}") if error > 0: by_cmd.send_distance_x(-10, int(error)) else: @@ -172,7 +170,7 @@ 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) <= 10: + if abs(error) <= 20: car_stop() logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车") @@ -181,13 +179,17 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): ret, box = filter.get(label) 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 校准") + + logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") + if abs(error) < 8: error = error * 3 - if error > 0: - by_cmd.send_distance_x(-10, int(error)) - else: - by_cmd.send_distance_x(10, int(-error)) + else: + error = error + if error > 0: + by_cmd.send_distance_x(-10, int(error)) + else: + by_cmd.send_distance_x(10, int(-error)) + break # 任务类 class task: @@ -266,9 +268,10 @@ class task_queuem(task): # 人员施救 class get_block1(): def init(self): + utils.task_speed = 12 logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}") - while (by_cmd.send_angle_camera(180) == -1): - by_cmd.send_angle_camera(180) + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) filter.switch_camera(1) if cfg['get_block']['first_block'] == "blue": self.target_label = tlabel.BBLOCK @@ -325,8 +328,8 @@ class get_block1(): 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) + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) filter.switch_camera(1) if cfg['get_block']['first_block'] == "red": self.target_label = tlabel.BBLOCK @@ -357,6 +360,8 @@ class get_block2(): by_cmd.send_beep(0) time.sleep(0.5) by_cmd.send_position_axis_x(1, 100) + + by_cmd.send_distance_x(15, 100) time.sleep(2) pass def nexec(self): @@ -367,8 +372,9 @@ class get_block2(): # 紧急转移 class put_block(): def init(self): - while (by_cmd.send_angle_camera(180) == -1): - by_cmd.send_angle_camera(180) + utils.task_speed = 0 + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) logger.info("紧急转移初始化") socket.send_string("1") socket.recv() @@ -377,16 +383,19 @@ class put_block(): ret, box = filter.get(tlabel.HOSPITAL) if ret > 0: width = box[0][2] - box[0][0] - if width > 120: + if width > 145: return True return False def exec(self): logger.info("找到医院") car_stop() calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) + + by_cmd.send_distance_x(10, 100) + by_cmd.send_position_axis_z(20, 0) time.sleep(3) - by_cmd.send_position_axis_x(1, 0) + by_cmd.send_position_axis_x(1, 40) time.sleep(1) by_cmd.send_angle_claw(63) time.sleep(1) @@ -404,10 +413,10 @@ class put_block(): 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_distance_x(-10, 110) by_cmd.send_position_axis_z(20, 0) time.sleep(1) - by_cmd.send_position_axis_x(1, 0) + by_cmd.send_position_axis_x(1, 40) time.sleep(2) by_cmd.send_angle_claw(63) time.sleep(3) @@ -447,18 +456,22 @@ class get_bball(): logger.info("整装上阵初始化") # time.sleep(0.5) + self.record = CountRecord(5) def find(self): # 目标检测蓝球 ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) if ret: - return True + if self.record(tlabel.BBALL): + return True + # if ret: + # return True return False def exec(self): logger.info("找到蓝色球") car_stop() time.sleep(0.5) for _ in range(3): - calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4) + calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 4) logger.info("抓蓝色球") time.sleep(0.5) by_cmd.send_angle_claw_arm(36) @@ -472,15 +485,15 @@ class get_bball(): by_cmd.send_distance_axis_z(20, 20) time.sleep(1) by_cmd.send_position_axis_x(1, 0) - time.sleep(1.5) + time.sleep(1) + by_cmd.send_distance_axis_z(20, -20) + time.sleep(0.5) 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(20, -20) - time.sleep(1) # 继续向前走 # by_cmd.send_speed_x(4) # 下一动作预备位置 @@ -515,7 +528,11 @@ class up_tower(): time.sleep(0.5) by_cmd.send_light(0) # calibrate(tlabel.TOWER, 27, False, 6) + by_cmd.send_distance_x(-10, 130) + time.sleep(1) + by_cmd.send_distance_y(-10, 50) time.sleep(3) + # while True: # pass # 下一动作预备位置 @@ -533,8 +550,8 @@ class get_rball(): socket.send_string("1") socket.recv() logger.info("高空排险初始化") - while (by_cmd.send_angle_camera(0) == -1): - by_cmd.send_angle_camera(0) + while (by_cmd.send_angle_camera(180) == -1): + by_cmd.send_angle_camera(180) self.record = CountRecord(3) def find(self): # 目标检测红球 @@ -553,9 +570,9 @@ class get_rball(): time.sleep(0.5) # 靠近塔 by_cmd.send_angle_scoop(20) - by_cmd.send_distance_y(-15, 50) + by_cmd.send_distance_y(-15, 42) # 50 time.sleep(2) - calibrate_new(tlabel.RBALL,offset = 50, run = True) + calibrate_new(tlabel.RBALL,offset = 44, run = True) time.sleep(1) logger.info("抓红球") # by_cmd.send_angle_scoop(15) @@ -565,6 +582,7 @@ class get_rball(): by_cmd.send_angle_scoop(7) by_cmd.send_distance_y(15, 70) time.sleep(1) + by_cmd.send_angle_omega(-55,30) # while True: # pass pass @@ -694,8 +712,8 @@ class put_hanoi1(): # 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) + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) time.sleep(0.5) socket.send_string("1") socket.recv() @@ -743,6 +761,7 @@ class put_hanoi2(): # 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 = 5) + time.sleep(1) logger.info("抓大平台") if utils.direction is tlabel.RMARK: # by_cmd.send_position_axis_z(20, 40) @@ -758,20 +777,21 @@ class put_hanoi2(): # 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, 50) + by_cmd.send_position_axis_z(20, 10) + by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(1) by_cmd.send_angle_claw(90) time.sleep(2) - by_cmd.send_angle_claw(50) + by_cmd.send_angle_claw(45) time.sleep(2) by_cmd.send_distance_axis_z(20, 20) - time.sleep(2) + time.sleep(1) 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 = 5) + time.sleep(1) logger.info("放大平台") if utils.direction is tlabel.RMARK: # by_cmd.send_distance_axis_z(20, -10) @@ -781,7 +801,7 @@ class put_hanoi2(): # by_cmd.send_position_axis_x(1, 0) pass else: - by_cmd.send_position_axis_x(2, 60) + by_cmd.send_position_axis_x(2, 40) time.sleep(4) by_cmd.send_distance_axis_z(20, -20) time.sleep(1) @@ -790,6 +810,7 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 160) explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5) + time.sleep(1) logger.info("抓中平台") if utils.direction is tlabel.RMARK: # by_cmd.send_position_axis_z(20, 40) @@ -805,7 +826,7 @@ class put_hanoi2(): # by_cmd.send_distance_axis_z(20, 10) pass else: - by_cmd.send_position_axis_z(20, 30) + by_cmd.send_position_axis_z(20, 10) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(1) @@ -814,11 +835,12 @@ class put_hanoi2(): by_cmd.send_angle_claw(40) time.sleep(2) by_cmd.send_distance_axis_z(20, 20) - time.sleep(2) + time.sleep(1) by_cmd.send_position_axis_x(2, 160) time.sleep(4) pass explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + time.sleep(1) logger.info("放中平台") if utils.direction is tlabel.RMARK: # by_cmd.send_distance_axis_z(20, -10) @@ -828,7 +850,7 @@ class put_hanoi2(): # by_cmd.send_position_axis_x(1, 0) pass else: - by_cmd.send_distance_axis_z(20, 60) + by_cmd.send_position_axis_z(20, 100) time.sleep(4) by_cmd.send_position_axis_x(2, 40) time.sleep(4) @@ -839,6 +861,7 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 160) explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5) + time.sleep(1) logger.info("抓小平台") if utils.direction is tlabel.RMARK: # by_cmd.send_position_axis_z(20, 40) @@ -854,7 +877,7 @@ class put_hanoi2(): # by_cmd.send_distance_axis_z(20, 10) pass else: - by_cmd.send_position_axis_z(20, 30) + by_cmd.send_position_axis_z(20, 10) by_cmd.send_position_axis_x(1, 40) by_cmd.send_angle_claw(63) time.sleep(1) @@ -862,12 +885,13 @@ class put_hanoi2(): 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_distance_axis_z(20, 10) + time.sleep(1) by_cmd.send_position_axis_x(2, 160) time.sleep(4) pass explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) + time.sleep(1) logger.info("放小平台") if utils.direction is tlabel.RMARK: # by_cmd.send_distance_axis_z(20, -10) @@ -877,7 +901,7 @@ class put_hanoi2(): # by_cmd.send_position_axis_x(1, 0) pass else: - by_cmd.send_distance_axis_z(20, 155) + by_cmd.send_position_axis_z(20, 160) time.sleep(4) by_cmd.send_position_axis_x(2, 40) time.sleep(4) @@ -916,8 +940,8 @@ class put_hanoi3(): class move_area1(): def init(self): logger.info("应急避险第一阶段初始化") - while (by_cmd.send_angle_camera(180) == -1): - by_cmd.send_angle_camera(180) + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) def find(self): ret = filter.find(tlabel.SIGN) if ret: @@ -971,8 +995,8 @@ class move_area2(): # 扫黑除暴 class kick_ass(): def init(self): - while (by_cmd.send_angle_camera(180) == -1): - by_cmd.send_angle_camera(180) + while (by_cmd.send_angle_camera(0) == -1): + by_cmd.send_angle_camera(0) logger.info("扫黑除暴初始化") self.pos_gap1 = cfg['kick_ass']['pos_gap1'] self.pos_gap2 = cfg['kick_ass']['pos_gap2']