diff --git a/action.py b/action.py new file mode 100644 index 0000000..e14b941 --- /dev/null +++ b/action.py @@ -0,0 +1,99 @@ +import threading +import time +import zmq +import numpy as np +from loguru import logger +from simple_pid import PID +from utils import PidWrap + +bycmd = None +move = None + +def import_obj(_bycmd): + global bycmd + global move + + bycmd = _bycmd + move = move_cls() + +class lane_cls: + def __init__(self, _speed, _time, _pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0}) -> None: + self.context = zmq.Context() + self.socket = self.context.socket(zmq.REQ) + self.socket.connect("tcp://localhost:6666") + self.speed = _speed + self.time = _time + # 每 20ms 控制一次,故次数乘以 50 + self.count = self.time * 50 + logger.info(f"count = {self.count}") + self.busy = False + self. pid = PidWrap(_pid_argv["kp"], _pid_argv["ki"], _pid_argv["kd"], output_limits=50) + pass + def loop(self): + while self.count >= 0: + self.count -= 1 + + self.socket.send_string("") + resp = self.socket.recv_pyobj() + # 检查该值是否有效 + ck_val = resp.get('data') + if isinstance(ck_val, np.ndarray): + x = ck_val[0] + y = ck_val[1] + else: + continue + err = x - 160 + + pid_out = self.pid.get(err) + bycmd.send_speed_omega(pid_out) + + # logger.debug(f"[任务中巡线模式]# error:{err} out:{pid_out}") + time.sleep(0.012) # 时间补偿,保证控制周期 (虽然还是挺不准的) + + for _ in range(3): + bycmd.send_speed_x(0) + bycmd.send_speed_omega(0) + self.busy = False + logger.info("[任务中巡线模式] #结束") + return + + def start(self): + logger.info(f"[任务中巡线模式] #开启,速度:{self.speed} 时间:{self.time}") + bycmd.send_speed_x(self.speed) + self.busy = True + thread = threading.Thread(target=self.loop) + thread.start() + pass + def inquire(self): + # logger.debug(f"[任务中巡线模式]# 查询 当前 busy 状态 {self.busy}") + return self.busy + +class move_cls: + # def x(self, _speed): + # bycmd.send_speed_x(_speed) + # pass + # def y(self, _speed): + # bycmd.send_speed_y(_speed) + # pass + # def z(self, _speed): + # bycmd.send_speed_omega(_speed) + # pass + def lane(self, _speed, _time): + lane_obj = lane_cls(_speed, _time) + lane_obj.start() + # 此处为阻塞实现,非阻塞调用可直接从 lane_cls 类构造对象,然后使用查询方法 + # time.sleep(0.5) # 等待子线程启动 + while lane_obj.inquire() is True: + pass + +# TODO 新运动指令类,指令的发送和完成查询功能,发送时开启新线程 + +# TODO 轴控制指令类,增加指令后加入动作队列,非阻塞 + +class axis_cls(): + def axis_z(self, _distance): + pass + def axis_z2(self, _position): + pass + def exec(self): + pass \ No newline at end of file diff --git a/main.py b/main.py index 0b9a47c..5758090 100644 --- a/main.py +++ b/main.py @@ -6,9 +6,11 @@ import subtask as sb import majtask as mj from by_cmd_py import by_cmd_py import time +import action as act cmd_py_obj = by_cmd_py() sb.import_obj(cmd_py_obj) +act.import_obj(cmd_py_obj) # 读取配置 cfg_main = toml.load('cfg_main.toml') @@ -16,62 +18,64 @@ cfg_main = toml.load('cfg_main.toml') # 配置日志输出 logger.add(cfg_main['debug']['logger_filename'], format=cfg_main['debug']['logger_format'], retention = 5, level="INFO") -# 向任务队列添加任务 -task_queue = queue.Queue() -task_queue.put(sb.task(sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) -task_queue.put(sb.task(sb.get_block2, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) -task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'])) -task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) -# TODO 添加一个空任务用于提前降 z 轴 -task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable'])) -task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable'])) -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'])) +act.move.lane(15, 5) -# 将任务队列传入调度模块中 -task_queuem_t = sb.task_queuem(task_queue) +# # 向任务队列添加任务 +# task_queue = queue.Queue() +# task_queue.put(sb.task(sb.get_block1, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) +# task_queue.put(sb.task(sb.get_block2, cfg_main['find_counts']['GetBlock_counts'], cfg_main['task']['GetBlock_enable'])) +# task_queue.put(sb.task(sb.put_block, cfg_main['find_counts']['PutBlock_counts'], cfg_main['task']['GetBlock_enable'])) +# task_queue.put(sb.task(sb.get_bball, cfg_main['find_counts']['GetBBall_counts'], cfg_main['task']['GetBBall_enable'])) +# # TODO 添加一个空任务用于提前降 z 轴 +# task_queue.put(sb.task(sb.up_tower, cfg_main['find_counts']['UpTower_counts'], cfg_main['task']['UpTower_enable'])) +# task_queue.put(sb.task(sb.get_rball, cfg_main['find_counts']['GetRBall_counts'], cfg_main['task']['GetRBall_enable'])) +# 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'])) -# 创建任务队列的工作线程 -def worker_thread(): - while task_queuem_t.exec() is True: - pass +# # 将任务队列传入调度模块中 +# task_queuem_t = sb.task_queuem(task_queue) -# 启动工作线程 -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_position_axis_x(1, 140) -time.sleep(0.5) -cmd_py_obj.send_angle_storage(20) -time.sleep(0.5) -cmd_py_obj.send_angle_scoop(25) -time.sleep(2) +# # 创建任务队列的工作线程 +# def worker_thread(): +# while task_queuem_t.exec() is True: +# pass -# 创建主任务 -main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象 +# # 启动工作线程 +# 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_position_axis_x(1, 140) +# time.sleep(0.5) +# cmd_py_obj.send_angle_storage(20) +# time.sleep(0.5) +# cmd_py_obj.send_angle_scoop(25) +# time.sleep(2) -# 主线程仅在子线程搜索 (SEARCHING) 和 空闲 (IDLE) 状态下进行操作 -# while task_queuem_t.busy is True: -try: - while True: - if task_queuem_t.status is sb.task_queuem_status.EXECUTING: - pass - else: - main_task_t.run() - pass -except KeyboardInterrupt: - logger.info("Interrupt received, stopping...") - # 停车 - for _ in range(3): - cmd_py_obj.send_speed_x(0) - time.sleep(0.1) - cmd_py_obj.send_speed_omega(0) - time.sleep(0.1) +# # 创建主任务 +# main_task_t = mj.main_task(cmd_py_obj) # 初始化时传入 zmq socket 对象 -logger.info("Main thread exit") +# # 主线程仅在子线程搜索 (SEARCHING) 和 空闲 (IDLE) 状态下进行操作 +# # while task_queuem_t.busy is True: +# try: +# while True: +# if task_queuem_t.status is sb.task_queuem_status.EXECUTING: +# pass +# else: +# main_task_t.run() +# pass +# except KeyboardInterrupt: +# logger.info("Interrupt received, stopping...") +# # 停车 +# for _ in range(3): +# cmd_py_obj.send_speed_x(0) +# time.sleep(0.1) +# cmd_py_obj.send_speed_omega(0) +# time.sleep(0.1) + +# logger.info("Main thread exit") diff --git a/majtask.py b/majtask.py index a6db79e..ffb9f45 100644 --- a/majtask.py +++ b/majtask.py @@ -1,21 +1,9 @@ -from simple_pid import PID import zmq import time from loguru import logger import utils - -class PidWrap: - def __init__(self, kp, ki, kd, setpoint=0, output_limits=1): - self.pid_t = PID(kp, ki, kd, setpoint, output_limits=(0-output_limits, output_limits)) - 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 - def get(self, val_in): - return self.pid_t(val_in) +from utils import PidWrap class main_task(): def __init__(self,by_cmd): diff --git a/utils.py b/utils.py index ea869bf..aefe444 100644 --- a/utils.py +++ b/utils.py @@ -2,6 +2,8 @@ from enum import Enum import numpy as np import erniebot +from simple_pid import PID + # 巡线误差 lane_error = 0 # 进入任务时可以通过修改 task_speed 控制巡线速度 @@ -302,6 +304,18 @@ class CountRecord: else: return False +class PidWrap: + def __init__(self, kp, ki, kd, setpoint=0, output_limits=1): + self.pid_t = PID(kp, ki, kd, setpoint, output_limits=(0-output_limits, output_limits)) + 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 + def get(self, val_in): + return self.pid_t(val_in) + if __name__ == '__main__':