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

暂存

Browse Source 
fix: 修复task多次开启问题
feat: 新增 891 参数（医院停早，红球靠外）
pref: 弃用 action 类
This commit is contained in:
bmy
2024-08-17 01:40:07 +08:00
parent 7ae8162da7
commit 91d6491f64
10 changed files with 1828 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
subtask.py
View File

@@ -10,7 +10,6 @@ import toml
import zmq
import time
import variable as var
import action as act
import re
import math
import json
@@ -116,7 +115,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) <= 8:
if abs(error) <= 10:
car_stop()
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
ret, error = filter.aim_right(label)
@@ -233,10 +232,10 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(label)
while not ret:
ret, box = filter.get(label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
# ret, box = filter.get(label)
# while not ret:
# ret, box = filter.get(label)
# error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
@@ -288,10 +287,10 @@ def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.
car_stop()
logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
ret, box = filter.get(target_label)
while not ret:
ret, box = filter.get(target_label)
error = (box[0][2] + box[0][0] - 320) / 2 + offset
# ret, box = filter.get(target_label)
# while not ret:
# ret, box = filter.get(target_label)
# error = (box[0][2] + box[0][0] - 320) / 2 + offset
logger.info(f"停车后像素误差:{error}")
break
return True
@@ -426,8 +425,7 @@ class task_queuem(task):
class get_block1():
def init(self):
var.task_speed = 15
act.cmd.camera(0)
act.cmd.z2(20, 60, 0)
by_cmd.send_angle_camera(0)
filter.switch_camera(1)
# if cfg['get_block']['first_block'] == "blue":
# self.target_label = tlabel.BBLOCK
@@ -464,7 +462,7 @@ class get_block1():
logger.info("抓取块")
by_cmd.send_position_axis_z(30, 80)
time.sleep(1)
time.sleep(1.5)
by_cmd.send_angle_claw(63)
by_cmd.send_position_axis_x(1, 20)
time.sleep(1)
@@ -557,7 +555,7 @@ class put_block():
ret, box = filter.get(tlabel.HOSPITAL)
if ret > 0:
width = box[0][2] - box[0][0]
if width > 130: #135
if width > 125: #135 130 128
return True
return False
def exec(self):
@@ -568,7 +566,7 @@ class put_block():
by_cmd.send_distance_x(10, 100)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 30) # 20
by_cmd.send_position_axis_x(1, 50) # 20
time.sleep(1)
by_cmd.send_angle_claw(63)
time.sleep(1)
@@ -593,7 +591,7 @@ class put_block():
time.sleep(1)
by_cmd.send_position_axis_z(30, 0)
time.sleep(0.5)
by_cmd.send_position_axis_x(1, 30)
by_cmd.send_position_axis_x(1, 50)
time.sleep(1.5)
by_cmd.send_angle_claw(45)
time.sleep(1)
@@ -651,6 +649,7 @@ class get_bball():
car_stop()
time.sleep(0.5)
for _ in range(3):
by_cmd.send_position_axis_z(30, 155)
calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
logger.info("抓蓝色球")
time.sleep(0.5)
@@ -672,7 +671,7 @@ class get_bball():
time.sleep(0.5)
by_cmd.send_angle_claw_arm(45)
time.sleep(1)
by_cmd.send_position_axis_z(30, 155)
# by_cmd.send_position_axis_z(30, 155)
# 继续向前走
# by_cmd.send_speed_x(4)
pass
@@ -710,10 +709,10 @@ class up_tower():
calibrate_new(tlabel.TOWER, offset = 20, run = True)
time.sleep(1)
# calibrate(tlabel.TOWER, 27, False, 6)
by_cmd.send_distance_x(-10, 120)
by_cmd.send_distance_x(-10, 100)
time.sleep(1)
# 上古參數
by_cmd.send_distance_y(-10, 50) # 80
# by_cmd.send_distance_y(-10, 50) # 80
# 6_9 模型參數
# by_cmd.send_distance_y(-10, 40)
# 7_12_3 模型參數
@@ -724,6 +723,11 @@ class up_tower():
# time.sleep(3)
# by_cmd.send_speed_y(-10)
# time.sleep(0.15)
# 8822
# by_cmd.send_distance_y(-10, 50)
# 891
by_cmd.send_distance_y(-10, 35)
time.sleep(0.3)
by_cmd.send_angle_zhuan(10)
time.sleep(12)
@@ -770,8 +774,8 @@ class get_rball():
by_cmd.send_angle_scoop(20)
# 上古參數
# by_cmd.send_distance_y(-15, 50) # 50 # 70
by_cmd.send_distance_y(-15, 40) # 50 # 70
time.sleep(1.5)
# by_cmd.send_distance_y(-15, 40) # 50 # 70
# time.sleep(1.5)
# 6_9 參數
# by_cmd.send_distance_y(-15, 35)
# time.sleep(2)
@@ -779,6 +783,12 @@ class get_rball():
# by_cmd.send_distance_y(-15, 45)
# time.sleep(2)
# car_stop()
# 8822 参数
# by_cmd.send_distance_y(-15, 40)
# time.sleep(1.5)
# 891 参数
by_cmd.send_distance_y(-15, 30)
time.sleep(0.3)
calibrate_new(tlabel.RBALL,offset = 44, run = True)
time.sleep(1)
logger.info("抓红球")
@@ -826,14 +836,14 @@ class put_bball():
calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
# by_cmd.send_distance_x(10, 10)
# 向左运动
by_cmd.send_distance_y(-10, 35)
# by_cmd.send_distance_y(-10, 35)
# by_cmd.send_angle_storage(10)
# time.sleep(1)
by_cmd.send_angle_storage(50)
logger.info("把球放篮筐里")
time.sleep(1)
time.sleep(2)
# by_cmd.send_distance_y(10, 55)
by_cmd.send_angle_storage(20)
# time.sleep(1)
@@ -959,7 +969,7 @@ class put_hanoi1():
def after(self):
# var.switch_lane_model = True
if utils.direction == tlabel.RMARK:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
else:
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
pass
@@ -1102,7 +1112,7 @@ class put_hanoi2():
time.sleep(1)
pass
# ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.LPILLER, [tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
return
@@ -1163,7 +1173,7 @@ class put_hanoi2():
time.sleep(2)
pass
# ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
ret = hanoi_calibrate(tlabel.MPILLER, [tlabel.LPILLER, tlabel.TPLATFORM], offset = self.offset, run_direc = -1, run_speed = 5)
if not ret:
logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
return
@@ -1537,14 +1547,6 @@ class move_area2():
if len(resp_commands) == 0:
return
action_list = resp_commands
# 先检查一下 action 是否生成正确，如果不正确直接跳过
actions_keys = self.action_dict.keys()
try:
for action in action_list:
if not (action.get('action') in actions_keys):
return
except:
return
# 进入停车区域
by_cmd.send_distance_y(10, 450)
time.sleep((450 * 5 / 1000) + 0.5)
@@ -1656,6 +1658,8 @@ class kick_ass():
# by_cmd.send_speed_x(25)
# time.sleep(4)
pass
def nexec(self):
pass
def after(self):