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-17 17:19:18 +08:00
parent d340ad044d
commit 1ac91b678b
3 changed files with 77 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
action.py
View File

@@ -182,33 +182,64 @@ class cmd_cls():
while bycmd.send_angle_zhuan(angle) == -1:
pass
time.sleep(0.5)
def wait(self, _time):
time.sleep(_time)
# TODO 轴控制指令类，增加指令后加入动作队列，非阻塞
# TODO 增加任务队列中非阻塞控制
class axis_cls():
def __init__(self) -> None:
self.axis_queue = queue.Queue()
self.busy = False
pass
def axis_z(self, _distance, _time_via = -1):
def z(self, _distance, _time_via = -1):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.z(20, _distance, _time_via))
pass
def axis_z2(self, _position, _time_via = -1):
def z2(self, _position, _time_via = -1):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.z2(20, _position, _time_via))
pass
def axis_x(self, _distance, _time_via = -1):
def x(self, _distance, _time_via = -1):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.x(1, _distance, _time_via))
pass
def axis_x2(self, _position, _time_via = -1):
def x2(self, _position, _time_via = -1):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.x2(1, _position, _time_via))
pass
def camera(self, _angle):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.camera(_angle))
pass
def claw(self, _angle):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.claw(_angle))
pass
def claw_arm(self, _angle):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.claw_arm(_angle))
pass
def scoop(self, _angle):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.scoop(_angle))
pass
def storage(self, _angle):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.storage(_angle))
pass
def wait(self, _time):
while self.busy is True:
pass
self.axis_queue.put(lambda: cmd.wait(_time))
def pop(self):
self.busy = True
while self.axis_queue.qsize() > 0:

10
main.py
View File

@@ -18,11 +18,11 @@ cfg_main = toml.load('cfg_main.toml')
# 配置日志输出
logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO")
act.cmd.camera(180)
act.cmd.x2(1, 140)
act.cmd.storage(20)
act.cmd.scoop(25)
act.axis.camera(180)
act.axis.x2(140)
act.axis.storage(20)
act.axis.scoop(25)
act.axis.exec()
# 向任务队列添加任务
task_queue = queue.Queue()

58
subtask.py
View File

@@ -15,6 +15,7 @@ import toml
import zmq
import time
import variable as var
import action as act
context = zmq.Context()
socket = context.socket(zmq.REQ)
@@ -184,6 +185,7 @@ 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}")
@@ -303,8 +305,7 @@ class get_block1():
def init(self):
var.task_speed = 12
logger.info(f"人员施救 1 初始化，第一抓取块为 {cfg['get_block']['first_block']}")
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
act.cmd.camera(0)
filter.switch_camera(1)
if cfg['get_block']['first_block'] == "blue":
self.target_label = tlabel.BBLOCK
@@ -323,7 +324,25 @@ class get_block1():
car_stop()
calibrate_new(self.target_label, offset = 15, run = True)
logger.info("抓取块")
time.sleep(0.5)
# act.axis.wait(0.5)
# act.axis.z2(60)
# act.axis.x2(100)
# act.axis.claw_arm(220)
# act.axis.claw(63)
# act.axis.x2(20)
# act.axis.wait(2)
# act.axis.claw(30)
# act.axis.x2(80)
# act.axis.z2(130)
# act.axis.claw_arm(36)
# act.axis.z2(20)
# act.axis.x2(40)
# act.axis.claw(45)
# act.axis.z2(80)
# act.axis.claw_arm(220)
# act.axis.x2(100)
# act.axis.exec()
by_cmd.send_position_axis_z(20, 60)
by_cmd.send_position_axis_x(1, 100)
time.sleep(1)
@@ -345,7 +364,7 @@ class get_block1():
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
by_cmd.send_angle_claw(45)
by_cmd.send_angle_claw(30)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 80)
time.sleep(2)
@@ -416,8 +435,7 @@ class put_block():
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
logger.info("紧急转移初始化")
socket.send_string("1")
socket.recv()
filter.switch_camera(1)
def find(self):
# 目标检测医院
ret, box = filter.get(tlabel.HOSPITAL)
@@ -450,7 +468,7 @@ class put_block():
time.sleep(2)
by_cmd.send_angle_claw(30)
time.sleep(0.5)
by_cmd.send_position_axis_z(20, 130)
by_cmd.send_position_axis_z(20, 140)
time.sleep(3)
by_cmd.send_angle_claw_arm(220)
by_cmd.send_distance_x(-10, 110)
@@ -491,8 +509,7 @@ class get_bball():
by_cmd.send_angle_storage(0)
# 调试 临时换源
socket.send_string("1")
socket.recv()
filter.switch_camera(1)
logger.info("整装上阵初始化")
# time.sleep(0.5)
@@ -518,18 +535,18 @@ class get_bball():
by_cmd.send_angle_claw(54)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
by_cmd.send_angle_claw(70)
by_cmd.send_angle_claw(60)
time.sleep(1)
by_cmd.send_angle_claw(30)
time.sleep(1)
by_cmd.send_distance_axis_z(20, 20)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(60)
time.sleep(0.5)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(0.5)
by_cmd.send_angle_claw_arm(67)
time.sleep(0.5)
time.sleep(1)
by_cmd.send_angle_claw(54)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
@@ -594,8 +611,7 @@ class up_tower():
# 高空排险
class get_rball():
def init(self):
socket.send_string("1")
socket.recv()
filter.switch_camera(1)
logger.info("高空排险初始化")
while (by_cmd.send_angle_camera(180) == -1):
by_cmd.send_angle_camera(180)
@@ -645,8 +661,7 @@ class get_rball():
class put_bball():
def init(self):
logger.info("派发物资初始化")
socket.send_string("1")
socket.recv()
filter.switch_camera(1)
by_cmd.send_position_axis_z(20, 0)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
@@ -684,8 +699,7 @@ class put_bball():
class put_hanoi1():
def init(self):
logger.info("物资盘点 1 初始化")
socket.send_string("2")
socket.recv()
filter.switch_camera(2)
def find(self):
ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
if label == tlabel.RMARK:
@@ -770,8 +784,7 @@ class put_hanoi1():
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
time.sleep(0.5)
socket.send_string("1")
socket.recv()
filter.switch_camera(1)
def nexec(self):
pass
def after(self):
@@ -1047,6 +1060,7 @@ class move_area2():
by_cmd.send_light(0)
# 离开停车区域
by_cmd.send_distance_y(-10, 450)
time.sleep(4)
pass
def nexec(self):
pass