Initial commit: 上传完整代码
This commit is contained in:
@@ -0,0 +1,390 @@
|
||||
import os
|
||||
import math
|
||||
import time
|
||||
import threading
|
||||
import subprocess
|
||||
import re
|
||||
import json
|
||||
from config import *
|
||||
from PySide6.QtCore import *
|
||||
from PySide6.QtNetwork import QUdpSocket, QHostAddress
|
||||
import socket, struct
|
||||
|
||||
from PySide6.QtCore import QTimer
|
||||
|
||||
class ScannerState:
|
||||
STOP = 'stop'
|
||||
MOVING_UP = 'moving_up'
|
||||
MOVING_DOWN = 'moving_down'
|
||||
STOP_AT_UPPER = 'stop_at_upper'
|
||||
STOP_AT_BOTTOM = 'stop_at_bottom'
|
||||
STOP_AT_ORIGIN = 'stop_at_origin'
|
||||
UNKNOWN = 'unknown'
|
||||
|
||||
valid_states = [STOP, MOVING_UP, MOVING_DOWN,
|
||||
STOP_AT_UPPER, STOP_AT_BOTTOM,
|
||||
STOP_AT_ORIGIN, UNKNOWN]
|
||||
|
||||
class Scanner:
|
||||
def __init__(self, id):
|
||||
self.id = id
|
||||
self.config = {}
|
||||
self.state = ScannerState.UNKNOWN
|
||||
self.running = False
|
||||
self.last_update_time = 0
|
||||
self.location = 0
|
||||
self.velocity = 0
|
||||
|
||||
self.daq_info = {}
|
||||
|
||||
self.last_moving_time = 0
|
||||
self.last_idle_time = 0
|
||||
|
||||
self.run_flag = False
|
||||
|
||||
self.read_config()
|
||||
|
||||
self.ip = self.config.get("ip", "192.168.100.21")
|
||||
|
||||
def read_config(self):
|
||||
try:
|
||||
config = {}
|
||||
path = "/home/admin/.config/cpy/scanner.json"
|
||||
if not os.path.exists(path):
|
||||
return
|
||||
|
||||
with open(path, "r") as f:
|
||||
config = json.load(f)
|
||||
|
||||
self.config = config[f"scanner{self.id}"]
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
class MotorManager(QObject):
|
||||
update_location = Signal(int,int)
|
||||
update_direction = Signal(int,str)
|
||||
update_estimates = Signal(int, float, float)
|
||||
update_online_status = Signal(int, int)
|
||||
update_setting_velocity = Signal(int, float)
|
||||
update_duration = Signal(int,float)
|
||||
update_state = Signal(int, int)
|
||||
update_work_mode = Signal(str)
|
||||
|
||||
# 定义ID_MT1、ID_MT2两个常量为0x01, 0x02
|
||||
ID_MT1 = 0x01
|
||||
ID_MT2 = 0x02
|
||||
|
||||
UDP_STATUS_PORT = 11200
|
||||
UDP_CMD_PORT = 11300
|
||||
|
||||
def __init__(self, bus_id = 1):
|
||||
super().__init__()
|
||||
|
||||
self.scanners = [Scanner(self.ID_MT1),
|
||||
Scanner(self.ID_MT2)]
|
||||
|
||||
|
||||
self.mqtt_client = None
|
||||
|
||||
self.read_config()
|
||||
self.work_mode = "fast"
|
||||
print(self.config)
|
||||
|
||||
self.light_status = None
|
||||
self.last_light_on_time = 0
|
||||
self.start_scan_time = 0
|
||||
|
||||
self.online_status_update_time = 0
|
||||
self.moto_info_update_time = 0
|
||||
|
||||
self.udp_socket = QUdpSocket(self)
|
||||
self.udp_socket.bind(QHostAddress.Any, self.UDP_STATUS_PORT)
|
||||
self.udp_socket.readyRead.connect(self.udp_data_handle)
|
||||
|
||||
self.timer = QTimer(self)
|
||||
self.timer.setInterval(100)
|
||||
self.timer.timeout.connect(self.timer_callback)
|
||||
self.timer.start()
|
||||
|
||||
def scanner(self, index):
|
||||
return self.scanners[index - 1]
|
||||
|
||||
def send_udp_cmd(self, index, cmd):
|
||||
try:
|
||||
ip = self.scanner(index).ip
|
||||
self.udp_socket.writeDatagram(json.dumps(cmd).encode(), QHostAddress(ip), self.UDP_CMD_PORT)
|
||||
print("send to", ip, cmd)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
def start_scanning(self, index, count = 1, period = 6000):
|
||||
if self.scanner(index).run_flag == True:
|
||||
return
|
||||
|
||||
print("scanner", index, self.scanner(index).run_flag)
|
||||
|
||||
self.scanner(index).run_flag = True
|
||||
self.scanner(index).running = True
|
||||
self.send_udp_cmd(index, {"cmd": "start", "count": count, "period": period})
|
||||
|
||||
def stop_scanning(self, index):
|
||||
self.scanner(index).running = False
|
||||
self.send_udp_cmd(index, {"cmd": "stop"})
|
||||
|
||||
def stop_scanning_emer(self, index):
|
||||
self.scanner(index).running = False
|
||||
self.send_udp_cmd(index, {"cmd": "emer_stop"})
|
||||
|
||||
def reset_scanning(self, index):
|
||||
self.scanner(index).running = False
|
||||
self.send_udp_cmd(index, {"cmd": "reset"})
|
||||
|
||||
def update_motor_info(self):
|
||||
try:
|
||||
if time.time() > self.moto_info_update_time + 0.300:
|
||||
state_map = {
|
||||
ScannerState.STOP_AT_BOTTOM: 3,
|
||||
ScannerState.STOP_AT_UPPER: 1,
|
||||
ScannerState.STOP_AT_ORIGIN: 0,
|
||||
ScannerState.MOVING_UP: 2,
|
||||
ScannerState.MOVING_DOWN: 2,
|
||||
}
|
||||
|
||||
try:
|
||||
state = state_map[self.scanner(self.ID_MT1).state]
|
||||
self.update_location.emit(self.ID_MT1, state)
|
||||
self.update_estimates.emit(self.ID_MT1, self.scanner(self.ID_MT1).location, self.scanner(self.ID_MT1).velocity)
|
||||
# print(self.scanner(self.ID_MT1).location, self.scanner(self.ID_MT1).velocity)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
try:
|
||||
state = state_map[self.scanner(self.ID_MT2).state]
|
||||
self.update_location.emit(self.ID_MT2, state)
|
||||
self.update_estimates.emit(self.ID_MT2, self.scanner(self.ID_MT2).location, self.scanner(self.ID_MT2).velocity)
|
||||
# print(self.scanner(self.ID_MT2).location, self.scanner(self.ID_MT2).velocity)
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
self.moto_info_update_time = time.time()
|
||||
except Exception as e:
|
||||
print(e)
|
||||
self.moto_info_update_time = time.time()
|
||||
|
||||
def update_online_state(self):
|
||||
try:
|
||||
if time.time() > self.online_status_update_time + 1:
|
||||
online_state = {
|
||||
self.ID_MT1: time.time() < self.scanner(self.ID_MT1).last_update_time + 1,
|
||||
self.ID_MT2: time.time() < self.scanner(self.ID_MT2).last_update_time + 1,
|
||||
}
|
||||
self.update_online_status.emit(self.ID_MT1, online_state[self.ID_MT1])
|
||||
self.update_online_status.emit(self.ID_MT2, online_state[self.ID_MT2])
|
||||
self.online_status_update_time = time.time()
|
||||
# print("online state:", online_state)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
self.online_status_update_time = time.time()
|
||||
|
||||
@Slot(bytes)
|
||||
def udp_data_handle(self):
|
||||
while self.udp_socket.hasPendingDatagrams():
|
||||
datagram, addr, port = self.udp_socket.readDatagram(self.udp_socket.pendingDatagramSize())
|
||||
datagram = bytes(datagram.data())
|
||||
# print(f"Received datagram: {datagram} from {addr}:{port}")
|
||||
try:
|
||||
data = json.loads(datagram.decode())
|
||||
# print(f"Parsed JSON data: {data}")
|
||||
index = int(data['id'][0])
|
||||
|
||||
self.scanner(index).state = data['state']
|
||||
self.scanner(index).location = data['pos']
|
||||
self.scanner(index).velocity = data['speed']
|
||||
self.scanner(index).run_flag = data['run_flag']
|
||||
self.scanner(index).last_update_time = time.time()
|
||||
|
||||
if 'daq_temp' in data:
|
||||
self.scanner(index).daq_info['temp'] = data['daq_temp']
|
||||
if 'daq_online' in data:
|
||||
self.scanner(index).daq_info['online'] = data['daq_online']
|
||||
|
||||
if self.scanner(index).state in [ScannerState.MOVING_UP, ScannerState.MOVING_DOWN]:
|
||||
self.scanner(index).last_moving_time = time.time()
|
||||
|
||||
elif self.scanner(index).state in [ScannerState.STOP_AT_BOTTOM, ScannerState.STOP_AT_UPPER, ScannerState.STOP_AT_ORIGIN]:
|
||||
self.scanner(index).last_idle_time = time.time()
|
||||
|
||||
|
||||
except json.JSONDecodeError as e:
|
||||
print(f"JSON decode error: {e}")
|
||||
|
||||
def scanner_idle_time(self, index):
|
||||
return time.time() - self.scanner(index).last_moving_time
|
||||
|
||||
def scan(self):
|
||||
# print(self.scanner(self.ID_MT1).running, self.scanner(self.ID_MT2).running)
|
||||
# print(self.scanner(self.ID_MT1).last_moving_time, self.scanner(self.ID_MT2).last_moving_time)
|
||||
# print(time.time(), "\n")
|
||||
|
||||
if self.work_mode == "normal" and self.scan_count <= 0:
|
||||
idle_time = 60.0 * 1
|
||||
if self.scanner_idle_time(self.ID_MT1) > idle_time and self.scanner_idle_time(self.ID_MT2) > idle_time:
|
||||
self.scan_count = 8
|
||||
elif self.work_mode == "slow" and self.scan_count <= 0:
|
||||
idle_time = 60.0 * 5
|
||||
if self.scanner_idle_time(self.ID_MT1) > idle_time and self.scanner_idle_time(self.ID_MT2) > idle_time:
|
||||
self.scan_count = 8
|
||||
elif self.work_mode == "fast":
|
||||
self.scan_count = 8
|
||||
|
||||
if self.scan_count <= 0:
|
||||
return
|
||||
|
||||
if time.time() - self.start_scan_time < 2.0:
|
||||
return
|
||||
|
||||
if self.scanner(self.ID_MT1).running and (not self.scanner(self.ID_MT2).running):
|
||||
if time.time() > self.scanner(self.ID_MT1).last_moving_time + 3.0:
|
||||
self.start_scanning(self.ID_MT1)
|
||||
print("start_scanning(self.ID_MT1)")
|
||||
self.scan_count -= 2
|
||||
self.start_scan_time = time.time()
|
||||
elif (not self.scanner(self.ID_MT1).running) and self.scanner(self.ID_MT2).running:
|
||||
if time.time() > self.scanner(self.ID_MT2).last_moving_time + 3.0:
|
||||
self.start_scanning(self.ID_MT2)
|
||||
self.scan_count -= 2
|
||||
self.start_scan_time = time.time()
|
||||
elif self.scanner(self.ID_MT1).running and self.scanner(self.ID_MT2).running:
|
||||
idle_time = 3.0
|
||||
if self.scanner_idle_time(self.ID_MT1) > idle_time and self.scanner_idle_time(self.ID_MT2) > idle_time:
|
||||
self.start_scanning(self.ID_MT1)
|
||||
self.scan_count -= 1
|
||||
self.start_scan_time = time.time()
|
||||
elif (self.scanner_idle_time(self.ID_MT1) > 1.0) and (self.scanner_idle_time(self.ID_MT2) > 1.0):
|
||||
if self.scanner_idle_time(self.ID_MT1) > self.scanner_idle_time(self.ID_MT2):
|
||||
self.start_scanning(self.ID_MT1)
|
||||
self.scan_count -= 1
|
||||
self.start_scan_time = time.time()
|
||||
else:
|
||||
self.start_scanning(self.ID_MT2)
|
||||
self.scan_count -= 1
|
||||
self.start_scan_time = time.time()
|
||||
|
||||
def update_daq_info(self):
|
||||
try:
|
||||
daq_index = self.daq_index
|
||||
if time.time() - self.daq_info_update_time < 1.0:
|
||||
return
|
||||
packet = {
|
||||
'id' : daq_index,
|
||||
'type' : "daq_info",
|
||||
'status' : self.scanner(daq_index).daq_info
|
||||
}
|
||||
self.udp_socket.writeDatagram(json.dumps(packet).encode(), QHostAddress("127.0.0.1"), 50009)
|
||||
self.daq_info_update_time = time.time()
|
||||
if daq_index == self.ID_MT1:
|
||||
self.daq_index = self.ID_MT2
|
||||
else:
|
||||
self.daq_index = self.ID_MT1
|
||||
except Exception as e:
|
||||
self.daq_index = self.ID_MT1
|
||||
self.daq_info_update_time = time.time()
|
||||
print(f"Error reading DAQ: {e}")
|
||||
|
||||
def timer_callback(self):
|
||||
self.update_online_state()
|
||||
self.update_motor_info()
|
||||
self.update_daq_info()
|
||||
|
||||
self.scan()
|
||||
|
||||
def write_config(self):
|
||||
with open("/home/admin/.config/cpy/config.json", "w") as f:
|
||||
json.dump(self.config, f)
|
||||
|
||||
def default_config(self):
|
||||
config = {
|
||||
|
||||
}
|
||||
self.config = config
|
||||
|
||||
return config
|
||||
|
||||
def read_config(self):
|
||||
config = {}
|
||||
path = "/home/admin/.config/cpy/config.json"
|
||||
if not os.path.exists(path):
|
||||
self.default_config()
|
||||
return self.config
|
||||
|
||||
with open(path, "r") as f:
|
||||
config = json.load(f)
|
||||
|
||||
self.config = config
|
||||
return config
|
||||
|
||||
def start(self, motor_index):
|
||||
if motor_index & self.ID_MT1:
|
||||
self.scanner(self.ID_MT1).running = True
|
||||
if self.scanner_idle_time(self.ID_MT1) > 3.0:
|
||||
self.scanner(self.ID_MT1).last_moving_time = 0.0
|
||||
if motor_index & self.ID_MT2:
|
||||
self.scanner(self.ID_MT2).running = True
|
||||
if self.scanner_idle_time(self.ID_MT2) > 3.0:
|
||||
self.scanner(self.ID_MT2).last_moving_time = 0.0
|
||||
|
||||
def emergency_stop(self, motor_index):
|
||||
|
||||
if motor_index & self.ID_MT1:
|
||||
self.stop_scanning_emer(self.ID_MT1)
|
||||
if motor_index & self.ID_MT2:
|
||||
self.stop_scanning_emer(self.ID_MT2)
|
||||
|
||||
def stop(self, motor_index):
|
||||
|
||||
if motor_index & self.ID_MT1:
|
||||
self.stop_scanning(self.ID_MT1)
|
||||
if motor_index & self.ID_MT2:
|
||||
self.stop_scanning(self.ID_MT2)
|
||||
|
||||
def destroy(self):
|
||||
pass
|
||||
|
||||
@Slot(int, str, str)
|
||||
def set_globaldev_value(self, motor_index, key, value):
|
||||
print("set_globaldev_value", motor_index, key, value)
|
||||
if motor_index == 3 and key == "sports_mode":
|
||||
self.work_mode = value
|
||||
print("self.work_mode =",self.work_mode)
|
||||
self.update_work_mode.emit(self.work_mode)
|
||||
if motor_index == 3 and key == "switch":
|
||||
try:
|
||||
val = int(value)
|
||||
|
||||
except ValueError:
|
||||
print("set_globaldev_value ValueError", motor_index, key, value)
|
||||
return
|
||||
|
||||
if val == 0:
|
||||
self.reset_scanning(self.ID_MT1)
|
||||
self.reset_scanning(self.ID_MT2)
|
||||
|
||||
@Slot(int, str, int)
|
||||
def set_value(self, motor_index, key, value):
|
||||
if key == "profileVelocity":
|
||||
return
|
||||
|
||||
if key == "emergency_stop":
|
||||
self.emergency_stop(motor_index)
|
||||
elif key == "switch":
|
||||
if value == 1:
|
||||
self.start(motor_index)
|
||||
else:
|
||||
if motor_index == 3:
|
||||
self.reset_scanning(self.ID_MT1)
|
||||
self.reset_scanning(self.ID_MT2)
|
||||
else:
|
||||
self.stop(motor_index)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user