Files

391 lines
14 KiB
Plaintext
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
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)