1407 lines
47 KiB
Python
1407 lines
47 KiB
Python
|
from enum import Enum
|
|||
|
|
from loguru import logger
|
|||
|
|
from utils import label_filter
|
|||
|
|
from utils import tlabel
|
|||
|
|
from utils import LLM
|
|||
|
|
from utils import CountRecord
|
|||
|
|
import utils
|
|||
|
|
import toml
|
|||
|
|
import zmq
|
|||
|
|
import time
|
|||
|
|
import variable as var
|
|||
|
|
import action as act
|
|||
|
|
import re
|
|||
|
|
import threading
|
|||
|
|
import ctypes
|
|||
|
|
cfg = None
|
|||
|
|
cfg_args = None
|
|||
|
|
by_cmd = None
|
|||
|
|
filter = None
|
|||
|
|
llm_bot = None
|
|||
|
|
|
|||
|
|
# 目标检测 socket 客户端
|
|||
|
|
context = None
|
|||
|
|
socket = None
|
|||
|
|
|
|||
|
|
context1 = None
|
|||
|
|
ocr_socket = None
|
|||
|
|
|
|||
|
|
global_skip_queue = None
|
|||
|
|
|
|||
|
|
'''
|
|||
|
|
description: main.py 里执行 引入全局变量
|
|||
|
|
param {*} _by_cmd 控制器对象
|
|||
|
|
return {*}
|
|||
|
|
'''
|
|||
|
|
def import_obj(_by_cmd, skip_queue):
|
|||
|
|
|
|||
|
|
global by_cmd
|
|||
|
|
global filter
|
|||
|
|
global llm_bot
|
|||
|
|
|
|||
|
|
global context
|
|||
|
|
global socket
|
|||
|
|
|
|||
|
|
global context1
|
|||
|
|
global ocr_socket
|
|||
|
|
|
|||
|
|
global cfg
|
|||
|
|
global cfg_args
|
|||
|
|
global global_skip_queue
|
|||
|
|
cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
|
|||
|
|
cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.toml')
|
|||
|
|
by_cmd = _by_cmd
|
|||
|
|
global_skip_queue = skip_queue
|
|||
|
|
|
|||
|
|
# 目标检测 socket 客户端
|
|||
|
|
context = zmq.Context()
|
|||
|
|
socket = context.socket(zmq.REQ)
|
|||
|
|
socket.connect("tcp://localhost:6667")
|
|||
|
|
logger.info("subtask yolo client init")
|
|||
|
|
|
|||
|
|
# ocr socket 客户端
|
|||
|
|
# context1 = zmq.Context()
|
|||
|
|
# ocr_socket = context1.socket(zmq.REQ)
|
|||
|
|
# ocr_socket.connect("tcp://localhost:6668")
|
|||
|
|
# logger.info("subtask ocr client init")
|
|||
|
|
|
|||
|
|
filter = label_filter(socket)
|
|||
|
|
if cfg['move_area']['llm_enable']:
|
|||
|
|
llm_bot = LLM()
|
|||
|
|
def car_stop():
|
|||
|
|
for _ in range(3):
|
|||
|
|
by_cmd.send_speed_x(0)
|
|||
|
|
time.sleep(0.2)
|
|||
|
|
by_cmd.send_speed_y(0)
|
|||
|
|
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("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
|
|||
|
|
break
|
|||
|
|
ret, error = filter.aim_right(label)
|
|||
|
|
|
|||
|
|
error += offset
|
|||
|
|
if abs(error) > 10 and run:
|
|||
|
|
if error > 0:
|
|||
|
|
by_cmd.send_speed_x(-run_speed)
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_speed_x(run_speed)
|
|||
|
|
# 停的位置已经很接近目标，可以直接使用 distance 校准
|
|||
|
|
else:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
while True:
|
|||
|
|
ret, error = filter.aim_right(label)
|
|||
|
|
while not ret:
|
|||
|
|
ret, error = filter.aim_right(label)
|
|||
|
|
error += offset
|
|||
|
|
# if ret:
|
|||
|
|
if abs(error) <= 8:
|
|||
|
|
car_stop()
|
|||
|
|
logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
|
|||
|
|
ret, error = filter.aim_right(label)
|
|||
|
|
while not ret:
|
|||
|
|
not_found_counts += 1
|
|||
|
|
if not_found_counts >=30:
|
|||
|
|
return
|
|||
|
|
ret, error = filter.aim_right(label)
|
|||
|
|
error += offset
|
|||
|
|
logger.info(f"calibrate_right_new:停车后的误差是{error}")
|
|||
|
|
if abs(error) > 8:
|
|||
|
|
logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
|
|||
|
|
error = error * 3
|
|||
|
|
if error > 0:
|
|||
|
|
by_cmd.send_distance_x(-10, int(error))
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_distance_x(10, int(-error))
|
|||
|
|
|
|||
|
|
break
|
|||
|
|
'''
|
|||
|
|
description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车
|
|||
|
|
如果停车后 error > 8 则使用 distance 校准
|
|||
|
|
这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准
|
|||
|
|
param {*} label
|
|||
|
|
param {*} offset
|
|||
|
|
param {*} run
|
|||
|
|
param {*} run_speed
|
|||
|
|
return {*}
|
|||
|
|
'''
|
|||
|
|
def calibrate_new(label, offset, run = True, run_speed = 3.5):
|
|||
|
|
not_found_counts = 0
|
|||
|
|
ret, box = filter.get(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("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
|
|||
|
|
break
|
|||
|
|
ret, box = filter.get(label)
|
|||
|
|
|
|||
|
|
# 如果超过二十帧跳出，则此时 box 为空值，需要再校验 ret
|
|||
|
|
if ret is True:
|
|||
|
|
error = (box[0][2] + box[0][0] - 320) / 2 + offset
|
|||
|
|
if abs(error) > 10 and run:
|
|||
|
|
if error > 0:
|
|||
|
|
by_cmd.send_speed_x(-run_speed)
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_speed_x(run_speed)
|
|||
|
|
# 停的位置已经很接近目标，可以直接使用 distance 校准
|
|||
|
|
else:
|
|||
|
|
if abs(error) > 8:
|
|||
|
|
logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
|
|||
|
|
# error = error # 3
|
|||
|
|
if error > 0:
|
|||
|
|
by_cmd.send_distance_x(-10, int(error))
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_distance_x(10, int(-error))
|
|||
|
|
return
|
|||
|
|
logger.info(f"calibrate_new:停车后误差{error}小于 8 不校准")
|
|||
|
|
return
|
|||
|
|
while True:
|
|||
|
|
ret, box = filter.get(label)
|
|||
|
|
while not ret:
|
|||
|
|
ret, box = filter.get(label)
|
|||
|
|
|
|||
|
|
error = (box[0][2] + box[0][0] - 320) / 2 + offset
|
|||
|
|
if ret:
|
|||
|
|
if abs(error) <= 10: # 5
|
|||
|
|
car_stop()
|
|||
|
|
logger.info("calibrate_new:行进时 误差小于 10 直接停车")
|
|||
|
|
|
|||
|
|
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"calibrate_new:停车后的误差是{error}")
|
|||
|
|
if abs(error) > 8:
|
|||
|
|
logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
|
|||
|
|
error = error * 3
|
|||
|
|
# logger.error(f"error * 3 {error}")
|
|||
|
|
if error > 0:
|
|||
|
|
by_cmd.send_distance_x(-10, int(error))
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_distance_x(10, int(-error))
|
|||
|
|
break
|
|||
|
|
# 弃用 distance 校准
|
|||
|
|
def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
|
|||
|
|
# run_direc == 1 向前
|
|||
|
|
stop_error = 0
|
|||
|
|
if run_direc == 1:
|
|||
|
|
by_cmd.send_speed_x(run_speed)
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_speed_x(-run_speed)
|
|||
|
|
|
|||
|
|
if label == tlabel.TPLATFORM:
|
|||
|
|
stop_error = 8
|
|||
|
|
else:
|
|||
|
|
stop_error = 15
|
|||
|
|
while True:
|
|||
|
|
ret, box = filter.get(label)
|
|||
|
|
while not ret:
|
|||
|
|
if not global_skip_queue.empty():
|
|||
|
|
_ = global_skip_queue.get()
|
|||
|
|
logger.error("跳过 explore_calibrate_new")
|
|||
|
|
return False
|
|||
|
|
ret, box = filter.get(label)
|
|||
|
|
|
|||
|
|
error = (box[0][2] + box[0][0] - 320) / 2 + offset
|
|||
|
|
if ret:
|
|||
|
|
# 校准速度越大 停车的条件越宽泛 20 15
|
|||
|
|
if abs(error) <= stop_error:
|
|||
|
|
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
|
|||
|
|
logger.info(f"停车后像素误差:{error}")
|
|||
|
|
|
|||
|
|
# logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
|
|||
|
|
# if abs(error) < 8:
|
|||
|
|
# error = error * 3
|
|||
|
|
# else:
|
|||
|
|
# error = error * 2
|
|||
|
|
# if error > 0:
|
|||
|
|
# by_cmd.send_distance_x(-10, int(error))
|
|||
|
|
# else:
|
|||
|
|
# by_cmd.send_distance_x(10, int(-error))
|
|||
|
|
|
|||
|
|
break
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
# 任务类
|
|||
|
|
class task:
|
|||
|
|
def __init__(self, name, task_template, find_counts=10, enable=True):
|
|||
|
|
self.enable = enable
|
|||
|
|
self.task_t = task_template()
|
|||
|
|
|
|||
|
|
self.counts = 0
|
|||
|
|
self.find_counts = int(find_counts)
|
|||
|
|
self.name = name
|
|||
|
|
|
|||
|
|
def init(self):
|
|||
|
|
if hasattr(self.task_t, 'init') and callable(getattr(self.task_t, 'init', None)):
|
|||
|
|
self.task_t.init()
|
|||
|
|
else:
|
|||
|
|
logger.warning("[Task ]# 该任务没有 init 方法")
|
|||
|
|
def find(self):
|
|||
|
|
if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)):
|
|||
|
|
return self.task_t.find()
|
|||
|
|
else:
|
|||
|
|
logger.warning("[Task ]# 该任务没有 find 方法")
|
|||
|
|
def exec(self):
|
|||
|
|
# 根据标志位确定是否执行该任务
|
|||
|
|
if self.enable is True:
|
|||
|
|
if hasattr(self.task_t, 'exec') and callable(getattr(self.task_t, 'exec', None)):
|
|||
|
|
logger.info(f"[Task ]# Executing task")
|
|||
|
|
self.task_t.exec()
|
|||
|
|
else:
|
|||
|
|
logger.warning("[Task ]# 该任务没有 exec 方法")
|
|||
|
|
else:
|
|||
|
|
logger.warning(f"[Task ]# Skip task")
|
|||
|
|
if hasattr(self.task_t, 'nexec') and callable(getattr(self.task_t, 'nexec', None)):
|
|||
|
|
self.task_t.nexec()
|
|||
|
|
else:
|
|||
|
|
logger.warning("[Task ]# 该任务没有 nexec 方法")
|
|||
|
|
def after(self):
|
|||
|
|
if hasattr(self.task_t, 'after') and callable(getattr(self.task_t, 'after', None)):
|
|||
|
|
logger.info(f"[Task ]# {self.name} 正在执行 after")
|
|||
|
|
self.task_t.after()
|
|||
|
|
logger.debug(f"[Task ]# Task completed")
|
|||
|
|
else:
|
|||
|
|
logger.warning("[Task ]# 该任务没有 after 方法")
|
|||
|
|
|
|||
|
|
# 任务队列状态类
|
|||
|
|
class task_queuem_status(Enum):
|
|||
|
|
IDEL = 0
|
|||
|
|
SEARCHING = 1
|
|||
|
|
EXECUTING = 2
|
|||
|
|
|
|||
|
|
# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
|
|||
|
|
class task_queuem(task):
|
|||
|
|
# task_now = task(None, False)
|
|||
|
|
def __init__(self, queue):
|
|||
|
|
super(task_queuem, self)
|
|||
|
|
self.queue = queue
|
|||
|
|
self.status = task_queuem_status.IDEL
|
|||
|
|
self.busy = True
|
|||
|
|
logger.info(f"[TaskM]# Task num {self.queue.qsize()}")
|
|||
|
|
# exec 线程
|
|||
|
|
self.exec_thread = None
|
|||
|
|
def exec(self, skip_queue):
|
|||
|
|
# 如果空闲状态则将下一个队列任务取出
|
|||
|
|
if self.status is task_queuem_status.IDEL:
|
|||
|
|
if self.queue.qsize() == 0:
|
|||
|
|
self.busy = False
|
|||
|
|
logger.info(f"[TaskM]# Task queue empty, exit")
|
|||
|
|
return False
|
|||
|
|
self.task_now = self.queue.get()
|
|||
|
|
|
|||
|
|
# 如果当前任务没有使能，则直接转入执行状态，由任务执行函数打印未执行信息
|
|||
|
|
if self.task_now.enable is True:
|
|||
|
|
self.status = task_queuem_status.SEARCHING
|
|||
|
|
# 如果使能该任务则执行该任务的初始化动作
|
|||
|
|
self.task_now.init()
|
|||
|
|
else:
|
|||
|
|
self.status = task_queuem_status.EXECUTING
|
|||
|
|
logger.info(f"[TaskM]# ---------------------->>>>")
|
|||
|
|
# 阻塞搜索任务标志位
|
|||
|
|
elif self.status is task_queuem_status.SEARCHING:
|
|||
|
|
logger.info(f"[{self.task_now.name}]# Start searching task target")
|
|||
|
|
while True:
|
|||
|
|
if not skip_queue.empty():
|
|||
|
|
_ = skip_queue.get()
|
|||
|
|
logger.error(f"{self.task_now.name} 任务在 find 中已经被手动跳过")
|
|||
|
|
self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
|
|||
|
|
self.task_now.after() # 执行任务后处理
|
|||
|
|
self.queue.task_done() # 弹出已执行的任务
|
|||
|
|
return True
|
|||
|
|
ret = self.task_now.find()
|
|||
|
|
self.task_now.counts += ret
|
|||
|
|
if self.task_now.counts >= self.task_now.find_counts:
|
|||
|
|
self.status = task_queuem_status.EXECUTING
|
|||
|
|
break
|
|||
|
|
# 执行任务函数
|
|||
|
|
elif self.status is task_queuem_status.EXECUTING:
|
|||
|
|
if self.task_now.enable is True:
|
|||
|
|
logger.info(f"[TaskM]# Start execute task function")
|
|||
|
|
# self.exec_thread = threading.Thread(target=self.task_now.exec)
|
|||
|
|
# # 启动线程
|
|||
|
|
# self.exec_thread.start()
|
|||
|
|
# while True:
|
|||
|
|
# if not self.exec_thread.is_alive():
|
|||
|
|
# break
|
|||
|
|
# else:
|
|||
|
|
# if not skip_queue.empty():
|
|||
|
|
# car_stop()
|
|||
|
|
# thread_id = self.exec_thread.ident
|
|||
|
|
# res = ctypes.pythonapi.PyThreadState_SetAsyncExc(thread_id, ctypes.py_object(SystemExit))
|
|||
|
|
# _ = skip_queue.get()
|
|||
|
|
# logger.error(f"{self.task_now.name} 任务在 exec 中已经被手动跳过")
|
|||
|
|
# self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
|
|||
|
|
# self.task_now.after() # 执行任务后处理
|
|||
|
|
# self.queue.task_done() # 弹出已执行的任务
|
|||
|
|
# return True
|
|||
|
|
self.task_now.exec() # 执行当前任务函数
|
|||
|
|
self.status = task_queuem_status.IDEL # 执行完成后为退出巡线阻塞
|
|||
|
|
self.task_now.after() # 执行任务后处理
|
|||
|
|
self.queue.task_done() # 弹出已执行的任务
|
|||
|
|
logger.info(f"[TaskM]# <<<<----------------------")
|
|||
|
|
else:
|
|||
|
|
logger.info(f"[TaskM]# Start execute task function (nexec)")
|
|||
|
|
self.status = task_queuem_status.IDEL # 空动作不需要阻塞巡线，直接置位
|
|||
|
|
self.task_now.exec() # 执行当前任务函数
|
|||
|
|
self.task_now.after() # 执行任务后处理
|
|||
|
|
self.queue.task_done() # 弹出已执行的任务
|
|||
|
|
logger.info(f"[TaskM]# <<<<----------------------")
|
|||
|
|
return True
|
|||
|
|
|
|||
|
|
# 人员施救
|
|||
|
|
class get_block1():
|
|||
|
|
def init(self):
|
|||
|
|
var.task_speed = 15
|
|||
|
|
act.cmd.camera(0)
|
|||
|
|
act.cmd.z2(20, 60, 0)
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
# if cfg['get_block']['first_block'] == "blue":
|
|||
|
|
# self.target_label = tlabel.BBLOCK
|
|||
|
|
# self.another_label = tlabel.RBLOCK
|
|||
|
|
# else:
|
|||
|
|
# self.target_label = tlabel.RBLOCK
|
|||
|
|
# self.another_label = tlabel.BBLOCK
|
|||
|
|
# cfg['get_block']['first_block'] = "red"
|
|||
|
|
self.target_label = [tlabel.RBLOCK, tlabel.BBLOCK]
|
|||
|
|
self.target_counts = [0, 0]
|
|||
|
|
def find(self):
|
|||
|
|
# 目标检测红/蓝方块
|
|||
|
|
# ret = filter.find(self.target_label)
|
|||
|
|
# if ret > 0:
|
|||
|
|
# return True
|
|||
|
|
# return False
|
|||
|
|
ret = filter.find_mult(self.target_label)
|
|||
|
|
self.target_counts[0] += ret[0]
|
|||
|
|
self.target_counts[1] += ret[1]
|
|||
|
|
if any(ret):
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
|
|||
|
|
|
|||
|
|
def exec(self):
|
|||
|
|
car_stop()
|
|||
|
|
if self.target_counts[0] > self.target_counts[1]:
|
|||
|
|
var.first_block = tlabel.RBLOCK
|
|||
|
|
var.second_block = tlabel.BBLOCK
|
|||
|
|
else:
|
|||
|
|
var.first_block = tlabel.BBLOCK
|
|||
|
|
var.second_block = tlabel.RBLOCK
|
|||
|
|
calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5)
|
|||
|
|
logger.info("抓取块")
|
|||
|
|
|
|||
|
|
by_cmd.send_position_axis_z(30, 60)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
by_cmd.send_position_axis_x(1, 20)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(25)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_z(30, 90)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
by_cmd.send_angle_claw_arm(175)
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
by_cmd.send_position_axis_x(1, 120)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_z(30, 70)
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
by_cmd.send_position_axis_z(30, 130)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_x(1, 140)
|
|||
|
|
by_cmd.send_angle_claw_arm(220)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_storage(55)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
# TODO 完成不执行任务的空动作
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
class get_block2():
|
|||
|
|
def init(self):
|
|||
|
|
while (by_cmd.send_angle_camera(0) == -1):
|
|||
|
|
by_cmd.send_angle_camera(0)
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
# if cfg['get_block']['first_block'] == "red":
|
|||
|
|
# self.target_label = tlabel.BBLOCK
|
|||
|
|
# self.another_label = tlabel.RBLOCK
|
|||
|
|
# else:
|
|||
|
|
# self.target_label = tlabel.RBLOCK
|
|||
|
|
# self.another_label = tlabel.BBLOCK
|
|||
|
|
def find(self):
|
|||
|
|
# 目标检测红/蓝方块
|
|||
|
|
ret = filter.find(var.second_block)
|
|||
|
|
if ret > 0:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
car_stop()
|
|||
|
|
calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5)
|
|||
|
|
logger.info("抓取块")
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw_arm(220)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
by_cmd.send_position_axis_z(30, 60)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_x(1, 20)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(25)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 100)
|
|||
|
|
|
|||
|
|
# by_cmd.send_distance_x(15, 100)
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
# TODO 完成不执行任务的空动作
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.task_speed = 0
|
|||
|
|
var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
|
|||
|
|
# 任务检查间隔
|
|||
|
|
time.sleep(7)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 紧急转移
|
|||
|
|
class put_block():
|
|||
|
|
def init(self):
|
|||
|
|
var.task_speed = 0
|
|||
|
|
while (by_cmd.send_angle_camera(0) == -1):
|
|||
|
|
by_cmd.send_angle_camera(0)
|
|||
|
|
logger.info("紧急转移初始化")
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
def find(self):
|
|||
|
|
# 目标检测医院
|
|||
|
|
ret, box = filter.get(tlabel.HOSPITAL)
|
|||
|
|
if ret > 0:
|
|||
|
|
width = box[0][2] - box[0][0]
|
|||
|
|
if width > 130:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到医院")
|
|||
|
|
car_stop()
|
|||
|
|
calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
|
|||
|
|
|
|||
|
|
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, 20)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
# 放置第二個塊
|
|||
|
|
by_cmd.send_angle_storage(20)
|
|||
|
|
by_cmd.send_position_axis_x(1, 130)
|
|||
|
|
by_cmd.send_position_axis_z(30, 120)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
by_cmd.send_angle_claw_arm(180)
|
|||
|
|
by_cmd.send_angle_claw(85)
|
|||
|
|
# by_cmd.send_angle_storage(0)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_z(30,70)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(25)
|
|||
|
|
by_cmd.send_distance_x(-10, 110)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_z(30, 110)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw_arm(220)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_z(30, 0)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 20)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
by_cmd.send_angle_claw(45)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
|
|||
|
|
# 下一动作预备位置
|
|||
|
|
while by_cmd.send_position_axis_z(30, 130) == -1:
|
|||
|
|
pass
|
|||
|
|
time.sleep(1)
|
|||
|
|
while by_cmd.send_position_axis_x(1, 0) == -1:
|
|||
|
|
pass
|
|||
|
|
while by_cmd.send_angle_claw_arm(36) == -1:
|
|||
|
|
pass
|
|||
|
|
# 任务检查间隔
|
|||
|
|
# time.sleep(2)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 整装上阵
|
|||
|
|
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(30, 135)
|
|||
|
|
# time.sleep(0.5)
|
|||
|
|
# by_cmd.send_position_axis_x(1, 0)
|
|||
|
|
# time.sleep(2)
|
|||
|
|
# by_cmd.send_angle_claw_arm(36)
|
|||
|
|
|
|||
|
|
while (by_cmd.send_angle_storage(0) == -1):
|
|||
|
|
by_cmd.send_angle_storage(0)
|
|||
|
|
|
|||
|
|
# 调试 临时换源
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
logger.info("整装上阵初始化")
|
|||
|
|
# time.sleep(0.5)
|
|||
|
|
|
|||
|
|
self.record = CountRecord(5)
|
|||
|
|
def find(self):
|
|||
|
|
# 目标检测蓝球
|
|||
|
|
ret = filter.find(tlabel.BBALL)
|
|||
|
|
# ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
|
|||
|
|
# TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
|
|||
|
|
# if ret:
|
|||
|
|
# if self.record(tlabel.BBALL):
|
|||
|
|
# return True
|
|||
|
|
if ret:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到蓝色球")
|
|||
|
|
car_stop()
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
for _ in range(3):
|
|||
|
|
calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5)
|
|||
|
|
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, 160)
|
|||
|
|
time.sleep(1.2)
|
|||
|
|
by_cmd.send_angle_claw(25)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 20)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_x(1, 60)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -40)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw_arm(80)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(54)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw_arm(36)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_position_axis_z(30, 135)
|
|||
|
|
# 继续向前走
|
|||
|
|
# by_cmd.send_speed_x(4)
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
|
|||
|
|
# 下一动作预备位置
|
|||
|
|
by_cmd.send_angle_claw(30)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
while by_cmd.send_position_axis_z(30, 0) == -1:
|
|||
|
|
pass
|
|||
|
|
time.sleep(1)
|
|||
|
|
# # 任务检查间隔
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 通信抢修
|
|||
|
|
class up_tower():
|
|||
|
|
def init(self):
|
|||
|
|
logger.info("通信抢修初始化")
|
|||
|
|
while (by_cmd.send_angle_camera(90) == -1):
|
|||
|
|
by_cmd.send_angle_camera(90)
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
def find(self):
|
|||
|
|
# 目标检测通信塔
|
|||
|
|
ret = filter.find(tlabel.TOWER)
|
|||
|
|
if ret:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到塔")
|
|||
|
|
car_stop()
|
|||
|
|
time.sleep(1)
|
|||
|
|
calibrate_new(tlabel.TOWER, offset = 20, run = True)
|
|||
|
|
time.sleep(1)
|
|||
|
|
# calibrate(tlabel.TOWER, 27, False, 6)
|
|||
|
|
by_cmd.send_distance_x(-10, 120)
|
|||
|
|
time.sleep(1)
|
|||
|
|
# 上古參數
|
|||
|
|
# by_cmd.send_distance_y(-10, 50)
|
|||
|
|
# 6_9 模型參數
|
|||
|
|
by_cmd.send_distance_y(-10, 40)
|
|||
|
|
# 7_12_3 模型參數
|
|||
|
|
# by_cmd.send_distance_y(-10, 50)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
car_stop()
|
|||
|
|
# FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
|
|||
|
|
# time.sleep(3)
|
|||
|
|
# by_cmd.send_speed_y(-10)
|
|||
|
|
# time.sleep(0.15)
|
|||
|
|
|
|||
|
|
by_cmd.send_angle_zhuan(10)
|
|||
|
|
time.sleep(12)
|
|||
|
|
by_cmd.send_speed_y(10)
|
|||
|
|
time.sleep(0.3)
|
|||
|
|
car_stop()
|
|||
|
|
by_cmd.send_angle_zhuan(0)
|
|||
|
|
# while True:
|
|||
|
|
# pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
|
|||
|
|
# 下一动作预备位置
|
|||
|
|
by_cmd.send_position_axis_z(30, 0)
|
|||
|
|
# 任务检查间隔
|
|||
|
|
time.sleep(4)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 高空排险
|
|||
|
|
class get_rball():
|
|||
|
|
def init(self):
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
logger.info("高空排险初始化")
|
|||
|
|
while (by_cmd.send_angle_camera(180) == -1):
|
|||
|
|
by_cmd.send_angle_camera(180)
|
|||
|
|
self.record = CountRecord(3)
|
|||
|
|
def find(self):
|
|||
|
|
# 目标检测红球
|
|||
|
|
ret = filter.find(tlabel.RBALL)
|
|||
|
|
if ret > 0:
|
|||
|
|
# TODO 连续两帧才开始减速
|
|||
|
|
if self.record(tlabel.RBALL):
|
|||
|
|
var.task_speed = 5
|
|||
|
|
return True
|
|||
|
|
else:
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到红球")
|
|||
|
|
var.task_speed = 0
|
|||
|
|
car_stop()
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
# 靠近塔
|
|||
|
|
by_cmd.send_angle_scoop(20)
|
|||
|
|
# 上古參數
|
|||
|
|
# by_cmd.send_distance_y(-15, 45) # 50
|
|||
|
|
# 6_9 參數
|
|||
|
|
by_cmd.send_distance_y(-15, 35)
|
|||
|
|
time.sleep(2)
|
|||
|
|
# 7_12_3 參數
|
|||
|
|
# by_cmd.send_distance_y(-15, 45)
|
|||
|
|
# time.sleep(2)
|
|||
|
|
car_stop()
|
|||
|
|
calibrate_new(tlabel.RBALL,offset = 44, run = True)
|
|||
|
|
time.sleep(1)
|
|||
|
|
logger.info("抓红球")
|
|||
|
|
# by_cmd.send_angle_scoop(15)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_z(30, 170)
|
|||
|
|
time.sleep(2.5)
|
|||
|
|
by_cmd.send_angle_scoop(7)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_speed_y(15)
|
|||
|
|
time.sleep(0.2)
|
|||
|
|
car_stop()
|
|||
|
|
# by_cmd.send_angle_omega(-55,30)
|
|||
|
|
# while True:
|
|||
|
|
# pass
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["get_rball"]["pid_kp"], cfg["get_rball"]["pid_ki"], cfg["get_rball"]["pid_kd"])
|
|||
|
|
# 任务检查间隔
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 派发物资
|
|||
|
|
class put_bball():
|
|||
|
|
def init(self):
|
|||
|
|
logger.info("派发物资初始化")
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
while by_cmd.send_position_axis_z(30, 0) == -1:
|
|||
|
|
pass
|
|||
|
|
while by_cmd.send_angle_camera(90) == -1:
|
|||
|
|
pass
|
|||
|
|
def find(self):
|
|||
|
|
ret = filter.find(tlabel.BASKET)
|
|||
|
|
if ret > 0:
|
|||
|
|
return True
|
|||
|
|
else:
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到篮筐")
|
|||
|
|
car_stop()
|
|||
|
|
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_angle_storage(10)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
by_cmd.send_angle_storage(50)
|
|||
|
|
logger.info("把球放篮筐里")
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
# by_cmd.send_distance_y(10, 55)
|
|||
|
|
by_cmd.send_angle_storage(20)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
# car_stop()
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
|
|||
|
|
# 任务检查间隔
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 物资盘点
|
|||
|
|
class put_hanoi1():
|
|||
|
|
def init(self):
|
|||
|
|
logger.info("物资盘点 1 初始化")
|
|||
|
|
filter.switch_camera(2)
|
|||
|
|
def find(self):
|
|||
|
|
ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
|
|||
|
|
if label == tlabel.RMARK:
|
|||
|
|
if abs(error) < 55:
|
|||
|
|
logger.info("向右拐")
|
|||
|
|
utils.direction_right += 1
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
elif label == tlabel.LMARK:
|
|||
|
|
if abs(error) < 50:
|
|||
|
|
logger.info("向左拐")
|
|||
|
|
utils.direction_left += 1
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
else:
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
global direction
|
|||
|
|
for _ in range(3):
|
|||
|
|
by_cmd.send_speed_x(0)
|
|||
|
|
time.sleep(0.2)
|
|||
|
|
by_cmd.send_speed_omega(0)
|
|||
|
|
time.sleep(0.2)
|
|||
|
|
|
|||
|
|
by_cmd.send_position_axis_z(30, 130)
|
|||
|
|
|
|||
|
|
# 校准牌子
|
|||
|
|
if utils.direction_right > utils.direction_left:
|
|||
|
|
ret, error = filter.aim_near(tlabel.RMARK)
|
|||
|
|
while not ret:
|
|||
|
|
ret, error = filter.aim_near(tlabel.RMARK)
|
|||
|
|
utils.direction = tlabel.RMARK
|
|||
|
|
logger.info("应该向右转")
|
|||
|
|
else:
|
|||
|
|
ret, error = filter.aim_near(tlabel.LMARK)
|
|||
|
|
while not ret:
|
|||
|
|
ret, error = filter.aim_near(tlabel.LMARK)
|
|||
|
|
utils.direction = tlabel.LMARK
|
|||
|
|
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, 250)
|
|||
|
|
time.sleep(1)
|
|||
|
|
car_stop()
|
|||
|
|
|
|||
|
|
# 根据方向初始化执行器位置
|
|||
|
|
if utils.direction is tlabel.RMARK:
|
|||
|
|
# FIXME 右侧的爪子会被 storage 挡住
|
|||
|
|
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(220)
|
|||
|
|
by_cmd.send_angle_storage(55)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
by_cmd.send_position_axis_z(30, 10)
|
|||
|
|
|
|||
|
|
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)
|
|||
|
|
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)
|
|||
|
|
time.sleep(2)
|
|||
|
|
while (by_cmd.send_angle_camera(0) == -1):
|
|||
|
|
by_cmd.send_angle_camera(0)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
filter.switch_camera(1)
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
class put_hanoi2():
|
|||
|
|
def __init__(self):
|
|||
|
|
if cfg['put_hanoi2']['first_target'] == "lp":
|
|||
|
|
self.target_label = tlabel.LPILLER
|
|||
|
|
elif cfg['put_hanoi2']['first_target'] == "mp":
|
|||
|
|
self.target_label = tlabel.MPILLER
|
|||
|
|
elif cfg['put_hanoi2']['first_target'] == "sp":
|
|||
|
|
self.target_label = tlabel.SPILLER
|
|||
|
|
def init(self):
|
|||
|
|
logger.info("物资盘点 2 初始化")
|
|||
|
|
if utils.direction == tlabel.RMARK:
|
|||
|
|
# 15
|
|||
|
|
self.offset = 19
|
|||
|
|
# self.platform_offset = -25
|
|||
|
|
self.platform_offset = -10
|
|||
|
|
else:
|
|||
|
|
self.offset = 10
|
|||
|
|
#self.platform_offset = -30
|
|||
|
|
self.platform_offset = -15
|
|||
|
|
def find(self):
|
|||
|
|
# ret, box = filter.get(self.target_label)
|
|||
|
|
ret, box = filter.get(tlabel.TPLATFORM)
|
|||
|
|
if ret:
|
|||
|
|
var.task_speed = 8.5
|
|||
|
|
error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
|
|||
|
|
if error > 0:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info(f"direction:{utils.direction.name}")
|
|||
|
|
var.task_speed = 0
|
|||
|
|
car_stop()
|
|||
|
|
|
|||
|
|
# if utils.direction is tlabel.RMARK:
|
|||
|
|
# by_cmd.send_distance_y(10, 50)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
# time.sleep(0.5)
|
|||
|
|
# by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
|
|||
|
|
if not ret:
|
|||
|
|
logger.error("跳过物资盘点 2 exec")
|
|||
|
|
return
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
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_angle_claw(63)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_angle_claw(40)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 10)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_position_axis_z(30, 10)
|
|||
|
|
by_cmd.send_position_axis_x(1, 40)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_angle_claw(40)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
ret = explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
|
|||
|
|
if not ret:
|
|||
|
|
logger.error("跳过物资盘点 2 exec")
|
|||
|
|
return
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
logger.info("放大平台")
|
|||
|
|
if utils.direction is tlabel.RMARK:
|
|||
|
|
by_cmd.send_position_axis_x(1, 130)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -20)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(81)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 10)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_position_axis_x(1, 40)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -20)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw(81)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
|
|||
|
|
if not ret:
|
|||
|
|
logger.error("跳过物资盘点 2 exec")
|
|||
|
|
return
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
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_angle_claw(63)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_angle_claw(35)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 10)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_position_axis_z(30, 10)
|
|||
|
|
by_cmd.send_position_axis_x(1, 40)
|
|||
|
|
by_cmd.send_angle_claw(63)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_angle_claw(35)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
|
|||
|
|
if not ret:
|
|||
|
|
logger.error("跳过物资盘点 2 exec")
|
|||
|
|
return
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
logger.info("放中平台")
|
|||
|
|
if utils.direction is tlabel.RMARK:
|
|||
|
|
by_cmd.send_position_axis_z(30, 100)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_position_axis_x(1, 130)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(65)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 10)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_position_axis_z(30, 100)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_position_axis_x(1, 40)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(65)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
ret = explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
|
|||
|
|
if not ret:
|
|||
|
|
logger.error("跳过物资盘点 2 exec")
|
|||
|
|
return
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
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_angle_claw(50)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_angle_claw(27)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 10)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 10)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_position_axis_z(30, 10)
|
|||
|
|
by_cmd.send_position_axis_x(1, 40)
|
|||
|
|
by_cmd.send_angle_claw(50)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_angle_claw(27)
|
|||
|
|
time.sleep(1)
|
|||
|
|
by_cmd.send_distance_axis_z(30, 10)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
time.sleep(1)
|
|||
|
|
pass
|
|||
|
|
ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
|
|||
|
|
if not ret:
|
|||
|
|
logger.error("跳过物资盘点 2 exec")
|
|||
|
|
return
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
logger.info("放小平台")
|
|||
|
|
if utils.direction is tlabel.RMARK:
|
|||
|
|
by_cmd.send_position_axis_z(30, 170)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 130)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(80)
|
|||
|
|
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)
|
|||
|
|
car_stop()
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_position_axis_z(30, 170)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 40)
|
|||
|
|
time.sleep(2)
|
|||
|
|
by_cmd.send_distance_axis_z(30, -20)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_angle_claw(80)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
time.sleep(1.5)
|
|||
|
|
# while True:
|
|||
|
|
# pass
|
|||
|
|
|
|||
|
|
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"])
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
class put_hanoi3():
|
|||
|
|
def init(self):
|
|||
|
|
while by_cmd.send_position_axis_z(30, 130) == -1:
|
|||
|
|
pass
|
|||
|
|
time.sleep(3)
|
|||
|
|
logger.info("完成任务，爪回左侧")
|
|||
|
|
while by_cmd.send_angle_claw_arm(128) == -1:
|
|||
|
|
pass
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
while by_cmd.send_position_axis_x(1, 0) == -1:
|
|||
|
|
pass
|
|||
|
|
time.sleep(1)
|
|||
|
|
# while by_cmd.send_angle_claw_arm(220) == -1:
|
|||
|
|
# pass
|
|||
|
|
# while by_cmd.send_angle_claw(90) == -1:
|
|||
|
|
# pass
|
|||
|
|
def find(self):
|
|||
|
|
time.sleep(1)
|
|||
|
|
return True
|
|||
|
|
def exec(self):
|
|||
|
|
while by_cmd.send_position_axis_z(30, 100) == -1:
|
|||
|
|
pass
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
by_cmd.send_position_axis_x(1, 150)
|
|||
|
|
var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
|
|||
|
|
|
|||
|
|
# 应急避险 第一阶段 找目标牌
|
|||
|
|
class move_area1():
|
|||
|
|
def init(self):
|
|||
|
|
logger.info("应急避险第一阶段初始化")
|
|||
|
|
while (by_cmd.send_angle_camera(0) == -1):
|
|||
|
|
by_cmd.send_angle_camera(0)
|
|||
|
|
def find(self):
|
|||
|
|
ret = filter.find(tlabel.SIGN)
|
|||
|
|
if ret:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到标示牌")
|
|||
|
|
# 停车 ocr 识别文字 调用大模型
|
|||
|
|
car_stop()
|
|||
|
|
time.sleep(1)
|
|||
|
|
# calibrate_new(tlabel.SIGN, offset = 8, run = True)
|
|||
|
|
calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
by_cmd.send_position_axis_x(1, 50)
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
# filter_w = (148, 560)
|
|||
|
|
# filter_h = (165, 390)
|
|||
|
|
|
|||
|
|
counts = 0
|
|||
|
|
while True:
|
|||
|
|
ocr_socket.send_string("")
|
|||
|
|
resp = ocr_socket.recv_pyobj()
|
|||
|
|
var.llm_text = ''
|
|||
|
|
counts += 1
|
|||
|
|
if resp.get('code') == 0:
|
|||
|
|
for item in resp.get('content'):
|
|||
|
|
|
|||
|
|
if item['probability']['average'] < 0.80:
|
|||
|
|
continue
|
|||
|
|
# box = item['location']
|
|||
|
|
# center_x = box['left'] + box['width'] / 2
|
|||
|
|
# center_y = box['top'] + box['height'] / 2
|
|||
|
|
# if center_x < filter_w[0] or center_x > filter_w[1] \
|
|||
|
|
# or center_y < filter_h[0] or center_y > filter_h[1]:
|
|||
|
|
# continue
|
|||
|
|
var.llm_text += item['words']
|
|||
|
|
break
|
|||
|
|
if counts >= 2:
|
|||
|
|
var.skip_llm_task_flag = True
|
|||
|
|
return
|
|||
|
|
logger.error(var.llm_text)
|
|||
|
|
if len(var.llm_text) < 3:
|
|||
|
|
var.skip_llm_task_flag = True
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
var.task_speed = 9 # 12
|
|||
|
|
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
# 任务检查间隔
|
|||
|
|
by_cmd.send_position_axis_x(1, 150)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
by_cmd.send_angle_claw_arm(220)
|
|||
|
|
|
|||
|
|
pass
|
|||
|
|
|
|||
|
|
# 应急避险 第二阶段 找停车区域
|
|||
|
|
class move_area2():
|
|||
|
|
def init(self):
|
|||
|
|
logger.info("应急避险第二阶段初始化")
|
|||
|
|
self.offset = 15
|
|||
|
|
self.delta_x = 0
|
|||
|
|
self.delta_y = 0
|
|||
|
|
self.delta_omage = 0
|
|||
|
|
def find(self):
|
|||
|
|
# time.sleep(0.001)
|
|||
|
|
if var.skip_llm_task_flag:
|
|||
|
|
return 5000
|
|||
|
|
ret, box = filter.get(tlabel.SHELTER)
|
|||
|
|
if ret:
|
|||
|
|
error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
|
|||
|
|
if abs(error) < 20:
|
|||
|
|
return 5000
|
|||
|
|
return False
|
|||
|
|
def sub_light(self, delay_time):
|
|||
|
|
by_cmd.send_light(1)
|
|||
|
|
time.sleep(delay_time)
|
|||
|
|
by_cmd.send_light(0)
|
|||
|
|
def sub_beep(self,delay_time):
|
|||
|
|
by_cmd.send_beep(1)
|
|||
|
|
time.sleep(delay_time)
|
|||
|
|
by_cmd.send_beep(0)
|
|||
|
|
def sub_move(self, x, y):
|
|||
|
|
# FIXME 如果同時有 xy，是否會造成 delay 不足
|
|||
|
|
self.delta_x += x
|
|||
|
|
self.delta_y += y
|
|||
|
|
|
|||
|
|
if x != 0:
|
|||
|
|
delay_time = int(abs(x) * 500)
|
|||
|
|
if x > 0:
|
|||
|
|
by_cmd.send_distance_x(15, delay_time)
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_distance_x(-15, delay_time)
|
|||
|
|
elif y != 0:
|
|||
|
|
delay_time = int(abs(y) * 500)
|
|||
|
|
if y > 0: # 向左
|
|||
|
|
by_cmd.send_distance_y(-15, delay_time)
|
|||
|
|
else:
|
|||
|
|
by_cmd.send_distance_y(15, delay_time)
|
|||
|
|
time.sleep(delay_time / 500)
|
|||
|
|
car_stop()
|
|||
|
|
pass
|
|||
|
|
def sub_turn(self, angle):
|
|||
|
|
self.delta_omage += angle
|
|||
|
|
delay_time = int(abs(angle) * 400 / 90)
|
|||
|
|
if angle < 0:
|
|||
|
|
# 左转
|
|||
|
|
by_cmd.send_angle_omega(+55, delay_time)
|
|||
|
|
else:
|
|||
|
|
# 右转
|
|||
|
|
by_cmd.send_angle_omega(-55, delay_time)
|
|||
|
|
time.sleep(delay_time / 300 * 1.5)
|
|||
|
|
def exec(self):
|
|||
|
|
var.task_speed = 0
|
|||
|
|
if var.skip_llm_task_flag:
|
|||
|
|
logger.error("ocr 识别出错 直接跳过改任务")
|
|||
|
|
return
|
|||
|
|
logger.info("开始寻找停车区域")
|
|||
|
|
car_stop()
|
|||
|
|
calibrate_new(tlabel.SHELTER, offset = 15, run = True)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
# 调用大模型 然后执行动作
|
|||
|
|
try:
|
|||
|
|
resp = llm_bot.get_command_json(var.llm_text)
|
|||
|
|
logger.info(resp)
|
|||
|
|
except:
|
|||
|
|
logger.error("大模型超时，跳过任务")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0])
|
|||
|
|
if len(resp_commands) == 0:
|
|||
|
|
return
|
|||
|
|
# 进入停车区域
|
|||
|
|
# by_cmd.send_speed_y(15)
|
|||
|
|
by_cmd.send_distance_y(25, 180)
|
|||
|
|
time.sleep(1)
|
|||
|
|
# time.sleep(1.25)
|
|||
|
|
car_stop()
|
|||
|
|
logger.info(resp_commands)
|
|||
|
|
for command in resp_commands:
|
|||
|
|
logger.info(command)
|
|||
|
|
if command['func'] == 'move':
|
|||
|
|
self.sub_move(float(command['x']), float(command['y']))
|
|||
|
|
elif command['func'] == 'light':
|
|||
|
|
self.sub_light(int(command['time']))
|
|||
|
|
elif command['func'] == 'beep':
|
|||
|
|
self.sub_beep(int(command['time']))
|
|||
|
|
elif command['func'] == 'turn':
|
|||
|
|
self.sub_turn(int(command['angle']))
|
|||
|
|
pass
|
|||
|
|
else:
|
|||
|
|
continue
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
except:
|
|||
|
|
pass
|
|||
|
|
time.sleep(1)
|
|||
|
|
# 回到原位
|
|||
|
|
|
|||
|
|
delay_time = int(abs(self.delta_omage) * 400 / 90)
|
|||
|
|
if int(abs(self.delta_omage)) == 360:
|
|||
|
|
delay_time = 0
|
|||
|
|
if self.delta_omage < 0:
|
|||
|
|
# 左转
|
|||
|
|
by_cmd.send_angle_omega(-55, delay_time)
|
|||
|
|
else:
|
|||
|
|
# 右转
|
|||
|
|
by_cmd.send_angle_omega(55, delay_time)
|
|||
|
|
time.sleep(delay_time / 300 * 1.5)
|
|||
|
|
if self.delta_y > 0:
|
|||
|
|
# 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
|
|||
|
|
delay_time = 180 - (self.delta_y * 500)
|
|||
|
|
else:
|
|||
|
|
delay_time = 180 + (abs(self.delta_y) * 500)
|
|||
|
|
# 离开停车区域
|
|||
|
|
by_cmd.send_distance_y(-25, delay_time)
|
|||
|
|
|
|||
|
|
time.sleep(delay_time * 5e-3)
|
|||
|
|
|
|||
|
|
car_stop()
|
|||
|
|
|
|||
|
|
# FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
|
|||
|
|
# by_cmd.send_distance_y(-15, 300)
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
logger.warning("正在跳過大模型任務")
|
|||
|
|
time.sleep(2)
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
|
|||
|
|
by_cmd.send_position_axis_z(30, 0)
|
|||
|
|
while by_cmd.send_angle_claw(90) == -1:
|
|||
|
|
pass
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
# 扫黑除暴
|
|||
|
|
class kick_ass():
|
|||
|
|
def init(self):
|
|||
|
|
while (by_cmd.send_angle_camera(0) == -1):
|
|||
|
|
by_cmd.send_angle_camera(0)
|
|||
|
|
logger.info("扫黑除暴初始化")
|
|||
|
|
self.pos_gap1 = cfg['kick_ass']['pos_gap1']
|
|||
|
|
self.pos_gap2 = cfg['kick_ass']['pos_gap2']
|
|||
|
|
self.target_person = cfg_args['kick_ass']['target_person']
|
|||
|
|
|
|||
|
|
# by_cmd.send_angle_claw(15)
|
|||
|
|
by_cmd.send_position_axis_x(1, 160)
|
|||
|
|
|
|||
|
|
def find(self):
|
|||
|
|
ret = filter.find(tlabel.SIGN)
|
|||
|
|
if ret:
|
|||
|
|
return True
|
|||
|
|
return False
|
|||
|
|
def exec(self):
|
|||
|
|
logger.info("找到标示牌")
|
|||
|
|
# 停的晚无需校准 omage
|
|||
|
|
car_stop()
|
|||
|
|
time.sleep(1)
|
|||
|
|
calibrate_new(tlabel.SIGN, offset = 8, run = True)
|
|||
|
|
by_cmd.send_angle_claw(15)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_z(30, 60)
|
|||
|
|
by_cmd.send_position_axis_x(1, 130)
|
|||
|
|
|
|||
|
|
if self.target_person == 1:
|
|||
|
|
target_distance = self.pos_gap1
|
|||
|
|
else:
|
|||
|
|
target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
|
|||
|
|
by_cmd.send_distance_x(10, target_distance)
|
|||
|
|
logger.info(f"target distance {target_distance}")
|
|||
|
|
time.sleep(1.5 + (self.target_person - 1) * 0.7 )
|
|||
|
|
car_stop()
|
|||
|
|
|
|||
|
|
# by_cmd.send_angle_claw_arm(220)
|
|||
|
|
# time.sleep(0.5)
|
|||
|
|
by_cmd.send_position_axis_x(1, 20)
|
|||
|
|
time.sleep(3)
|
|||
|
|
by_cmd.send_position_axis_x(1, 120)
|
|||
|
|
time.sleep(1)
|
|||
|
|
logger.debug("結束任務，前進四")
|
|||
|
|
# by_cmd.send_speed_x(25)
|
|||
|
|
# time.sleep(4)
|
|||
|
|
pass
|
|||
|
|
def nexec(self):
|
|||
|
|
pass
|
|||
|
|
def after(self):
|
|||
|
|
var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
|