Initial commit: 上传完整代码

This commit is contained in:
2026-06-19 20:47:06 +08:00
commit 78b5898da2
71 changed files with 8767 additions and 0 deletions
@@ -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)