diff --git a/cfg_main.toml b/cfg_main.toml index 2c7fa4d..6d76109 100644 --- a/cfg_main.toml +++ b/cfg_main.toml @@ -15,7 +15,7 @@ MoveArea_enable = true # 应急避险使能 KickAss_enable = true # 扫黑除暴使能 [find_counts] -GetBlock_counts = 10 # 人员施救计数 +GetBlock_counts = 5 # 人员施救计数 PutBlock_counts = 5 # 紧急转移计数 GetBBall_counts = 1 # 整装上阵计数 UpTower_counts = 3 # 通信抢修计数 diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 3d43aca..7748e92 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -6,8 +6,8 @@ pid_ki = 0 pid_kd = 0 # *第一个抓取的方块 -first_block = "red" -# first_block = "blue" +# first_block = "red" +first_block = "blue" ################################################ [put_block] @@ -33,7 +33,7 @@ pid_kd = 0 ################################################ [get_rball] # pid 参数值 -pid_kp = 0.4 +pid_kp = 0.8 pid_ki = 0 pid_kd = 0 @@ -88,5 +88,5 @@ pid_kd = 0 pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 pos_gap2 = 80 # person 之间的距离 -target_person = 4 # 击打的人 - 最靠近标识牌的为 1 +target_person = 2 # 击打的人 - 最靠近标识牌的为 1 ################################################ diff --git a/subtask.py b/subtask.py index 82b4c1f..f35f924 100644 --- a/subtask.py +++ b/subtask.py @@ -187,7 +187,6 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5): ret, box = filter.get(label) while not ret: ret, box = filter.get(label) - print(box) error = (box[0][2] + box[0][0] - 320) / 2 + offset logger.info(f"停车后像素误差:{error}") @@ -347,9 +346,7 @@ class get_block1(): # act.axis.exec() by_cmd.send_position_axis_z(30, 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) @@ -366,7 +363,7 @@ class get_block1(): by_cmd.send_position_axis_z(30, 20) time.sleep(0.5) by_cmd.send_position_axis_x(1, 40) - time.sleep(2) + time.sleep(1) by_cmd.send_angle_claw(45) time.sleep(0.5) by_cmd.send_position_axis_z(30, 80) @@ -407,10 +404,10 @@ class get_block2(): time.sleep(0.5) by_cmd.send_angle_claw_arm(220) by_cmd.send_angle_claw(53) - by_cmd.send_position_axis_x(1, 20) - time.sleep(1) by_cmd.send_position_axis_z(30, 60) time.sleep(1) + by_cmd.send_position_axis_x(1, 20) + time.sleep(1) by_cmd.send_angle_claw(30) by_cmd.send_beep(1) time.sleep(0.5) @@ -465,6 +462,8 @@ class put_block(): time.sleep(2) by_cmd.send_angle_claw_arm(36) time.sleep(1) + by_cmd.send_angle_claw(45) + time.sleep(1) by_cmd.send_position_axis_z(30, 20) time.sleep(0.5) by_cmd.send_position_axis_x(1, 40) @@ -472,8 +471,9 @@ class put_block(): by_cmd.send_angle_claw(30) time.sleep(0.5) by_cmd.send_position_axis_z(30, 140) - time.sleep(3) + time.sleep(2) by_cmd.send_angle_claw_arm(220) + time.sleep(1) by_cmd.send_distance_x(-10, 110) by_cmd.send_position_axis_z(30, 0) time.sleep(2) @@ -489,11 +489,13 @@ class put_block(): def after(self): var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"]) # 下一动作预备位置 - by_cmd.send_position_axis_z(30, 130) + while by_cmd.send_position_axis_z(30, 135) == -1: + pass time.sleep(1) - by_cmd.send_position_axis_x(1, 0) - time.sleep(1) - by_cmd.send_angle_claw_arm(36) + while by_cmd.send_position_axis_x(1, 0) == -1: + pass + while by_cmd.send_angle_claw_arm(36) == -1: + pass # 任务检查间隔 # time.sleep(2) @@ -503,11 +505,11 @@ class get_bball(): def init(self): while (by_cmd.send_angle_camera(90) == -1): by_cmd.send_angle_camera(90) - by_cmd.send_position_axis_z(30, 135) - 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_position_axis_z(30, 135) + # time.sleep(0.5) + # by_cmd.send_position_axis_x(1, 0) + # time.sleep(2) + # by_cmd.send_angle_claw_arm(36) while (by_cmd.send_angle_storage(0) == -1): by_cmd.send_angle_storage(0) @@ -520,7 +522,8 @@ class get_bball(): self.record = CountRecord(5) def find(self): # 目标检测蓝球 - ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) + ret = filter.find(tlabel.BBALL) + # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关 # if ret: # if self.record(tlabel.BBALL): @@ -546,12 +549,12 @@ class get_bball(): time.sleep(1) by_cmd.send_distance_axis_z(30, 20) time.sleep(1) - by_cmd.send_position_axis_x(1, 0) + by_cmd.send_position_axis_x(1, 40) + time.sleep(0.5) + by_cmd.send_distance_axis_z(30, -40) time.sleep(0.5) by_cmd.send_angle_claw_arm(70) time.sleep(0.5) - by_cmd.send_distance_axis_z(30, -40) - time.sleep(1) by_cmd.send_angle_claw(54) time.sleep(1) by_cmd.send_angle_claw_arm(36) @@ -566,8 +569,9 @@ class get_bball(): var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"]) # 下一动作预备位置 by_cmd.send_angle_claw(30) + time.sleep(0.5) by_cmd.send_position_axis_z(30, 0) - time.sleep(2) + time.sleep(1) # # 任务检查间隔 # time.sleep(1) @@ -589,16 +593,16 @@ class up_tower(): logger.info("找到塔") car_stop() time.sleep(1) - calibrate_new(tlabel.TOWER, offset = 27, run = True) + calibrate_new(tlabel.TOWER, offset = 20, run = True) time.sleep(1) # calibrate(tlabel.TOWER, 27, False, 6) - by_cmd.send_distance_x(-10, 130) + by_cmd.send_distance_x(-10, 120) time.sleep(1) by_cmd.send_distance_y(-10, 50) time.sleep(3) by_cmd.send_angle_zhuan(10) time.sleep(9) - by_cmd.send_distance_y(10, 50) + by_cmd.send_distance_y(10, 60) time.sleep(1) by_cmd.send_angle_zhuan(0) time.sleep(0.5) @@ -658,7 +662,7 @@ class get_rball(): def nexec(self): pass def after(self): - var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"]) + var.pid_turning.set(cfg["get_rball"]["pid_kp"], cfg["get_rball"]["pid_ki"], cfg["get_rball"]["pid_kd"]) # 任务检查间隔 time.sleep(2) @@ -680,15 +684,16 @@ class put_bball(): def exec(self): logger.info("找到篮筐") car_stop() - calibrate_new(tlabel.BASKET,offset = -15, run = True, run_speed = 5.5) - by_cmd.send_distance_y(-10, 65) + calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 5.5) + by_cmd.send_distance_x(10, 20) + by_cmd.send_distance_y(-10, 55) time.sleep(1) by_cmd.send_angle_storage(55) logger.info("把球放篮筐里") time.sleep(2) - by_cmd.send_distance_y(10, 65) + by_cmd.send_distance_y(10, 55) time.sleep(1) by_cmd.send_angle_storage(20) @@ -794,6 +799,7 @@ class put_hanoi1(): def nexec(self): pass def after(self): + var.pid_turning.set(0.8, 0, 0) pass class put_hanoi2(): @@ -841,13 +847,13 @@ class put_hanoi2(): by_cmd.send_angle_claw(63) time.sleep(1) by_cmd.send_angle_claw(90) - time.sleep(2) + time.sleep(1.5) by_cmd.send_angle_claw(45) time.sleep(2) by_cmd.send_distance_axis_z(30, 20) time.sleep(1) - by_cmd.send_position_axis_x(2, 10) - time.sleep(4) + by_cmd.send_position_axis_x(1, 10) + time.sleep(2) pass else: by_cmd.send_position_axis_z(30, 10) @@ -855,20 +861,20 @@ class put_hanoi2(): by_cmd.send_angle_claw(63) time.sleep(1) by_cmd.send_angle_claw(90) - time.sleep(2) + time.sleep(1.5) by_cmd.send_angle_claw(45) time.sleep(2) by_cmd.send_distance_axis_z(30, 20) time.sleep(1) - by_cmd.send_position_axis_x(2, 160) - time.sleep(4) + by_cmd.send_position_axis_x(1, 160) + time.sleep(2) 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_position_axis_x(2, 130) - time.sleep(4) + by_cmd.send_position_axis_x(1, 130) + time.sleep(1.5) by_cmd.send_distance_axis_z(30, -20) time.sleep(1) by_cmd.send_angle_claw(90) @@ -876,8 +882,8 @@ class put_hanoi2(): by_cmd.send_position_axis_x(1, 10) pass else: - by_cmd.send_position_axis_x(2, 40) - time.sleep(4) + by_cmd.send_position_axis_x(1, 40) + time.sleep(1.5) by_cmd.send_distance_axis_z(30, -20) time.sleep(1) by_cmd.send_angle_claw(90) @@ -898,8 +904,8 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_distance_axis_z(30, 20) time.sleep(1) - by_cmd.send_position_axis_x(2, 10) - time.sleep(4) + by_cmd.send_position_axis_x(1, 10) + time.sleep(3) pass else: by_cmd.send_position_axis_z(30, 10) @@ -912,33 +918,35 @@ class put_hanoi2(): time.sleep(2) by_cmd.send_distance_axis_z(30, 20) time.sleep(1) - by_cmd.send_position_axis_x(2, 160) - time.sleep(4) + by_cmd.send_position_axis_x(1, 160) + time.sleep(3) 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_position_axis_z(30, 100) - time.sleep(4) - by_cmd.send_position_axis_x(2, 130) - time.sleep(4) + time.sleep(2) + by_cmd.send_position_axis_x(1, 130) + time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) by_cmd.send_angle_claw(90) - time.sleep(1) + 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) - time.sleep(4) - by_cmd.send_position_axis_x(2, 40) - time.sleep(4) + 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(90) - time.sleep(1) + time.sleep(0.5) by_cmd.send_position_axis_x(1, 160) + time.sleep(1) explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5) time.sleep(1) @@ -949,13 +957,13 @@ class put_hanoi2(): by_cmd.send_angle_claw(63) time.sleep(1) by_cmd.send_angle_claw(85) - time.sleep(2) + time.sleep(1) by_cmd.send_angle_claw(30) - time.sleep(2) + time.sleep(1) by_cmd.send_distance_axis_z(30, 10) time.sleep(1) - by_cmd.send_position_axis_x(2, 10) - time.sleep(4) + by_cmd.send_position_axis_x(1, 10) + time.sleep(2) pass else: by_cmd.send_position_axis_z(30, 10) @@ -963,40 +971,40 @@ class put_hanoi2(): by_cmd.send_angle_claw(63) time.sleep(1) by_cmd.send_angle_claw(85) - time.sleep(2) + time.sleep(1) by_cmd.send_angle_claw(30) - time.sleep(2) + time.sleep(1) by_cmd.send_distance_axis_z(30, 10) time.sleep(1) - by_cmd.send_position_axis_x(2, 160) - time.sleep(4) + by_cmd.send_position_axis_x(1, 160) + time.sleep(2) 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_position_axis_z(30, 170) - time.sleep(4) - by_cmd.send_position_axis_x(2, 130) - time.sleep(4) - by_cmd.send_distance_axis_z(30, -20) - time.sleep(1) - by_cmd.send_angle_claw(90) - time.sleep(0.5) - by_cmd.send_position_axis_x(1, 10) + time.sleep(1.5) + by_cmd.send_position_axis_x(1, 130) time.sleep(2) + by_cmd.send_distance_axis_z(30, -20) + time.sleep(0.5) + by_cmd.send_angle_claw(90) + time.sleep(1) + by_cmd.send_position_axis_x(1, 10) + time.sleep(1.5) pass else: by_cmd.send_position_axis_z(30, 170) - time.sleep(4) - by_cmd.send_position_axis_x(2, 40) - time.sleep(4) - by_cmd.send_distance_axis_z(30, -20) - time.sleep(1) - by_cmd.send_angle_claw(90) - time.sleep(0.5) - by_cmd.send_position_axis_x(1, 160) + time.sleep(1.5) + 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(90) + time.sleep(1) + by_cmd.send_position_axis_x(1, 160) + time.sleep(1.5) # while True: # pass @@ -1007,14 +1015,18 @@ class put_hanoi2(): class put_hanoi3(): def init(self): - by_cmd.send_position_axis_z(30, 130) + while by_cmd.send_position_axis_z(30, 130) == -1: + pass time.sleep(3) logger.info("完成任务，爪回左侧") - by_cmd.send_angle_claw_arm(128) + while by_cmd.send_angle_claw_arm(128) == -1: + pass time.sleep(0.5) - by_cmd.send_position_axis_x(1, 150) + while by_cmd.send_position_axis_x(1, 150) == -1: + pass time.sleep(1) - by_cmd.send_angle_claw_arm(220) + while by_cmd.send_angle_claw_arm(220) == -1: + pass def find(self): time.sleep(1) return True @@ -1072,6 +1084,8 @@ class move_area2(): time.sleep(1) # 进入停车区域 by_cmd.send_distance_y(10, 450) + + time.sleep(3) # TODO 调用大模型 然后执行动作 @@ -1098,6 +1112,10 @@ class kick_ass(): self.pos_gap1 = cfg['kick_ass']['pos_gap1'] self.pos_gap2 = cfg['kick_ass']['pos_gap2'] self.target_person = cfg['kick_ass']['target_person'] + + by_cmd.send_angle_claw(15) + by_cmd.send_position_axis_x(1, 160) + def find(self): ret = filter.find(tlabel.SIGN) if ret: @@ -1123,9 +1141,9 @@ class kick_ass(): by_cmd.send_position_axis_x(1, 160) time.sleep(2) by_cmd.send_position_axis_x(1, 0) - time.sleep(4) - by_cmd.send_position_axis_x(1, 160) time.sleep(3) + by_cmd.send_position_axis_x(1, 160) + time.sleep(2) pass def nexec(self): pass