99 lines
3.0 KiB
Python
99 lines
3.0 KiB
Python
|
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
|