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

feat: 增加雙lane服務端，修改hanoi動作以應對放置在圍擋上的問題

Browse Source
This commit is contained in:
bmy
2024-07-13 15:21:21 +08:00
parent ac6e055c42
commit 5b6ca482e5
3 changed files with 71 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
app.py
View File

@@ -15,8 +15,9 @@ from main_upper import main_func
server_command = [
{"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"},
{"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server.py"},
{"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server1.py"},
{"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"},
# {"path": "/home/evan/Workplace/project_infer/ocr_server/", "script": "ocr_infer_server.py"},
]
processes = []

14
majtask.py
View File

@@ -8,9 +8,14 @@ import variable as var
class main_task():
def __init__(self,by_cmd):
# lane infer server
self.context = zmq.Context()
self.socket = self.context.socket(zmq.REQ)
self.socket.connect("tcp://localhost:6666")
# lane infer server1
self.context1 = zmq.Context()
self.socket1 = self.context.socket(zmq.REQ)
self.socket1.connect("tcp://localhost:6669")
# 赛道回归相关
self.x = 0
@@ -97,10 +102,9 @@ class main_task():
# logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
self.by_cmd.send_speed_omega(pid_out)
if var.switch_lane_model:
self.socket.send_string("1")
self.socket1.send_string("")
resp = self.socket1.recv_pyobj()
else:
self.socket.send_string("")
resp = self.socket.recv_pyobj()
var.switch_lane_model = False
return
self.socket.send_string("")
resp = self.socket.recv_pyobj()
self.parse_data(resp)

103
subtask.py
View File

@@ -118,7 +118,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
logger.info(f"calibrate_right_new:停车后的误差是{error}")
if abs(error) > 8:
logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3
error = error * 2.3
if error > 0:
by_cmd.send_distance_x(-10, int(error))
else:
@@ -406,7 +406,7 @@ class get_block1():
else:
var.first_block = tlabel.BBLOCK
var.second_block = tlabel.RBLOCK
calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5)
calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
logger.info("抓取块")
by_cmd.send_position_axis_z(30, 60)
@@ -421,10 +421,10 @@ class get_block1():
by_cmd.send_angle_claw_arm(175)
time.sleep(0.1)
by_cmd.send_position_axis_x(1, 120)
by_cmd.send_position_axis_x(1, 100)
time.sleep(1)
by_cmd.send_position_axis_z(30, 70)
time.sleep(0.1)
time.sleep(0.5)
by_cmd.send_angle_claw(63)
time.sleep(0.5)
@@ -435,6 +435,7 @@ class get_block1():
time.sleep(0.5)
# by_cmd.send_angle_storage(55)
# time.sleep(1)
by_cmd.send_position_axis_z(30, 60)
pass
def nexec(self):
@@ -463,13 +464,12 @@ class get_block2():
return False
def exec(self):
car_stop()
calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(225)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_z(30, 60)
time.sleep(1)
time.sleep(0.1)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
by_cmd.send_angle_claw(25)
@@ -521,7 +521,7 @@ class put_block():
# 放置第二個塊
by_cmd.send_angle_storage(20)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 110)
by_cmd.send_position_axis_z(30, 120)
time.sleep(1.5)
by_cmd.send_angle_claw_arm(180)
@@ -597,7 +597,7 @@ class get_bball():
car_stop()
time.sleep(0.5)
for _ in range(3):
calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5)
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
logger.info("抓蓝色球")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(45)
@@ -835,21 +835,31 @@ class put_hanoi1():
logger.info("应该向左转")
# # 校准 omega
# if error > 0:
# by_cmd.send_angle_omega(-20,abs(var.lane_error))
# else:
# by_cmd.send_angle_omega(20,abs(var.lane_error))
# time.sleep(0.5)
# car_stop()
# time.sleep(0.5)
# by_cmd.send_distance_x(10, 200)
# by_cmd.send_distance_x(10, 180)
by_cmd.send_distance_x(10, 180)
time.sleep(1.5)
# 校准 omega
for _ in range(10):
ret, box = filter.get(utils.direction)
if ret:
error = (box[0][2] + box[0][0] - 320) / 2
by_cmd.send_speed_omega(-error * 0.8)
time.sleep(0.2)
car_stop()
# 前进
# by_cmd.send_distance_x(10, 200)
# by_cmd.send_distance_x(10, 180)
# by_cmd.send_distance_x(10, 180)
# time.sleep(1.5)
# car_stop()
while True:
by_cmd.send_speed_x(8.5)
ret, box = filter.get(utils.direction)
if ret:
if abs(box[0][2] - box[0][0]) > 41:
car_stop()
break
# 根据方向初始化执行器位置
if utils.direction is tlabel.RMARK:
# FIXME 右侧的爪子会被 storage 挡住
@@ -867,16 +877,18 @@ class put_hanoi1():
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
# by_cmd.send_angle_omega(-25,430)
by_cmd.send_angle_omega(-45,238)
# by_cmd.send_angle_omega(-55,194)
# by_cmd.send_angle_omega(-45,238)
by_cmd.send_angle_omega(-45,252)
time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
else:
utils.direction = tlabel.LMARK
# by_cmd.send_angle_omega(25,430)
by_cmd.send_angle_omega(45,238)
# by_cmd.send_angle_omega(55,194)
# by_cmd.send_angle_omega(45,238)
by_cmd.send_angle_omega(45,252)
time.sleep(2)
while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0)
@@ -903,16 +915,16 @@ class put_hanoi2():
self.target_label = tlabel.SPILLER
def init(self):
logger.info("物资盘点 2 初始化")
var.task_speed = 8.5
var.task_speed = 10
if utils.direction == tlabel.RMARK:
# 15
self.offset = 19
self.offset = 14
# self.platform_offset = -25
self.platform_offset = -10
self.platform_offset = -19
else:
self.offset = 10
self.offset = 14
#self.platform_offset = -30
self.platform_offset = -15
self.platform_offset = -19
def find(self):
# ret, box = filter.get(self.target_label)
ret, box = filter.get(tlabel.TPLATFORM)
@@ -941,7 +953,7 @@ class put_hanoi2():
logger.info("抓大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(63)
time.sleep(2)
by_cmd.send_angle_claw(40)
@@ -970,7 +982,7 @@ class put_hanoi2():
time.sleep(0.5)
logger.info("放大平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
time.sleep(1.5)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(1)
@@ -1001,7 +1013,7 @@ class put_hanoi2():
logger.info("抓中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(63)
time.sleep(2)
by_cmd.send_angle_claw(35)
@@ -1030,9 +1042,9 @@ class put_hanoi2():
time.sleep(0.5)
logger.info("放中平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 100)
by_cmd.send_position_axis_z(30, 120)
time.sleep(2)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
@@ -1042,7 +1054,7 @@ class put_hanoi2():
time.sleep(1)
pass
else:
by_cmd.send_position_axis_z(30, 100)
by_cmd.send_position_axis_z(30, 120)
time.sleep(2)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
@@ -1061,7 +1073,7 @@ class put_hanoi2():
logger.info("抓小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw(50)
time.sleep(2)
by_cmd.send_angle_claw(27)
@@ -1090,9 +1102,9 @@ class put_hanoi2():
time.sleep(0.5)
logger.info("放小平台")
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 170)
by_cmd.send_position_axis_z(30, 190) # 170
time.sleep(1.5)
by_cmd.send_position_axis_x(1, 130)
by_cmd.send_position_axis_x(1, 150)
time.sleep(2)
by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5)
@@ -1100,12 +1112,13 @@ class put_hanoi2():
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10)
time.sleep(1)
# by_cmd.send_speed_y(15)
# time.sleep(0.1)
# by_cmd.send_speed_y(10)
# time.sleep(0.12)
# car_stop()
pass
else:
by_cmd.send_position_axis_z(30, 170)
by_cmd.send_position_axis_z(30, 190)
time.sleep(1.5)
by_cmd.send_position_axis_x(1, 40)
time.sleep(2)
@@ -1121,8 +1134,12 @@ class put_hanoi2():
def nexec(self):
pass
def after(self):
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
var.switch_lane_model = True
var.switch_lane_model = False
if utils.direction is tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"] - 0.2, cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
else:
var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
pass
class put_hanoi3():