390 lines
14 KiB
Python
390 lines
14 KiB
Python
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
|
||
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)
|
||
|
||
|
||
|