feat: 增加雙lane服務端，修改hanoi動作以應對放置在圍擋上的問題
This commit is contained in:
103
subtask.py
103
subtask.py
@@ -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,20 +835,30 @@ 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:
|
||||
@@ -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():
|
||||
|
||||
Reference in New Issue
Block a user