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

Compare commits

base: btl143:18154917a1fdd0fd7ab83057096ac04ce6ea130e
Branches Tags
btl143:master
btl143:v1
..
compare: btl143:master
Branches Tags
btl143:master
btl143:v1

4 Commits
18154917a1 ... master

Author SHA1 Message Date
bmy
5aef09bb63 国赛出发前版本 
fix: 补充 action 列表检查
pref: 修改 BBALL offset 值 (未同步到其他备份 subtask.py 中)
套用8822参数
 2024-08-17 22:18:49 +08:00
bmy
7dd0d6677e update: 更新891/8822/raw的subtask文件 2024-08-17 01:59:00 +08:00
bmy
91d6491f64 暂存 
fix: 修复task多次开启问题
feat: 新增 891 参数（医院停早，红球靠外）
pref: 弃用 action 类
 2024-08-17 01:40:07 +08:00
bmy
7ae8162da7 pref: 扫黑除暴动作修改 2024-08-12 14:56:20 +08:00
10 changed files with 1976 additions and 212 deletions
Expand all files Collapse all files

26
action.py
View File

@@ -13,15 +13,19 @@ move = None
axis = None axis = None
cmd = None cmd = None
action_run_flag = None
cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml') cfg = toml.load('/home/evan/Workplace/project_main/cfg_action.toml')
def import_obj(_bycmd): def import_obj(_run_flag,_bycmd):
global bycmd global bycmd
global move global move
global axis global axis
global cmd global cmd
global action_run_flag
bycmd = _bycmd bycmd = _bycmd
action_run_flag = _run_flag
move = move_cls() move = move_cls()
axis = axis_cls() axis = axis_cls()
cmd = cmd_cls() cmd = cmd_cls()
@@ -118,7 +122,7 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}") logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1: while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet():
pass pass
time.sleep(time_via) time.sleep(time_via)
def z2(self, _speed, _pos, _time_via = -1): def z2(self, _speed, _pos, _time_via = -1):
@@ -131,7 +135,7 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}") logger.info(f"z[{self.__axis_z_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1: while bycmd.send_position_axis_z(_speed, self.__axis_z_pos) == -1 and action_run_flag.isSet():
pass pass
time.sleep(time_via) time.sleep(time_via)
def x(self, _speed, _dis, _time_via = -1): def x(self, _speed, _dis, _time_via = -1):
@@ -143,7 +147,7 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}") logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, dis:{_dis:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1: while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet():
pass pass
time.sleep(time_via) time.sleep(time_via)
def x2(self, _speed, _pos, _time_via = -1): def x2(self, _speed, _pos, _time_via = -1):
@@ -156,30 +160,30 @@ class cmd_cls():
else: else:
time_via = _time_via time_via = _time_via
logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}") logger.info(f"x[{self.__axis_x_pos}] speed:{_speed:.2f}, pos:{_pos:.2f}, time_via:{time_via:.2f}")
while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1: while bycmd.send_position_axis_x(_speed, self.__axis_x_pos) == -1 and action_run_flag.isSet():
pass pass
time.sleep(time_via) time.sleep(time_via)
def camera(self, angle): def camera(self, angle):
while bycmd.send_angle_camera(angle) == -1: while bycmd.send_angle_camera(angle) == -1 and action_run_flag.isSet():
pass pass
def claw(self, angle): def claw(self, angle):
while bycmd.send_angle_claw(angle) == -1: while bycmd.send_angle_claw(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def claw_arm(self, angle): def claw_arm(self, angle):
while bycmd.send_angle_claw_arm(angle) == -1: while bycmd.send_angle_claw_arm(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(1) time.sleep(1)
def scoop(self, angle): def scoop(self, angle):
while bycmd.send_angle_scoop(angle) == -1: while bycmd.send_angle_scoop(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def storage(self, angle): def storage(self, angle):
while bycmd.send_angle_storage(angle) == -1: while bycmd.send_angle_storage(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def zhuan(self, angle): def zhuan(self, angle):
while bycmd.send_angle_zhuan(angle) == -1: while bycmd.send_angle_zhuan(angle) == -1 and action_run_flag.isSet():
pass pass
time.sleep(0.5) time.sleep(0.5)
def wait(self, _time): def wait(self, _time):

22
app.py
View File

@@ -23,7 +23,7 @@ processes = []
time_record = None time_record = None
task_run_flag = False
# 日志队列 # 日志队列
queue = Queue() queue = Queue()
@@ -37,7 +37,9 @@ app.config['SECRET_KEY'] = 'secret!'
socketio = SocketIO(app, allow_unsafe_werkzeug=True) socketio = SocketIO(app, allow_unsafe_werkzeug=True)
server_process = None server_process = None
# FIXME 在列表里存所有的 task_process
task_process = None task_process = None
task_run_flag = threading.Event()
class WebSocketHandler(logging.Handler): class WebSocketHandler(logging.Handler):
def emit(self, record): def emit(self, record):
@@ -124,23 +126,23 @@ def operate_handle(data):
elif data['type'] == 'operate_task': elif data['type'] == 'operate_task':
# 任务函数 # 任务函数
if data['content'] == 'run': if data['content'] == 'run':
task_run_flag = True task_run_flag.set()
# 开启 task 进程前先关闭所有历史进程
if task_process != None: if task_process != None:
task_process.terminate() task_process.terminate()
time_record = time.perf_counter() time_record = time.perf_counter()
task_process = Process(target=main_func, args=(queue,skip_task_queue)) task_process = Process(target=main_func, args=(task_run_flag,queue,skip_task_queue))
task_process.start() task_process.start()
logger.info("开启 task") logger.info("开启 task")
elif data['content'] == 'stop': elif data['content'] == 'stop':
task_run_flag = False task_run_flag.clear()
task_process.terminate() if task_process != None:
task_process.terminate()
logger.info(f"任务结束 用时{time.perf_counter() - time_record}s") logger.info(f"任务结束 用时{time.perf_counter() - time_record}s")
logger.info("关闭 task") logger.info("关闭 task")
elif data['content'] == 'restart': elif data['content'] == 'restart':
if task_process != None: pass
task_process.terminate()
task_process = Process(target=main_func, args=(queue,skip_task_queue))
task_process.start()
elif data['type'] == 'show_server_log': elif data['type'] == 'show_server_log':
content = '' content = ''
try: try:
@@ -167,7 +169,7 @@ def test_connect():
for item in fileOptions_list: for item in fileOptions_list:
config_data[item] = toml.load(os.path.join(fileOptions_path,item)) config_data[item] = toml.load(os.path.join(fileOptions_path,item))
socketio.emit('config_data', {'type': 'config_data', 'content': config_data}) socketio.emit('config_data', {'type': 'config_data', 'content': config_data})
socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag)}) socketio.emit('task_status', {'type': 'task_status', 'content': int(task_run_flag.isSet())})
def thread_function(): def thread_function():
global queue global queue
while True: while True:

61
cfg_subtask.toml.891.bak Normal file
View File

@@ -0,0 +1,61 @@
[get_block]
pid_kp = 1.5
pid_ki = 0
pid_kd = 0
first_block = "blue"
[put_block]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[get_bball]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[up_tower]
pid_kp = 1.8
pid_ki = 0
pid_kd = 0
[get_rball]
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
[put_bball]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
[put_hanoi1]
pid_kp = 0.7
pid_ki = 0
pid_kd = 0
[put_hanoi2]
pid_kp = 1.0
pid_ki = 0
pid_kd = 0
pos_gap = 160
first_target = "mp"
[put_hanoi3]
pid_kp = 1.5
pid_ki = 0
pid_kd = 0
[move_area]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
llm_enable = true
[kick_ass]
pid_kp = 1.2
pid_ki = 0
pid_kd = 0
pos_gap1 = 150
pos_gap2 = 80
target_person = 1

35
main_upper.py
View File

@@ -6,12 +6,12 @@ import subtask as sb
import majtask as mj import majtask as mj
from by_cmd_py import by_cmd_py from by_cmd_py import by_cmd_py
import time import time
import action as act # import action as act
import logging import logging
import signal import signal
running = True # running = True
def main_func(_queue, _skip_queue): def main_func(_run_flag,_queue, _skip_queue):
if _queue != None: if _queue != None:
# 日志重定向 # 日志重定向
class Handler(logging.Handler): class Handler(logging.Handler):
@@ -23,14 +23,13 @@ def main_func(_queue, _skip_queue):
logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG") logger.add(handler, format="{time:MM-DD HH:mm:ss} {message}", level="DEBUG")
def signal_handler(sig, frame): def signal_handler(sig, frame):
global running _run_flag.clear()
running = False
signal.signal(signal.SIGTERM, signal_handler) signal.signal(signal.SIGTERM, signal_handler)
cmd_py_obj = by_cmd_py(_queue) cmd_py_obj = by_cmd_py(_queue)
sb.import_obj(cmd_py_obj, _skip_queue) sb.import_obj(cmd_py_obj, _skip_queue)
act.import_obj(cmd_py_obj) # act.import_obj(_run_flag, cmd_py_obj)
# 读取配置 # 读取配置
cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml') cfg_main = toml.load('/home/evan/Workplace/project_main/cfg_main.toml')
@@ -42,12 +41,20 @@ def main_func(_queue, _skip_queue):
logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO") logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
logger.info(cfg_args) logger.info(cfg_args)
act.axis.camera(0) cmd_py_obj.send_angle_camera(0)
act.axis.x2(140) cmd_py_obj.send_position_axis_x(1, 140)
act.axis.storage(20) cmd_py_obj.send_angle_storage(20)
act.axis.scoop(25) cmd_py_obj.send_angle_scoop(25)
act.axis.claw_arm(225) # cmd_py_obj.send_angle_claw_arm(225)
act.axis.exec() time.sleep(1)
# 弃用 action
# act.axis.camera(0)
# act.axis.x2(140)
# act.axis.storage(20)
# act.axis.scoop(25)
# act.axis.claw_arm(225)
# act.axis.exec()
logger.info(cfg_main) logger.info(cfg_main)
@@ -72,7 +79,7 @@ def main_func(_queue, _skip_queue):
task_queuem_t = sb.task_queuem(task_queue) task_queuem_t = sb.task_queuem(task_queue)
# 创建任务队列的工作线程 # 创建任务队列的工作线程
def worker_thread(): def worker_thread():
while task_queuem_t.exec(_skip_queue) is True: while task_queuem_t.exec(_skip_queue) and _run_flag.isSet():
pass pass
# 启动工作线程 # 启动工作线程
@@ -83,7 +90,7 @@ def main_func(_queue, _skip_queue):
# 创建主任务 # 创建主任务
main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象 main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象
try: try:
while running: while _run_flag.isSet():
if task_queuem_t.status is sb.task_queuem_status.EXECUTING: if task_queuem_t.status is sb.task_queuem_status.EXECUTING:
pass pass
else: else:

112
subtask.py
View File

@@ -10,7 +10,6 @@ import toml
import zmq import zmq
import time import time
import variable as var import variable as var
import action as act
import re import re
import math import math
import json import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
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) <= 10:
car_stop() car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label) # ret, box = filter.get(label)
while not ret: # while not ret:
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}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
while not ret: # while not ret:
ret, box = filter.get(target_label) # ret, box = filter.get(target_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}")
break break
return True return True
@@ -426,8 +425,7 @@ class task_queuem(task):
class get_block1(): class get_block1():
def init(self): def init(self):
var.task_speed = 15 var.task_speed = 15
act.cmd.camera(0) by_cmd.send_angle_camera(0)
act.cmd.z2(20, 60, 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
@@ -464,7 +462,7 @@ class get_block1():
logger.info("抓取块") logger.info("抓取块")
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(1) time.sleep(1)
@@ -557,7 +555,7 @@ 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 > 135: if width > 125: #FIXME maybe 125 batter
return True return True
return False return False
def exec(self): def exec(self):
@@ -651,7 +649,8 @@ class get_bball():
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 = 18, run = True, run_speed = 5) by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 10, run = True, run_speed = 5)
logger.info("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
@@ -672,7 +671,7 @@ class get_bball():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 155) # by_cmd.send_position_axis_z(30, 155)
# 继续向前走 # 继续向前走
# by_cmd.send_speed_x(4) # by_cmd.send_speed_x(4)
pass pass
@@ -710,10 +709,10 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True) calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1) time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6) # calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120) by_cmd.send_distance_x(-10, 100) # used to be 120
time.sleep(1) time.sleep(1)
# 上古參數 # 上古參數
by_cmd.send_distance_y(-10, 50) # 80 # by_cmd.send_distance_y(-10, 50) # 80
# 6_9 模型參數 # 6_9 模型參數
# by_cmd.send_distance_y(-10, 40) # by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數 # 7_12_3 模型參數
@@ -724,6 +723,12 @@ class up_tower():
# time.sleep(3) # time.sleep(3)
# by_cmd.send_speed_y(-10) # by_cmd.send_speed_y(-10)
# time.sleep(0.15) # time.sleep(0.15)
# 8822
by_cmd.send_distance_y(-10, 50)
time.sleep(0.3)
# 891
# by_cmd.send_distance_y(-10, 35)
# time.sleep(0.3)
by_cmd.send_angle_zhuan(10) by_cmd.send_angle_zhuan(10)
time.sleep(12) time.sleep(12)
@@ -781,7 +786,10 @@ class get_rball():
# car_stop() # car_stop()
# 8822 参数 # 8822 参数
by_cmd.send_distance_y(-15, 40) by_cmd.send_distance_y(-15, 40)
time.sleep(1.5) time.sleep(0.5)
# 891 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") logger.info("抓红球")
@@ -1105,14 +1113,14 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass 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) ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret: if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放中平台") logger.info("放中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 140) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1124,7 +1132,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 140) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1166,14 +1174,14 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass 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) ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret: if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放小平台") logger.info("放小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 200) # 170 #190(new) by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1189,7 +1197,7 @@ class put_hanoi2():
# car_stop() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 200) by_cmd.send_position_axis_z(30, 210)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1540,6 +1548,16 @@ class move_area2():
if len(resp_commands) == 0: if len(resp_commands) == 0:
return return
action_list = resp_commands action_list = resp_commands
# 先检查一下 action 是否生成正确，如果不正确直接跳过
actions_keys = self.action_dict.keys()
try:
for action in action_list:
if not (action.get('action') in actions_keys):
return
except:
return
# 进入停车区域 # 进入停车区域
by_cmd.send_distance_y(10, 450) by_cmd.send_distance_y(10, 450)
time.sleep((450 * 5 / 1000) + 0.5) time.sleep((450 * 5 / 1000) + 0.5)
@@ -1610,44 +1628,26 @@ class kick_ass():
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
# 移动到中间 # 移动到中间
by_cmd.send_distance_x(10, 295)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(15) by_cmd.send_angle_claw(15)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1)
# 先移动到第一个人的地方
by_cmd.send_distance_x(10, self.pos_gap1)
time.sleep(1.5)
if self.target_person == 1: if self.target_person == 1:
by_cmd.send_distance_x(-10, 150) # target_distance = self.pos_gap1
time.sleep(1.5) pass
car_stop()
elif self.target_person == 2:
by_cmd.send_distance_x(-10, 50)
time.sleep(1.5)
car_stop()
elif self.target_person == 3:
by_cmd.send_distance_x(10, 50)
time.sleep(1.5)
car_stop()
else: else:
by_cmd.send_distance_x(10, 150) # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
time.sleep(1.5) target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10
car_stop() 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 )
# by_cmd.send_distance_x(10, self.pos_gap1) car_stop()
# time.sleep(1.5)
# if self.target_person == 1:
# # target_distance = self.pos_gap1
# pass
# else:
# # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
# target_distance = (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(225) # by_cmd.send_angle_claw_arm(225)
# time.sleep(0.5) # time.sleep(0.5)
@@ -1669,6 +1669,8 @@ class kick_ass():
# by_cmd.send_speed_x(25) # by_cmd.send_speed_x(25)
# time.sleep(4) # time.sleep(4)
pass pass
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):

110
subtask_8822.py
View File

@@ -10,7 +10,6 @@ import toml
import zmq import zmq
import time import time
import variable as var import variable as var
import action as act
import re import re
import math import math
import json import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
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) <= 10:
car_stop() car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label) # ret, box = filter.get(label)
while not ret: # while not ret:
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}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
while not ret: # while not ret:
ret, box = filter.get(target_label) # ret, box = filter.get(target_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}")
break break
return True return True
@@ -426,8 +425,7 @@ class task_queuem(task):
class get_block1(): class get_block1():
def init(self): def init(self):
var.task_speed = 15 var.task_speed = 15
act.cmd.camera(0) by_cmd.send_angle_camera(0)
act.cmd.z2(20, 60, 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
@@ -464,7 +462,7 @@ class get_block1():
logger.info("抓取块") logger.info("抓取块")
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(1) time.sleep(1)
@@ -557,7 +555,7 @@ 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 > 135: if width > 130: #FIXME maybe 125 batter
return True return True
return False return False
def exec(self): def exec(self):
@@ -651,6 +649,7 @@ class get_bball():
car_stop() car_stop()
time.sleep(0.5) time.sleep(0.5)
for _ in range(3): for _ in range(3):
by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5) calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
logger.info("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
@@ -672,7 +671,7 @@ class get_bball():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 155) # by_cmd.send_position_axis_z(30, 155)
# 继续向前走 # 继续向前走
# by_cmd.send_speed_x(4) # by_cmd.send_speed_x(4)
pass pass
@@ -710,10 +709,10 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True) calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1) time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6) # calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120) by_cmd.send_distance_x(-10, 100) # used to be 120
time.sleep(1) time.sleep(1)
# 上古參數 # 上古參數
by_cmd.send_distance_y(-10, 50) # 80 # by_cmd.send_distance_y(-10, 50) # 80
# 6_9 模型參數 # 6_9 模型參數
# by_cmd.send_distance_y(-10, 40) # by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數 # 7_12_3 模型參數
@@ -724,6 +723,12 @@ class up_tower():
# time.sleep(3) # time.sleep(3)
# by_cmd.send_speed_y(-10) # by_cmd.send_speed_y(-10)
# time.sleep(0.15) # time.sleep(0.15)
# 8822
by_cmd.send_distance_y(-10, 50)
time.sleep(0.3)
# 891
# by_cmd.send_distance_y(-10, 35)
# time.sleep(0.3)
by_cmd.send_angle_zhuan(10) by_cmd.send_angle_zhuan(10)
time.sleep(12) time.sleep(12)
@@ -781,7 +786,10 @@ class get_rball():
# car_stop() # car_stop()
# 8822 参数 # 8822 参数
by_cmd.send_distance_y(-15, 40) by_cmd.send_distance_y(-15, 40)
time.sleep(1.5) time.sleep(0.5)
# 891 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") logger.info("抓红球")
@@ -1105,14 +1113,14 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass 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) ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret: if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放中平台") logger.info("放中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 140) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1124,7 +1132,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 140) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1166,14 +1174,14 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass 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) ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret: if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放小平台") logger.info("放小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 200) # 170 #190(new) by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1189,7 +1197,7 @@ class put_hanoi2():
# car_stop() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 200) by_cmd.send_position_axis_z(30, 210)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1540,6 +1548,16 @@ class move_area2():
if len(resp_commands) == 0: if len(resp_commands) == 0:
return return
action_list = resp_commands action_list = resp_commands
# 先检查一下 action 是否生成正确，如果不正确直接跳过
actions_keys = self.action_dict.keys()
try:
for action in action_list:
if not (action.get('action') in actions_keys):
return
except:
return
# 进入停车区域 # 进入停车区域
by_cmd.send_distance_y(10, 450) by_cmd.send_distance_y(10, 450)
time.sleep((450 * 5 / 1000) + 0.5) time.sleep((450 * 5 / 1000) + 0.5)
@@ -1610,44 +1628,26 @@ class kick_ass():
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
# 移动到中间 # 移动到中间
by_cmd.send_distance_x(10, 295)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(15) by_cmd.send_angle_claw(15)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1)
# 先移动到第一个人的地方
by_cmd.send_distance_x(10, self.pos_gap1)
time.sleep(1.5)
if self.target_person == 1: if self.target_person == 1:
by_cmd.send_distance_x(-10, 150) # target_distance = self.pos_gap1
time.sleep(1.5) pass
car_stop()
elif self.target_person == 2:
by_cmd.send_distance_x(-10, 50)
time.sleep(1.5)
car_stop()
elif self.target_person == 3:
by_cmd.send_distance_x(10, 50)
time.sleep(1.5)
car_stop()
else: else:
by_cmd.send_distance_x(10, 150) # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
time.sleep(1.5) target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10
car_stop() 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 )
# by_cmd.send_distance_x(10, self.pos_gap1) car_stop()
# time.sleep(1.5)
# if self.target_person == 1:
# # target_distance = self.pos_gap1
# pass
# else:
# # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
# target_distance = (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(225) # by_cmd.send_angle_claw_arm(225)
# time.sleep(0.5) # time.sleep(0.5)
@@ -1669,6 +1669,8 @@ class kick_ass():
# by_cmd.send_speed_x(25) # by_cmd.send_speed_x(25)
# time.sleep(4) # time.sleep(4)
pass pass
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):

1676
subtask_891.py Normal file
View File

File diff suppressed because it is too large Load Diff

135
subtask_raw.py
View File

@@ -10,7 +10,6 @@ import toml
import zmq import zmq
import time import time
import variable as var import variable as var
import action as act
import re import re
import math import math
import json import json
@@ -116,7 +115,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
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) <= 10:
car_stop() car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车") logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label) ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label) # ret, box = filter.get(label)
while not ret: # while not ret:
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}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准") # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop() car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车") logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label) # ret, box = filter.get(target_label)
while not ret: # while not ret:
ret, box = filter.get(target_label) # ret, box = filter.get(target_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}")
break break
return True return True
@@ -426,8 +425,7 @@ class task_queuem(task):
class get_block1(): class get_block1():
def init(self): def init(self):
var.task_speed = 15 var.task_speed = 15
act.cmd.camera(0) by_cmd.send_angle_camera(0)
act.cmd.z2(20, 60, 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
@@ -464,7 +462,7 @@ class get_block1():
logger.info("抓取块") logger.info("抓取块")
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(1) time.sleep(1)
@@ -557,7 +555,7 @@ 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 > 135: if width > 125: #135
return True return True
return False return False
def exec(self): def exec(self):
@@ -568,7 +566,7 @@ class put_block():
by_cmd.send_distance_x(10, 100) by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 50) # 20 by_cmd.send_position_axis_x(1, 30) # 20
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(1) time.sleep(1)
@@ -593,7 +591,7 @@ class put_block():
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 0) by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 50) by_cmd.send_position_axis_x(1, 30)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_angle_claw(45) by_cmd.send_angle_claw(45)
time.sleep(1) time.sleep(1)
@@ -651,7 +649,8 @@ class get_bball():
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 = 18, run = True, run_speed = 5) by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 10, run = True, run_speed = 5)
logger.info("抓蓝色球") logger.info("抓蓝色球")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
@@ -666,13 +665,13 @@ class get_bball():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_distance_axis_z(30, -40) by_cmd.send_distance_axis_z(30, -40)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(80) by_cmd.send_angle_claw_arm(90)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw(54) by_cmd.send_angle_claw(54)
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 155) # by_cmd.send_position_axis_z(30, 155)
# 继续向前走 # 继续向前走
# by_cmd.send_speed_x(4) # by_cmd.send_speed_x(4)
pass pass
@@ -710,10 +709,11 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True) calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1) time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6) # calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120) by_cmd.send_distance_x(-10, 100)
time.sleep(1) time.sleep(1)
# 上古參數 # 上古參數
by_cmd.send_distance_y(-10, 50) # 80 by_cmd.send_distance_y(-10, 50) # 80
time.sleep(0.5)
# 6_9 模型參數 # 6_9 模型參數
# by_cmd.send_distance_y(-10, 40) # by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數 # 7_12_3 模型參數
@@ -724,6 +724,11 @@ class up_tower():
# time.sleep(3) # time.sleep(3)
# by_cmd.send_speed_y(-10) # by_cmd.send_speed_y(-10)
# time.sleep(0.15) # time.sleep(0.15)
# 8822
# by_cmd.send_distance_y(-10, 50)
# 891
# by_cmd.send_distance_y(-10, 35)
# time.sleep(0.3)
by_cmd.send_angle_zhuan(10) by_cmd.send_angle_zhuan(10)
time.sleep(12) time.sleep(12)
@@ -771,7 +776,7 @@ class get_rball():
# 上古參數 # 上古參數
# by_cmd.send_distance_y(-15, 50) # 50 # 70 # by_cmd.send_distance_y(-15, 50) # 50 # 70
by_cmd.send_distance_y(-15, 40) # 50 # 70 by_cmd.send_distance_y(-15, 40) # 50 # 70
time.sleep(1.5) time.sleep(0.5)
# 6_9 參數 # 6_9 參數
# by_cmd.send_distance_y(-15, 35) # by_cmd.send_distance_y(-15, 35)
# time.sleep(2) # time.sleep(2)
@@ -779,6 +784,12 @@ class get_rball():
# by_cmd.send_distance_y(-15, 45) # by_cmd.send_distance_y(-15, 45)
# time.sleep(2) # time.sleep(2)
# car_stop() # car_stop()
# 8822 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(1.5)
# 891 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True) calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1) time.sleep(1)
logger.info("抓红球") logger.info("抓红球")
@@ -826,14 +837,14 @@ class put_bball():
calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6) calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
# by_cmd.send_distance_x(10, 10) # by_cmd.send_distance_x(10, 10)
# 向左运动 # 向左运动
by_cmd.send_distance_y(-10, 35) # by_cmd.send_distance_y(-10, 35)
# by_cmd.send_angle_storage(10) # by_cmd.send_angle_storage(10)
# time.sleep(1) # time.sleep(1)
by_cmd.send_angle_storage(50) by_cmd.send_angle_storage(50)
logger.info("把球放篮筐里") logger.info("把球放篮筐里")
time.sleep(1) time.sleep(2)
# by_cmd.send_distance_y(10, 55) # by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20) by_cmd.send_angle_storage(20)
# time.sleep(1) # time.sleep(1)
@@ -1018,7 +1029,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓大平台") logger.info("抓大平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
@@ -1030,7 +1041,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_z(30, 40)
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(2) time.sleep(2)
@@ -1078,7 +1089,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓中平台") logger.info("抓中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(55) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
@@ -1090,7 +1101,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(55) by_cmd.send_angle_claw(55)
time.sleep(2) time.sleep(2)
@@ -1102,14 +1113,14 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass 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) ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret: if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放中平台") logger.info("放中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 140) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1121,7 +1132,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 140) by_cmd.send_position_axis_z(30, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1139,7 +1150,7 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
logger.info("抓小平台") logger.info("抓小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
@@ -1151,7 +1162,7 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 30) by_cmd.send_position_axis_z(30, 40)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(50) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
@@ -1163,14 +1174,14 @@ class put_hanoi2():
time.sleep(2) time.sleep(2)
pass 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) ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret: if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec") logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return return
time.sleep(0.5) time.sleep(0.5)
logger.info("放小平台") logger.info("放小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 200) # 170 #190(new) by_cmd.send_position_axis_z(30, 210) # 170 #190(new)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
@@ -1186,7 +1197,7 @@ class put_hanoi2():
# car_stop() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 200) by_cmd.send_position_axis_z(30, 210)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40) by_cmd.send_position_axis_x(1, 40)
time.sleep(2) time.sleep(2)
@@ -1537,6 +1548,16 @@ class move_area2():
if len(resp_commands) == 0: if len(resp_commands) == 0:
return return
action_list = resp_commands action_list = resp_commands
# 先检查一下 action 是否生成正确，如果不正确直接跳过
actions_keys = self.action_dict.keys()
try:
for action in action_list:
if not (action.get('action') in actions_keys):
return
except:
return
# 进入停车区域 # 进入停车区域
by_cmd.send_distance_y(10, 450) by_cmd.send_distance_y(10, 450)
time.sleep((450 * 5 / 1000) + 0.5) time.sleep((450 * 5 / 1000) + 0.5)
@@ -1607,44 +1628,26 @@ class kick_ass():
by_cmd.send_position_axis_x(1, 150) by_cmd.send_position_axis_x(1, 150)
# 移动到中间 # 移动到中间
by_cmd.send_distance_x(10, 295)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(15) by_cmd.send_angle_claw(15)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 80) by_cmd.send_position_axis_z(30, 80)
time.sleep(1) time.sleep(1)
# 先移动到第一个人的地方
by_cmd.send_distance_x(10, self.pos_gap1)
time.sleep(1.5)
if self.target_person == 1: if self.target_person == 1:
by_cmd.send_distance_x(-10, 150) # target_distance = self.pos_gap1
time.sleep(1.5) pass
car_stop()
elif self.target_person == 2:
by_cmd.send_distance_x(-10, 50)
time.sleep(1.5)
car_stop()
elif self.target_person == 3:
by_cmd.send_distance_x(10, 50)
time.sleep(1.5)
car_stop()
else: else:
by_cmd.send_distance_x(10, 150) # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
time.sleep(1.5) target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1 - 1) * 10
car_stop() 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 )
# by_cmd.send_distance_x(10, self.pos_gap1) car_stop()
# time.sleep(1.5)
# if self.target_person == 1:
# # target_distance = self.pos_gap1
# pass
# else:
# # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
# target_distance = (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(225) # by_cmd.send_angle_claw_arm(225)
# time.sleep(0.5) # time.sleep(0.5)
@@ -1666,6 +1669,8 @@ class kick_ass():
# by_cmd.send_speed_x(25) # by_cmd.send_speed_x(25)
# time.sleep(4) # time.sleep(4)
pass pass
def nexec(self): def nexec(self):
pass pass
def after(self): def after(self):

5
test/test_filter.py
View File

@@ -25,10 +25,11 @@ filter.switch_camera(1)
# label = tlabel.TPLATFORM # label = tlabel.TPLATFORM
while True: while True:
time.sleep(0.2) time.sleep(0.2)
ret, box = filter.get(tlabel.SIGN) ret, box = filter.get(tlabel.HOSPITAL)
if ret: if ret:
width = box[0][2] - box[0][0]
error = (box[0][2] + box[0][0] - 320) / 2 error = (box[0][2] + box[0][0] - 320) / 2
logger.error(error) logger.error(width)
# if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180: # if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180:
# # height = box[0][3] - box[0][1] # # height = box[0][3] - box[0][1]
# logger.error(111) # logger.error(111)

6
utils.py
View File

@@ -285,7 +285,11 @@ class label_filter:
else: else:
target_bool = False target_bool = False
target_box = None target_box = None
expect_boxes = (results[:, 0] == label.value) if len(label) == 1:
expect_boxes = (results[:, 0] == label[0].value)
elif len(label) == 2:
expect_boxes = np.logical_or(results[:, 0] == label[0].value, results[:, 0] == label[1].value)
boxes = results[expect_boxes, :] boxes = results[expect_boxes, :]
# 在此处过滤 # 在此处过滤
if len(boxes) != 0: if len(boxes) != 0: