222 lines
8.0 KiB
Python
222 lines
8.0 KiB
Python
import threading
|
|
import canopen
|
|
import time
|
|
import os
|
|
|
|
|
|
class CANBackend:
|
|
def __init__(self, eds_file =None):
|
|
self.network = None
|
|
self.nodes = {} # {1: RemoteNode(0x02), 2: RemoteNode(0x03), ...}
|
|
self.connected = False
|
|
self.lock = threading.Lock()
|
|
self.polling_thread = None
|
|
self.polling_active = False
|
|
self.latest_data = {
|
|
1: {}, # PU1
|
|
2: {}, # PU2
|
|
3: {} # PU3
|
|
}
|
|
if eds_file is None:
|
|
self.eds_path = os.path.join(os.path.dirname(__file__), "eds_file", "processBoard_0.eds")
|
|
else:
|
|
self.eds_path = eds_file
|
|
|
|
def connect(self):
|
|
try:
|
|
self.network = canopen.Network()
|
|
self.network.connect(channel='can0', bustype='socketcan')
|
|
|
|
# PU mapping: PU1->0x02, PU2->0x04, PU3->0x127
|
|
node_map = {
|
|
1: 0x02,
|
|
2: 0x04,
|
|
3: 0x127,
|
|
}
|
|
|
|
for pu_number, node_id in node_map.items():
|
|
node = canopen.RemoteNode(node_id, self.eds_path)
|
|
self.network.add_node(node)
|
|
self.nodes[pu_number] = node
|
|
|
|
self.connected = True
|
|
self._start_sdo_polling()
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"[CONNECT ERROR] {e}")
|
|
return False
|
|
|
|
def shutdown(self):
|
|
self.polling_active = False
|
|
if self.network:
|
|
self.network.disconnect()
|
|
self.nodes.clear()
|
|
self.connected = False
|
|
|
|
def _start_sdo_polling(self):
|
|
if self.polling_thread and self.polling_thread.is_alive():
|
|
return
|
|
self.polling_active = True
|
|
self.polling_thread = threading.Thread(target=self._sdo_polling_loop, daemon=True)
|
|
self.polling_thread.start()
|
|
|
|
def _sdo_polling_loop(self):
|
|
while self.polling_active:
|
|
with self.lock:
|
|
try:
|
|
for pu_number, node in self.nodes.items():
|
|
try:
|
|
fm1 = node.sdo[0x2004][1].raw
|
|
fm2 = node.sdo[0x2004][2].raw
|
|
fm3 = node.sdo[0x2004][3].raw
|
|
fm4 = node.sdo[0x2004][4].raw
|
|
|
|
ps1 = node.sdo[0x2005][1].raw
|
|
ps2 = node.sdo[0x2005][2].raw
|
|
ps3 = node.sdo[0x2005][3].raw
|
|
ps4 = node.sdo[0x2005][4].raw
|
|
|
|
mv02Cmd = node.sdo[0x2014][1].raw
|
|
mv03Cmd = node.sdo[0x2012][1].raw
|
|
mv04Cmd = node.sdo[0x2019][1].raw
|
|
mv05Cmd = node.sdo[0x2020][1].raw
|
|
mv06Cmd = node.sdo[0x2021][1].raw
|
|
mv07Cmd = node.sdo[0x2015][1].raw
|
|
mv08Cmd = node.sdo[0x2022][1].raw
|
|
|
|
# mv02fb = node.sdo[0x3000][2].raw
|
|
# mv03fb = node.sdo[0x3000][3].raw
|
|
# mv04fb = node.sdo[0x3000][4].raw
|
|
# mv05fb = node.sdo[0x3000][5].raw
|
|
# mv06fb = node.sdo[0x3000][6].raw
|
|
# mv07fb = node.sdo[0x3000][7].raw
|
|
# mv08fb = node.sdo[0x3000][8].raw
|
|
|
|
self.latest_data[pu_number] = {
|
|
"FM1": (fm1 / 100.0) * 60.0,
|
|
"FM2": (fm2 / 100.0) * 60.0,
|
|
"FM3": (fm3 / 100.0) * 60.0,
|
|
"FM4": (fm4 / 100.0) * 60.0,
|
|
|
|
"PS1": ps1 / 1000.0,
|
|
"PS2": ps2 / 1000.0,
|
|
"PS3": ps3 / 1000.0,
|
|
"PS4": ps4 / 1000.0,
|
|
|
|
"MV02_sp" : mv02Cmd / 100.0,
|
|
"MV03_sp" : mv03Cmd / 100.0,
|
|
"MV04_sp" : mv04Cmd / 100.0,
|
|
"MV05_sp" : mv05Cmd / 100.0,
|
|
"MV06_sp" : mv06Cmd / 100.0,
|
|
"MV07_sp" : mv07Cmd / 100.0,
|
|
"MV08_sp" : mv08Cmd / 100.0,
|
|
|
|
# "MV02" : mv02fb,
|
|
# "MV03" : mv03fb,
|
|
# "MV04" : mv04fb,
|
|
# "MV05" : mv05fb,
|
|
# "MV06" : mv06fb,
|
|
# "MV07" : mv07fb,
|
|
# "MV08" : mv08fb,
|
|
}
|
|
|
|
print(f"[PU{pu_number}] FM1: {fm1}, PS1: {ps1}")
|
|
except Exception as inner_e:
|
|
print(f"[SDO READ ERROR] PU{pu_number}: {inner_e}")
|
|
except Exception as outer_e:
|
|
print(f"[SDO POLL ERROR] {outer_e}")
|
|
|
|
time.sleep(1.0)
|
|
|
|
def get_latest_data(self, pu_number: int):
|
|
with self.lock:
|
|
return self.latest_data.get(pu_number, {}).copy()
|
|
|
|
def read_current_state(self, pu_number: int):
|
|
try:
|
|
node = self.nodes.get(pu_number)
|
|
if node is None:
|
|
return "Offline"
|
|
state_raw = node.sdo[0x2000].raw
|
|
return self.decode_state(state_raw)
|
|
except Exception as e:
|
|
print(f"[PU{pu_number} READ ERROR] {e}")
|
|
return "Offline"
|
|
|
|
def decode_state(self, state_val: int) -> str:
|
|
state_map = {
|
|
0: "SYSTEM_MODE_INIT",
|
|
1: "SYSTEM_MODE_OFF",
|
|
2: "SYSTEM_MODE_READY",
|
|
3: "SYSTEM_MODE_PRODUCTION",
|
|
4: "SYSTEM_MODE_LOW_LOOP_PRESSURE",
|
|
5: "SYSTEM_MODE_LOOP_CLEANING",
|
|
6: "SYSTEM_MODE_HEATING_RO",
|
|
7: "SYSTEM_MODE_RINSING_RO",
|
|
8: "SYSTEM_MODE_HEATING_EDI",
|
|
9: "SYSTEM_MODE_COOLING_EDI",
|
|
10: "SYSTEM_MODE_RO_FLUSH",
|
|
11: "SYSTEM_MODE_RO_RINSE",
|
|
12: "SYSTEM_MODE_EDI_RINSE",
|
|
15: "SYSTEM_MODE_FAIL_SAFE",
|
|
16: "SYSTEM_MODE_FIRST_FLUSH",
|
|
255: "SYSTEM_MODE_DEFAULT"
|
|
}
|
|
return state_map.get(state_val, f"UNKNOWN({state_val})")
|
|
|
|
def send_state_command(self, state: str, pu_number: int, ploop_setpoint: float):
|
|
if not self.connected:
|
|
raise RuntimeError("CAN not connected")
|
|
|
|
state_map = {
|
|
"IDLE": 1,
|
|
"PRE-PRODUCTION": 2,
|
|
"PRODUCTION" : 3,
|
|
"MAINTENANCE": 8,
|
|
"EMERGENCY_STOP": 9,
|
|
"FIRST_START": 10
|
|
}
|
|
|
|
if state not in state_map:
|
|
raise ValueError(f"Invalid state: {state}")
|
|
|
|
try:
|
|
node = self.nodes.get(pu_number)
|
|
if node is None:
|
|
raise ValueError(f"PU{pu_number} not connected")
|
|
|
|
print(f"[DEBUG] Writing state {state_map[state]} to 0x2024:{pu_number}")
|
|
node.sdo[0x2024][0x01].raw = state_map[state]
|
|
|
|
print(f"[DEBUG] Writing ploop_setpoint {ploop_setpoint} to 0x2007")
|
|
node.sdo[0x2007].raw = int(ploop_setpoint * 100)
|
|
|
|
except Exception as e:
|
|
print(f"[SDO WRITE ERROR] PU{pu_number}: {e}")
|
|
raise
|
|
|
|
def send_thermal_loop_cleaning(self, mode: str, pu_number: int):
|
|
if not self.connected:
|
|
raise RuntimeError("CAN not connected")
|
|
|
|
mode_map = {
|
|
"IDLE": 0,
|
|
"ACTIVE": 1
|
|
}
|
|
|
|
if mode not in mode_map:
|
|
raise ValueError(f"Invalid thermal loop mode: {mode}")
|
|
|
|
try:
|
|
node = self.nodes.get(pu_number)
|
|
if node is None:
|
|
raise ValueError(f"PU{pu_number} not connected")
|
|
|
|
print(f"[DEBUG] Sending thermal loop mode {mode} to 0x2024:{pu_number}")
|
|
node.sdo[0x2024][pu_number].raw = mode_map[mode]
|
|
|
|
except Exception as e:
|
|
print(f"[THERMAL LOOP ERROR] PU{pu_number}: {e}")
|
|
raise
|