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-09 21:54:22 +08:00
parent 188127b419
commit 9f4ffdbe7e
6 changed files with 161 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
subtask.py
View File

@@ -38,8 +38,15 @@ def car_stop():
time.sleep(0.2)
by_cmd.send_speed_omega(0)
def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
not_found_counts = 0
ret, error = filter.aim_right(label)
while not ret:
not_found_counts += 1
if not_found_counts >= 20:
not_found_counts = 0
error = -320 # error > 0 front run
logger.info("找不到 直接向前")
break
ret, error = filter.aim_right(label)
error += offset
@@ -48,6 +55,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
by_cmd.send_speed_x(-run_speed)
else:
by_cmd.send_speed_x(run_speed)
pass
# 停的位置已经很接近目标，可以直接使用 distance 校准
else:
error = error * 3
@@ -62,7 +70,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
ret, error = filter.aim_right(label)
error += offset
if ret:
if abs(error) <= 5:
if abs(error) <= 8:
car_stop()
logger.info("可以停车了")
@@ -262,7 +270,7 @@ class get_block():
calibrate_new(self.target_label, offset = 12, run = True)
logger.info("抓取块")
time.sleep(0.5)
by_cmd.send_position_axis_z(15, 70)
by_cmd.send_position_axis_z(20, 60)
by_cmd.send_position_axis_x(1, 140)
time.sleep(4)
by_cmd.send_angle_claw_arm(220)
@@ -338,8 +346,8 @@ class put_block():
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
by_cmd.send_position_axis_z(15, 0)
time.sleep(2)
by_cmd.send_position_axis_z(20, 0)
time.sleep(3)
by_cmd.send_angle_claw(63)
# time.sleep(2)
# by_cmd.send_position_axis_z(20, 130)
@@ -353,8 +361,9 @@ class put_block():
# by_cmd.send_position_axis_z(10, 170)
# 下一动作预备位置
by_cmd.send_position_axis_z(20, 130)
time.sleep(3)
time.sleep(4)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
by_cmd.send_angle_claw_arm(36)
pass
def nexec(self):
@@ -371,12 +380,14 @@ class get_bball():
def init(self):
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
by_cmd.send_position_axis_z(20, 140)
by_cmd.send_position_axis_z(20, 135)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 0)
time.sleep(0.5)
by_cmd.send_angle_storage(0)
while (by_cmd.send_angle_storage(0) == -1):
by_cmd.send_angle_storage(0)
# 调试 临时换源
socket.send_string("1")
socket.recv()
@@ -394,22 +405,21 @@ class get_bball():
car_stop()
time.sleep(0.5)
for _ in range(3):
explore_calibrate_new(tlabel.BBALL, offset = 27, run_direc = 1, run_speed = 4)
# calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
logger.info("抓蓝色球")
time.sleep(0.5)
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_claw(54)
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_position_axis_x(1, 160)
time.sleep(1)
by_cmd.send_angle_claw(70)
time.sleep(1)
by_cmd.send_angle_claw(30)
time.sleep(1)
by_cmd.send_distance_axis_z(20, 20)
time.sleep(2)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1)
by_cmd.send_position_axis_x(1, 0)
time.sleep(1.5)
by_cmd.send_angle_claw_arm(67)
time.sleep(0.5)
by_cmd.send_angle_claw(54)
@@ -418,7 +428,8 @@ class get_bball():
time.sleep(1)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(1)
# 继续向前走
# by_cmd.send_speed_x(4)
# 下一动作预备位置
by_cmd.send_angle_claw(30)
by_cmd.send_position_axis_z(20, 0)
@@ -426,7 +437,7 @@ class get_bball():
pass
def nexec(self):
by_cmd.send_angle_claw(30)
by_cmd.send_positioin_axis_z(20, 0)
by_cmd.send_position_axis_z(20, 0)
time.sleep(2)
pass
@@ -490,7 +501,7 @@ class get_rball():
logger.info("抓红球")
# by_cmd.send_angle_scoop(15)
time.sleep(0.5)
by_cmd.send_position_axis_z(15, 170)
by_cmd.send_position_axis_z(20, 170)
time.sleep(5)
by_cmd.send_angle_scoop(7)
by_cmd.send_distance_y(10, 65)
@@ -507,6 +518,7 @@ class put_bball():
logger.info("派发物资初始化")
socket.send_string("1")
socket.recv()
by_cmd.send_position_axis_z(20, 0)
while (by_cmd.send_angle_camera(90) == -1):
by_cmd.send_angle_camera(90)
def find(self):
@@ -525,10 +537,12 @@ class put_bball():
by_cmd.send_angle_storage(55)
logger.info("把球放篮筐里")
time.sleep(1)
time.sleep(2)
by_cmd.send_distance_y(10, 65)
time.sleep(1)
by_cmd.send_angle_storage(20)
pass
def nexec(self):
pass
@@ -564,6 +578,8 @@ class put_hanoi1():
by_cmd.send_speed_omega(0)
time.sleep(0.2)
by_cmd.send_position_axis_z(20, 130)
# 校准牌子
if utils.direction_right > utils.direction_left:
ret, error = filter.aim_near(tlabel.RMARK)
@@ -577,6 +593,8 @@ class put_hanoi1():
ret, error = filter.aim_near(tlabel.LMARK)
utils.direction = tlabel.LMARK
logger.info("应该向左转")
# 校准 omega
if error > 0:
by_cmd.send_angle_omega(-20,abs(utils.lane_error))
@@ -587,13 +605,20 @@ class put_hanoi1():
time.sleep(0.5)
by_cmd.send_distance_x(10, 200)
time.sleep(0.5)
# 根据岔路口切换爪子的方向
if utils.direction_right > utils.direction_left:
pass
# 根据方向初始化执行器位置
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_x(1, 0)
by_cmd.send_angle_claw_arm(36)
by_cmd.send_angle_storage(0)
else:
pass
by_cmd.send_position_axis_x(1, 150)
by_cmd.send_angle_claw_arm(215)
by_cmd.send_angle_storage(55)
time.sleep(1.5)
by_cmd.send_position_axis_z(20, 0)
if utils.direction_right > utils.direction_left:
utils.direction = tlabel.RMARK
by_cmd.send_angle_omega(-25,430)
@@ -609,7 +634,6 @@ class put_hanoi1():
time.sleep(0.5)
socket.send_string("1")
socket.recv()
pass
def nexec(self):
pass
@@ -645,6 +669,7 @@ class put_hanoi2():
return False
def exec(self):
logger.info(f"direction:{utils.direction.name}")
logger.info(f"offset:{self.offset}")
utils.task_speed = 0
car_stop()
@@ -653,53 +678,92 @@ class put_hanoi2():
time.sleep(1)
explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
logger.info("抓大平台")
time.sleep(5)
if utils.direction is tlabel.RMARK:
by_cmd.send_position_axis_z(20, 40)
by_cmd.send_position_axis_x(1, 160)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(85)
time.sleep(4)
by_cmd.send_angle_claw(50)
time.sleep(4)
by_cmd.send_position_axis_x(2, 80)
by_cmd.send_distance_axis_z(20, 10)
else:
by_cmd.send_position_axis_z(20, 30)
by_cmd.send_position_axis_x(1, 40)
by_cmd.send_angle_claw(63)
time.sleep(1)
by_cmd.send_angle_claw(90)
time.sleep(4)
by_cmd.send_angle_claw(50)
time.sleep(2)
by_cmd.send_distance_axis_z(20, 20)
time.sleep(2)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
logger.info("放大平台")
time.sleep(5)
if utils.direction is tlabel.RMARK:
by_cmd.send_distance_axis_z(20, -10)
by_cmd.send_position_axis_x(2, 160)
time.sleep(4)
by_cmd.send_angle_claw(90)
by_cmd.send_position_axis_x(1, 0)
else:
by_cmd.send_position_axis_x(2, 40)
time.sleep(4)
by_cmd.send_distance_axis_z(20, -20)
time.sleep(2)
by_cmd.send_angle_claw(90)
time.sleep(2)
by_cmd.send_position_axis_x(1, 160)
explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
logger.info("抓中平台")
time.sleep(5)
if utils.direction is tlabel.RMARK:
pass
else:
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
logger.info("放中平台")
time.sleep(5)
if utils.direction is tlabel.RMARK:
pass
else:
pass
explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 6)
logger.info("抓小平台")
time.sleep(5)
if utils.direction is tlabel.RMARK:
pass
else:
pass
explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 6)
logger.info("放小平台")
time.sleep(5)
while True:
if utils.direction is tlabel.RMARK:
pass
# 往回走一段 然后向前行进校准
time.sleep(1)
logger.info(f"方向{direction}")
if utils.lane_error > 0:
by_cmd.send_angle_omega(-20,abs(utils.lane_error))
else:
by_cmd.send_angle_omega(20,abs(utils.lane_error))
pass
# while True:
# pass
time.sleep(1)
sub_put_hanoi2(tlabel.LPILLER, self.distance_lp, True, True)
time.sleep(5)
# 对准中蓝色柱体
sub_put_hanoi2(tlabel.MPILLER, self.distance_mp, True, True)
time.sleep(5)
# 根据 direction 确定移动方向
# 移动 self.distance_mp 距离
# 放置物块
# 回移相同距离
sub_put_hanoi2(tlabel.SPILLER, self.distance_sp, True, True)
time.sleep(5)
# 对准小红色柱体
# 根据 direction 确定移动方向
# 移动 self.distance_sp 距离
# 放置物块
def nexec(self):
pass
class put_hanoi3():
def init(self):
logger.info("完成任务，爪回左侧")
by_cmd.send_angle_claw_arm(128)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 150)
time.sleep(1)
by_cmd.send_angle_claw_arm(220)
def find(self):
time.sleep(1)
return True
def exec(self):
by_cmd.send_position_axis_z(20, 100)
pass
def nexec(self):
pass
@@ -754,9 +818,10 @@ class move_area2():
# 离开停车区域
by_cmd.send_distance_y(-10, 450)
time.sleep(3)
by_cmd.send_position_axis_z(20, 0)
pass
def nexec(self):
by_cmd.send_position_axis_z(20, 0)
pass
# 扫黑除暴
class kick_ass():