btl143/project_main
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 通信塔定点停车及其他动作优化

Browse Source
This commit is contained in:
bmy
2024-06-14 11:18:50 +08:00
parent 21ac0f773e
commit 2a3993ea62
4 changed files with 113 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
cfg_main.toml
View File

@@ -4,19 +4,19 @@ logger_format = "{time} {level} {message}"
[task] [task]
GetBlock_enable = true # 人员施救使能 GetBlock_enable = true # 人员施救使能
PutBlock_enable = false # 紧急转移使能 PutBlock_enable = true # 紧急转移使能
GetBBall_enable = false # 整装上阵使能 GetBBall_enable = true # 整装上阵使能
UpTower_enable = false # 通信抢修使能 UpTower_enable = true # 通信抢修使能
GetRBall_enable = true # 高空排险使能 GetRBall_enable = true # 高空排险使能
PutBBall_enable = false # 派发物资使能 PutBBall_enable = true # 派发物资使能
PutHanoi_enable = false # 物资盘点使能 PutHanoi_enable = true # 物资盘点使能
MoveArea_enable = false # 应急避险使能 MoveArea_enable = true # 应急避险使能
KickAss_enable = false # 扫黑除暴使能 KickAss_enable = true # 扫黑除暴使能
[find_counts] [find_counts]
GetBlock_counts = 5 # 人员施救计数 GetBlock_counts = 10 # 人员施救计数
PutBlock_counts = 5 # 紧急转移计数 PutBlock_counts = 5 # 紧急转移计数
GetBBall_counts = 5 # 整装上阵计数 GetBBall_counts = 1 # 整装上阵计数
UpTower_counts = 3 # 通信抢修计数 UpTower_counts = 3 # 通信抢修计数
GetRBall_counts = 10 # 高空排险计数 GetRBall_counts = 10 # 高空排险计数
@@ -27,4 +27,4 @@ PutHanoi2_counts = 2 # 物资盘点计数
PutHanoi3_counts = 2 # 物资盘点计数 PutHanoi3_counts = 2 # 物资盘点计数
MoveArea1_counts = 6 # 应急避险计数 MoveArea1_counts = 6 # 应急避险计数
MoveArea2_counts = 1700 # 应急避险第二阶段计数 MoveArea2_counts = 1700 # 应急避险第二阶段计数
KickAss_counts = 20 # 扫黑除暴计数 KickAss_counts = 10 # 扫黑除暴计数

8
cfg_subtask.toml
View File

@@ -1,6 +1,6 @@
[get_block] [get_block]
first_block = "red" # first_block = "red"
# first_block = "blue" first_block = "blue"
[put_block] [put_block]
[get_bball] [get_bball]
@@ -16,7 +16,7 @@ first_block = "red"
[put_hanoi2] [put_hanoi2]
pos_gap = 160 # 标定值，两个放置位置的标定距离 pos_gap = 160 # 标定值，两个放置位置的标定距离
pos_lp = 2 # 1\2\3 数字越小越靠近红色置物区 pos_lp = 2 # 1\2\3 数字越小越靠近红色置物区
pos_mp = 3 pos_mp = 1
[move_area] [move_area]
llm_enable = false # 大模型机器人 llm_enable = false # 大模型机器人
@@ -24,4 +24,4 @@ llm_enable = false # 大模型机器人
[kick_ass] [kick_ass]
pos_gap1 = 150 # 目标牌和第一个 person 之间的距离 pos_gap1 = 150 # 目标牌和第一个 person 之间的距离
pos_gap2 = 80 # person 之间的距离 pos_gap2 = 80 # person 之间的距离
target_person = 3 target_person = 1

8
majtask.py
View File

@@ -34,7 +34,7 @@ class main_task():
# 转向 pid # 转向 pid
# self.pid1 = PidWrap(0.5, 0, 0, output_limits=55) # 1.2 # 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) self.pid1.set_target(0)
def parse_data(self,data): def parse_data(self,data):
@@ -71,11 +71,11 @@ class main_task():
error_abs = abs(self.lane_error) error_abs = abs(self.lane_error)
if error_abs > 50: if error_abs > 50:
speed = 10 speed = 15
elif error_abs > 45: elif error_abs > 45:
speed = 11 speed = 15
elif error_abs > 35: elif error_abs > 35:
speed = 13 speed = 15
elif error_abs > 25: elif error_abs > 25:
speed = 15 speed = 15
elif error_abs > 15: elif error_abs > 15:

162
subtask.py
View File

@@ -40,6 +40,7 @@ def car_stop():
by_cmd.send_speed_omega(0) by_cmd.send_speed_omega(0)
# 蓝色球使用 # 蓝色球使用
def calibrate_right_new(label, offset, run = True, run_speed = 3.5): def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
not_found_counts = 0 not_found_counts = 0
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
while not ret: while not ret:
@@ -57,40 +58,36 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
by_cmd.send_speed_x(-run_speed) by_cmd.send_speed_x(-run_speed)
else: else:
by_cmd.send_speed_x(run_speed) by_cmd.send_speed_x(run_speed)
pass
# 停的位置已经很接近目标，可以直接使用 distance 校准 # 停的位置已经很接近目标，可以直接使用 distance 校准
else: 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 return
while True: while True:
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
while not ret: while not ret:
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
error += offset error += offset
if ret: # if ret:
if abs(error) <= 8: if abs(error) <= 8:
car_stop() car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") 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) ret, error = filter.aim_right(label)
while not ret: error += offset
ret, error = filter.aim_right(label) logger.info(f"calibrate_right_new:停车后的误差是{error}")
error += offset if abs(error) > 8:
logger.info(f"calibrate_right_new:停车后的误差是{error}") logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
if abs(error) > 8: error = error * 3
logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准") if error > 0:
error = error * 3 by_cmd.send_distance_x(-10, int(error))
if error > 0: else:
by_cmd.send_distance_x(-10, int(error)) by_cmd.send_distance_x(10, int(-error))
else:
by_cmd.send_distance_x(10, int(-error))
break break
''' '''
description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车 description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车
如果停车后 error > 8 则使用 distance 校准 如果停车后 error > 8 则使用 distance 校准
@@ -110,6 +107,7 @@ def calibrate_new(label, offset, run = True, run_speed = 3.5):
not_found_counts = 0 not_found_counts = 0
error = -320 # error > 0 front run error = -320 # error > 0 front run
logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找") logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
break
ret, box = filter.get(label) ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset 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: if abs(error) > 8:
logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准") logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3 error = error * 3
logger.error(f"error * 3{error}") # logger.error(f"error * 3 {error}")
if error > 0: if error > 0:
by_cmd.send_distance_x(-10, int(error)) by_cmd.send_distance_x(-10, int(error))
else: 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 error = (box[0][2] + box[0][0] - 320) / 2 + offset
if ret: if ret:
# 校准速度越大 停车的条件越宽泛 # 校准速度越大 停车的条件越宽泛
if abs(error) <= 10: if abs(error) <= 20:
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车") 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) ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}") 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 error = error * 3
if error > 0: else:
by_cmd.send_distance_x(-10, int(error)) error = error
else: if error > 0:
by_cmd.send_distance_x(10, int(-error)) by_cmd.send_distance_x(-10, int(error))
else:
by_cmd.send_distance_x(10, int(-error))
break break
# 任务类 # 任务类
class task: class task:
@@ -266,9 +268,10 @@ class task_queuem(task):
# 人员施救 # 人员施救
class get_block1(): class get_block1():
def init(self): def init(self):
utils.task_speed = 12
logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}") logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
while (by_cmd.send_angle_camera(180) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(180) by_cmd.send_angle_camera(0)
filter.switch_camera(1) filter.switch_camera(1)
if cfg['get_block']['first_block'] == "blue": if cfg['get_block']['first_block'] == "blue":
self.target_label = tlabel.BBLOCK self.target_label = tlabel.BBLOCK
@@ -325,8 +328,8 @@ class get_block1():
class get_block2(): class get_block2():
def init(self): def init(self):
logger.info(f"人员施救 2 初始化，第一抓取块为 {cfg['get_block']['first_block']}") logger.info(f"人员施救 2 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
while (by_cmd.send_angle_camera(180) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(180) by_cmd.send_angle_camera(0)
filter.switch_camera(1) filter.switch_camera(1)
if cfg['get_block']['first_block'] == "red": if cfg['get_block']['first_block'] == "red":
self.target_label = tlabel.BBLOCK self.target_label = tlabel.BBLOCK
@@ -357,6 +360,8 @@ class get_block2():
by_cmd.send_beep(0) by_cmd.send_beep(0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 100) by_cmd.send_position_axis_x(1, 100)
by_cmd.send_distance_x(15, 100)
time.sleep(2) time.sleep(2)
pass pass
def nexec(self): def nexec(self):
@@ -367,8 +372,9 @@ class get_block2():
# 紧急转移 # 紧急转移
class put_block(): class put_block():
def init(self): def init(self):
while (by_cmd.send_angle_camera(180) == -1): utils.task_speed = 0
by_cmd.send_angle_camera(180) while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
logger.info("紧急转移初始化") logger.info("紧急转移初始化")
socket.send_string("1") socket.send_string("1")
socket.recv() socket.recv()
@@ -377,16 +383,19 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL) ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0: if ret > 0:
width = box[0][2] - box[0][0] width = box[0][2] - box[0][0]
if width > 120: if width > 145:
return True return True
return False return False
def exec(self): def exec(self):
logger.info("找到医院") logger.info("找到医院")
car_stop() car_stop()
calibrate_new(tlabel.HOSPITAL, offset = 7, run = False) calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(20, 0) by_cmd.send_position_axis_z(20, 0)
time.sleep(3) time.sleep(3)
by_cmd.send_position_axis_x(1, 0) by_cmd.send_position_axis_x(1, 40)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
@@ -404,10 +413,10 @@ class put_block():
by_cmd.send_position_axis_z(20, 130) by_cmd.send_position_axis_z(20, 130)
time.sleep(3) time.sleep(3)
by_cmd.send_angle_claw_arm(220) 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) by_cmd.send_position_axis_z(20, 0)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_x(1, 0) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(3) time.sleep(3)
@@ -447,18 +456,22 @@ class get_bball():
logger.info("整装上阵初始化") logger.info("整装上阵初始化")
# time.sleep(0.5) # time.sleep(0.5)
self.record = CountRecord(5)
def find(self): def find(self):
# 目标检测蓝球 # 目标检测蓝球
ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
if ret: if ret:
return True if self.record(tlabel.BBALL):
return True
# if ret:
# return True
return False return False
def exec(self): def exec(self):
logger.info("找到蓝色球") logger.info("找到蓝色球")
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
for _ in range(3): 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("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(36)
@@ -472,15 +485,15 @@ class get_bball():
by_cmd.send_distance_axis_z(20, 20) by_cmd.send_distance_axis_z(20, 20)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_x(1, 0) 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) by_cmd.send_angle_claw_arm(67)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw_arm(36) by_cmd.send_angle_claw_arm(36)
time.sleep(1) time.sleep(1)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(1)
# 继续向前走 # 继续向前走
# by_cmd.send_speed_x(4) # by_cmd.send_speed_x(4)
# 下一动作预备位置 # 下一动作预备位置
@@ -515,7 +528,11 @@ class up_tower():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_light(0) by_cmd.send_light(0)
# calibrate(tlabel.TOWER, 27, False, 6) # 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) time.sleep(3)
# while True: # while True:
# pass # pass
# 下一动作预备位置 # 下一动作预备位置
@@ -533,8 +550,8 @@ class get_rball():
socket.send_string("1") socket.send_string("1")
socket.recv() socket.recv()
logger.info("高空排险初始化") logger.info("高空排险初始化")
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(180)
self.record = CountRecord(3) self.record = CountRecord(3)
def find(self): def find(self):
# 目标检测红球 # 目标检测红球
@@ -553,9 +570,9 @@ class get_rball():
time.sleep(0.5) time.sleep(0.5)
# 靠近塔 # 靠近塔
by_cmd.send_angle_scoop(20) by_cmd.send_angle_scoop(20)
by_cmd.send_distance_y(-15, 50) by_cmd.send_distance_y(-15, 42) # 50
time.sleep(2) time.sleep(2)
calibrate_new(tlabel.RBALL,offset = 50, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") logger.info("抓红球")
# by_cmd.send_angle_scoop(15) # by_cmd.send_angle_scoop(15)
@@ -565,6 +582,7 @@ class get_rball():
by_cmd.send_angle_scoop(7) by_cmd.send_angle_scoop(7)
by_cmd.send_distance_y(15, 70) by_cmd.send_distance_y(15, 70)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_omega(-55,30)
# while True: # while True:
# pass # pass
pass pass
@@ -694,8 +712,8 @@ class put_hanoi1():
# by_cmd.send_angle_omega(45,238) # by_cmd.send_angle_omega(45,238)
by_cmd.send_angle_omega(55,194) by_cmd.send_angle_omega(55,194)
time.sleep(2) time.sleep(2)
while (by_cmd.send_angle_camera(180) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(180) by_cmd.send_angle_camera(0)
time.sleep(0.5) time.sleep(0.5)
socket.send_string("1") socket.send_string("1")
socket.recv() socket.recv()
@@ -743,6 +761,7 @@ class put_hanoi2():
# by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30) # by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
time.sleep(1) time.sleep(1)
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5) explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
logger.info("抓大平台") logger.info("抓大平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40) # by_cmd.send_position_axis_z(20, 40)
@@ -758,20 +777,21 @@ class put_hanoi2():
# by_cmd.send_distance_axis_z(20, 10) # by_cmd.send_distance_axis_z(20, 10)
pass pass
else: else:
by_cmd.send_position_axis_z(20, 30) by_cmd.send_position_axis_z(20, 10)
by_cmd.send_position_axis_x(1, 50) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(90) by_cmd.send_angle_claw(90)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(45)
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(20, 20) by_cmd.send_distance_axis_z(20, 20)
time.sleep(2) time.sleep(1)
by_cmd.send_position_axis_x(2, 160) by_cmd.send_position_axis_x(2, 160)
time.sleep(4) time.sleep(4)
pass pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5) explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
logger.info("放大平台") logger.info("放大平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10) # by_cmd.send_distance_axis_z(20, -10)
@@ -781,7 +801,7 @@ class put_hanoi2():
# by_cmd.send_position_axis_x(1, 0) # by_cmd.send_position_axis_x(1, 0)
pass pass
else: else:
by_cmd.send_position_axis_x(2, 60) by_cmd.send_position_axis_x(2, 40)
time.sleep(4) time.sleep(4)
by_cmd.send_distance_axis_z(20, -20) by_cmd.send_distance_axis_z(20, -20)
time.sleep(1) time.sleep(1)
@@ -790,6 +810,7 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 160) by_cmd.send_position_axis_x(1, 160)
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5) explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
logger.info("抓中平台") logger.info("抓中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40) # by_cmd.send_position_axis_z(20, 40)
@@ -805,7 +826,7 @@ class put_hanoi2():
# by_cmd.send_distance_axis_z(20, 10) # by_cmd.send_distance_axis_z(20, 10)
pass pass
else: 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_position_axis_x(1, 40)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
@@ -814,11 +835,12 @@ class put_hanoi2():
by_cmd.send_angle_claw(40) by_cmd.send_angle_claw(40)
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(20, 20) by_cmd.send_distance_axis_z(20, 20)
time.sleep(2) time.sleep(1)
by_cmd.send_position_axis_x(2, 160) by_cmd.send_position_axis_x(2, 160)
time.sleep(4) time.sleep(4)
pass pass
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5) explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
logger.info("放中平台") logger.info("放中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10) # by_cmd.send_distance_axis_z(20, -10)
@@ -828,7 +850,7 @@ class put_hanoi2():
# by_cmd.send_position_axis_x(1, 0) # by_cmd.send_position_axis_x(1, 0)
pass pass
else: else:
by_cmd.send_distance_axis_z(20, 60) by_cmd.send_position_axis_z(20, 100)
time.sleep(4) time.sleep(4)
by_cmd.send_position_axis_x(2, 40) by_cmd.send_position_axis_x(2, 40)
time.sleep(4) time.sleep(4)
@@ -839,6 +861,7 @@ class put_hanoi2():
by_cmd.send_position_axis_x(1, 160) by_cmd.send_position_axis_x(1, 160)
explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5) explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
time.sleep(1)
logger.info("抓小平台") logger.info("抓小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# by_cmd.send_position_axis_z(20, 40) # by_cmd.send_position_axis_z(20, 40)
@@ -854,7 +877,7 @@ class put_hanoi2():
# by_cmd.send_distance_axis_z(20, 10) # by_cmd.send_distance_axis_z(20, 10)
pass pass
else: 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_position_axis_x(1, 40)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
@@ -862,12 +885,13 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(30) by_cmd.send_angle_claw(30)
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(20, 20) by_cmd.send_distance_axis_z(20, 10)
time.sleep(2) time.sleep(1)
by_cmd.send_position_axis_x(2, 160) by_cmd.send_position_axis_x(2, 160)
time.sleep(4) time.sleep(4)
pass pass
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5) explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
time.sleep(1)
logger.info("放小平台") logger.info("放小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
# by_cmd.send_distance_axis_z(20, -10) # by_cmd.send_distance_axis_z(20, -10)
@@ -877,7 +901,7 @@ class put_hanoi2():
# by_cmd.send_position_axis_x(1, 0) # by_cmd.send_position_axis_x(1, 0)
pass pass
else: else:
by_cmd.send_distance_axis_z(20, 155) by_cmd.send_position_axis_z(20, 160)
time.sleep(4) time.sleep(4)
by_cmd.send_position_axis_x(2, 40) by_cmd.send_position_axis_x(2, 40)
time.sleep(4) time.sleep(4)
@@ -916,8 +940,8 @@ class put_hanoi3():
class move_area1(): class move_area1():
def init(self): def init(self):
logger.info("应急避险第一阶段初始化") logger.info("应急避险第一阶段初始化")
while (by_cmd.send_angle_camera(180) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(180) by_cmd.send_angle_camera(0)
def find(self): def find(self):
ret = filter.find(tlabel.SIGN) ret = filter.find(tlabel.SIGN)
if ret: if ret:
@@ -971,8 +995,8 @@ class move_area2():
# 扫黑除暴 # 扫黑除暴
class kick_ass(): class kick_ass():
def init(self): def init(self):
while (by_cmd.send_angle_camera(180) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(180) by_cmd.send_angle_camera(0)
logger.info("扫黑除暴初始化") logger.info("扫黑除暴初始化")
self.pos_gap1 = cfg['kick_ass']['pos_gap1'] self.pos_gap1 = cfg['kick_ass']['pos_gap1']
self.pos_gap2 = cfg['kick_ass']['pos_gap2'] self.pos_gap2 = cfg['kick_ass']['pos_gap2']