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 = [ server_command = [
{"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"}, {"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_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/yolo_server/", "script": "yolo_infer_server.py"},
# {"path": "/home/evan/Workplace/project_infer/ocr_server/", "script": "ocr_infer_server.py"},
] ]
processes = [] processes = []

14
majtask.py
View File

@@ -8,9 +8,14 @@ import variable as var
class main_task(): class main_task():
def __init__(self,by_cmd): def __init__(self,by_cmd):
# lane infer server
self.context = zmq.Context() self.context = zmq.Context()
self.socket = self.context.socket(zmq.REQ) self.socket = self.context.socket(zmq.REQ)
self.socket.connect("tcp://localhost:6666") 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 self.x = 0
@@ -97,10 +102,9 @@ class main_task():
# logger.debug(f"err={self.lane_error}, pwm out={pid_out}") # logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
self.by_cmd.send_speed_omega(pid_out) self.by_cmd.send_speed_omega(pid_out)
if var.switch_lane_model: 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() resp = self.socket.recv_pyobj()
var.switch_lane_model = False
return
self.socket.send_string("")
resp = self.socket.recv_pyobj()
self.parse_data(resp) 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}") logger.info(f"calibrate_right_new:停车后的误差是{error}")
if abs(error) > 8: if abs(error) > 8:
logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准") logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
error = error * 3 error = error * 2.3
if error > 0: if error > 0:
by_cmd.send_distance_x(-10, int(error)) by_cmd.send_distance_x(-10, int(error))
else: else:
@@ -406,7 +406,7 @@ class get_block1():
else: else:
var.first_block = tlabel.BBLOCK var.first_block = tlabel.BBLOCK
var.second_block = tlabel.RBLOCK 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("抓取块") logger.info("抓取块")
by_cmd.send_position_axis_z(30, 60) by_cmd.send_position_axis_z(30, 60)
@@ -421,10 +421,10 @@ class get_block1():
by_cmd.send_angle_claw_arm(175) by_cmd.send_angle_claw_arm(175)
time.sleep(0.1) time.sleep(0.1)
by_cmd.send_position_axis_x(1, 120) by_cmd.send_position_axis_x(1, 100)
time.sleep(1) time.sleep(1)
by_cmd.send_position_axis_z(30, 70) by_cmd.send_position_axis_z(30, 70)
time.sleep(0.1) time.sleep(0.5)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
time.sleep(0.5) time.sleep(0.5)
@@ -435,6 +435,7 @@ class get_block1():
time.sleep(0.5) time.sleep(0.5)
# by_cmd.send_angle_storage(55) # by_cmd.send_angle_storage(55)
# time.sleep(1) # time.sleep(1)
by_cmd.send_position_axis_z(30, 60)
pass pass
def nexec(self): def nexec(self):
@@ -463,13 +464,12 @@ class get_block2():
return False return False
def exec(self): def exec(self):
car_stop() 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("抓取块") logger.info("抓取块")
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw_arm(225)
by_cmd.send_angle_claw(63) by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_z(30, 60) time.sleep(0.1)
time.sleep(1)
by_cmd.send_position_axis_x(1, 20) by_cmd.send_position_axis_x(1, 20)
time.sleep(1) time.sleep(1)
by_cmd.send_angle_claw(25) by_cmd.send_angle_claw(25)
@@ -521,7 +521,7 @@ class put_block():
# 放置第二個塊 # 放置第二個塊
by_cmd.send_angle_storage(20) 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) by_cmd.send_position_axis_z(30, 120)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_angle_claw_arm(180) by_cmd.send_angle_claw_arm(180)
@@ -597,7 +597,7 @@ 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 = 16, 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)
by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw_arm(45)
@@ -835,21 +835,31 @@ class put_hanoi1():
logger.info("应该向左转") logger.info("应该向左转")
# # 校准 omega # 校准 omega
# if error > 0: for _ in range(10):
# by_cmd.send_angle_omega(-20,abs(var.lane_error)) ret, box = filter.get(utils.direction)
# else: if ret:
# by_cmd.send_angle_omega(20,abs(var.lane_error)) error = (box[0][2] + box[0][0] - 320) / 2
# time.sleep(0.5) by_cmd.send_speed_omega(-error * 0.8)
# car_stop() time.sleep(0.2)
# 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)
car_stop() 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: if utils.direction is tlabel.RMARK:
# FIXME 右侧的爪子会被 storage 挡住 # FIXME 右侧的爪子会被 storage 挡住
@@ -867,16 +877,18 @@ class put_hanoi1():
if utils.direction_right > utils.direction_left: if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK utils.direction = tlabel.RMARK
# by_cmd.send_angle_omega(-25,430) # 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(-55,194)
# by_cmd.send_angle_omega(-45,238)
by_cmd.send_angle_omega(-45,252)
time.sleep(2) time.sleep(2)
while (by_cmd.send_angle_camera(90) == -1): while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90) by_cmd.send_angle_camera(90)
else: else:
utils.direction = tlabel.LMARK utils.direction = tlabel.LMARK
# by_cmd.send_angle_omega(25,430) # 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(55,194)
# by_cmd.send_angle_omega(45,238)
by_cmd.send_angle_omega(45,252)
time.sleep(2) time.sleep(2)
while (by_cmd.send_angle_camera(0) == -1): while (by_cmd.send_angle_camera(0) == -1):
by_cmd.send_angle_camera(0) by_cmd.send_angle_camera(0)
@@ -903,16 +915,16 @@ class put_hanoi2():
self.target_label = tlabel.SPILLER self.target_label = tlabel.SPILLER
def init(self): def init(self):
logger.info("物资盘点 2 初始化") logger.info("物资盘点 2 初始化")
var.task_speed = 8.5 var.task_speed = 10
if utils.direction == tlabel.RMARK: if utils.direction == tlabel.RMARK:
# 15 # 15
self.offset = 19 self.offset = 14
# self.platform_offset = -25 # self.platform_offset = -25
self.platform_offset = -10 self.platform_offset = -19
else: else:
self.offset = 10 self.offset = 14
#self.platform_offset = -30 #self.platform_offset = -30
self.platform_offset = -15 self.platform_offset = -19
def find(self): def find(self):
# ret, box = filter.get(self.target_label) # ret, box = filter.get(self.target_label)
ret, box = filter.get(tlabel.TPLATFORM) ret, box = filter.get(tlabel.TPLATFORM)
@@ -941,7 +953,7 @@ class put_hanoi2():
logger.info("抓大平台") logger.info("抓大平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) 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) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(40) by_cmd.send_angle_claw(40)
@@ -970,7 +982,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_x(1, 130) by_cmd.send_position_axis_x(1, 150)
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_distance_axis_z(30, -20) by_cmd.send_distance_axis_z(30, -20)
time.sleep(1) time.sleep(1)
@@ -1001,7 +1013,7 @@ class put_hanoi2():
logger.info("抓中平台") logger.info("抓中平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) 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) by_cmd.send_angle_claw(63)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(35) by_cmd.send_angle_claw(35)
@@ -1030,9 +1042,9 @@ 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, 100) by_cmd.send_position_axis_z(30, 120)
time.sleep(2) time.sleep(2)
by_cmd.send_position_axis_x(1, 130) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(30, -20) by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5) time.sleep(0.5)
@@ -1042,7 +1054,7 @@ class put_hanoi2():
time.sleep(1) time.sleep(1)
pass pass
else: else:
by_cmd.send_position_axis_z(30, 100) by_cmd.send_position_axis_z(30, 120)
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)
@@ -1061,7 +1073,7 @@ class put_hanoi2():
logger.info("抓小平台") logger.info("抓小平台")
if utils.direction is tlabel.RMARK: if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(30, 10) 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) by_cmd.send_angle_claw(50)
time.sleep(2) time.sleep(2)
by_cmd.send_angle_claw(27) by_cmd.send_angle_claw(27)
@@ -1090,9 +1102,9 @@ 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, 170) by_cmd.send_position_axis_z(30, 190) # 170
time.sleep(1.5) time.sleep(1.5)
by_cmd.send_position_axis_x(1, 130) by_cmd.send_position_axis_x(1, 150)
time.sleep(2) time.sleep(2)
by_cmd.send_distance_axis_z(30, -20) by_cmd.send_distance_axis_z(30, -20)
time.sleep(0.5) time.sleep(0.5)
@@ -1100,12 +1112,13 @@ class put_hanoi2():
time.sleep(0.5) time.sleep(0.5)
by_cmd.send_position_axis_x(1, 10) by_cmd.send_position_axis_x(1, 10)
time.sleep(1) 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() # car_stop()
pass pass
else: else:
by_cmd.send_position_axis_z(30, 170) by_cmd.send_position_axis_z(30, 190)
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)
@@ -1121,8 +1134,12 @@ class put_hanoi2():
def nexec(self): def nexec(self):
pass pass
def after(self): 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 = False
var.switch_lane_model = True 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 pass
class put_hanoi3(): class put_hanoi3():