import os import math import time import threading import subprocess import re import json from copy import deepcopy 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.motor_power = 'on' 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 'daq_power' in data: self.scanner(index).daq_info['power'] = data['daq_power'] if 'filament_info' in data: filament_info = deepcopy(data['filament_info']) filament_info['id'] = data['id'] self.udp_socket.writeDatagram(QByteArray(json.dumps(filament_info).encode()) , QHostAddress("127.0.0.1"), 62000) if 'motor_power' in data: self.scanner(index).motor_power = data['motor_power'] 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 self.scan_count = 8 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.work_mode == "normal": idle_time = 18.0 if self.scanner_idle_time(self.ID_MT1) > idle_time and self.scanner_idle_time(self.ID_MT2) > 1.0: 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) > idle_time: 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)