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

19
cfg_main.toml
View File

@@ -3,12 +3,12 @@ logger_filename = "log/file_{time}.log"
logger_format = "{time} {level} {message}"
[task]
GetBlock_enable = true # 人员施救使能
PutBlock_enable = true # 紧急转移使能
GetBBall_enable = true # 整装上阵使能
UpTower_enable = true # 通信抢修使能
GetRBall_enable = true # 高空排险使能
PutBBall_enable = true # 派发物资使能
GetBlock_enable = false # 人员施救使能
PutBlock_enable = false # 紧急转移使能
GetBBall_enable = false # 整装上阵使能
UpTower_enable = false # 通信抢修使能
GetRBall_enable = false # 高空排险使能
PutBBall_enable = false # 派发物资使能
PutHanoi_enable = true # 物资盘点使能
MoveArea_enable = true # 应急避险使能
KickAss_enable = true # 扫黑除暴使能
@@ -16,14 +16,15 @@ KickAss_enable = true # 扫黑除暴使能
[find_counts]
GetBlock_counts = 20 # 人员施救计数
PutBlock_counts = 20 # 紧急转移计数
GetBBall_counts = 10 # 整装上阵计数
GetBBall_counts = 5 # 整装上阵计数
UpTower_counts = 10 # 通信抢修计数
GetRBall_counts = 2 # 高空排险计数
GetRBall_counts = 10 # 高空排险计数
PutBBall_counts = 5 # 派发物资计数
PutHanoi1_counts = 12 # 物资盘点计数
PutHanoi1_counts = 18 # 物资盘点计数
PutHanoi2_counts = 2 # 物资盘点计数
PutHanoi3_counts = 2 # 物资盘点计数
MoveArea1_counts = 10 # 应急避险计数
MoveArea2_counts = 1700 # 应急避险第二阶段计数
KickAss_counts = 20 # 扫黑除暴计数

2
cfg_subtask.toml
View File

@@ -1,5 +1,5 @@
[get_block]
first_block = "blue"
first_block = "red"
[put_block]
[get_bball]

3
main.py
View File

@@ -28,6 +28,7 @@ task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'],
task_queue.put(sb.task(sb.put_bball, cfg_main['find_counts']['PutBBall_counts'], cfg_main['task']['GetBBall_enable'] and cfg_main['task']['PutBBall_enable']))
task_queue.put(sb.task(sb.put_hanoi1, cfg_main['find_counts']['PutHanoi1_counts'], True)) # 无论是否进行任务，检测标识并转向都是必须进行的
task_queue.put(sb.task(sb.put_hanoi2, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable']))
task_queue.put(sb.task(sb.put_hanoi3, cfg_main['find_counts']['PutHanoi2_counts'], cfg_main['task']['PutHanoi_enable']))
task_queue.put(sb.task(sb.move_area1, cfg_main['find_counts']['MoveArea1_counts'], cfg_main['task']['MoveArea_enable']))
task_queue.put(sb.task(sb.move_area2, cfg_main['find_counts']['MoveArea2_counts'], cfg_main['task']['MoveArea_enable']))
task_queue.put(sb.task(sb.kick_ass, cfg_main['find_counts']['KickAss_counts'], cfg_main['task']['KickAss_enable']))
@@ -45,7 +46,7 @@ worker = threading.Thread(target=worker_thread, daemon=True)
worker.start()
# if (cmd_py_obj.send_angle_camera(180) == -1):
# cmd_py_obj.send_angle_camera(180)
cmd_py_obj.send_distance_axis_x(1, 140)
cmd_py_obj.send_position_axis_x(1, 140)
time.sleep(0.5)
cmd_py_obj.send_angle_storage(20)
time.sleep(0.5)

43
majtask.py
View File

@@ -11,9 +11,9 @@ class PidWrap:
def set_target(self, target):
self.pid_t.setpoint = target
def set(self, kp, ki, kd):
self.pid_t.kp = kp
self.pid_t.ki = ki
self.pid_t.kd = kd
self.pid_t.Kp = kp
self.pid_t.Ki = ki
self.pid_t.Kd = kd
def get(self, val_in):
return self.pid_t(val_in)
@@ -33,7 +33,7 @@ class main_task():
self.by_cmd = by_cmd
# 转向 pid
self.pid1 = PidWrap(0.7, 0, 0,output_limits=40)
self.pid1 = PidWrap(1, 0, 0, output_limits=40)
self.pid1.set_target(0)
def parse_data(self,data):
@@ -69,26 +69,33 @@ class main_task():
self.y = 0
error_abs = abs(self.lane_error)
if error_abs < 10:
self.pid1.set(0.7, 0, 0)
speed = 13
elif error_abs > 45:
speed = 6
self.pid1.set(1.8, 0, 0)
elif error_abs > 35:
speed = 8
self.pid1.set(1.5, 0, 0)
elif error_abs > 25:
if error_abs > 45:
speed = 10
self.pid1.set(1, 0, 0)
# pid_out = self.lane_error * 1.1
# self.pid1.set(1, 0, 0)
elif error_abs > 35:
speed = 10
# pid_out = self.lane_error * 1
# self.pid1.set(1, 0, 0)
elif error_abs > 25:
speed = 13
# pid_out = self.lane_error * 1
# self.pid1.set(1.8, 0, 0)
elif error_abs > 15:
speed = 13
# pid_out = self.lane_error * 0.8
# self.pid1.set(1.8, 0, 0)
else:
self.pid1.set(0.8, 0, 0)
speed = 11
speed = 15
# pid_out = self.lane_error * 0.4
# self.pid1.set(0.7, 0, 0)
if utils.task_speed != 0:
speed = utils.task_speed
self.by_cmd.send_speed_x(speed)
# pid_out = self.pid1.get(self.lane_error*0.65)
pid_out = self.pid1.get(self.lane_error*0.7)
pid_out = self.pid1.get(self.lane_error)
# pid_out = -pid_out
# logger.debug(f"err={self.lane_error}, pwm out={pid_out}")
self.by_cmd.send_speed_omega(pid_out)
self.socket.send_string("")

173
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)
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
else:
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:
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
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)
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 距离
# 放置物块
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():

4
test/test_filter.py
View File

@@ -17,7 +17,7 @@ logger.info("subtask yolo client init")
filter = label_filter(socket)
filter.switch_camera(1)
offset = 15
offset = 12
while True:
time.sleep(0.2)
# ret1 = filter.get_mult(tlabel.RMARK)
@@ -27,7 +27,7 @@ while True:
# if abs(error) < 40:
# logger.info('yes')
label = tlabel.BASKET
label = tlabel.RBLOCK
ret, box = filter.get(label)
while not ret:
# 注意这里一定要保证摄像头内有该目标 否则会无限循环