108 lines
3.3 KiB
Python
108 lines
3.3 KiB
Python
import threading
|
|
import canopen
|
|
import time
|
|
|
|
class CANBackend:
|
|
def __init__(self):
|
|
self.network = None
|
|
self.node = None
|
|
self.connected = False
|
|
self.latest_data = {}
|
|
self.lock = threading.Lock()
|
|
|
|
# Initialize thread-related attributes
|
|
self.polling_thread = None
|
|
self.polling_active = False
|
|
|
|
def connect(self, node_id, eds_path):
|
|
try:
|
|
self.network = canopen.Network()
|
|
self.network.connect(bustype='socketcan', channel='can0', bitrate=250000)
|
|
self.node = canopen.RemoteNode(node_id, eds_path)
|
|
self.network.add_node(self.node)
|
|
self.node.nmt.state = 'PRE-OPERATIONAL'
|
|
|
|
self.connected = True
|
|
self.latest_data = {}
|
|
|
|
# Start polling thread
|
|
self._start_sdo_polling()
|
|
return True
|
|
except Exception as e:
|
|
print(f"[CAN ERROR] {e}")
|
|
self.connected = False
|
|
return False
|
|
|
|
def _start_sdo_polling(self):
|
|
if self.polling_thread and self.polling_thread.is_alive():
|
|
return # Already running
|
|
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:
|
|
fm2 = self.node.sdo[0x2004][2].raw
|
|
ps1 = self.node.sdo[0x2005][1].raw
|
|
self.latest_data["FM2"] = fm2/100
|
|
self.latest_data["PS1"] = ps1/1000
|
|
print(f"[SDO POLL] FM2: {fm2}, PS1: {ps1}")
|
|
except Exception as e:
|
|
print(f"[SDO POLL ERROR] {e}")
|
|
time.sleep(1.0)
|
|
|
|
def get_latest_data(self):
|
|
with self.lock:
|
|
return self.latest_data.copy()
|
|
|
|
def read_current_state(self,pu_number: int):
|
|
# Placeholder for reading mode command
|
|
#TODO : CODE IT
|
|
pass
|
|
|
|
def send_state_command(self, state: str, pu_number: int, ploop_setpoint:float):
|
|
#TODO : link ploop_setpoint
|
|
if not self.connected:
|
|
raise RuntimeError("CAN not connected")
|
|
|
|
state_map = {
|
|
"IDLE": 0,
|
|
"PRODUCTION": 1,
|
|
"MAINTENANCE": 3,
|
|
"EMERGENCY_STOP": 4,
|
|
"FIRST_START": 5
|
|
}
|
|
|
|
if state not in state_map:
|
|
raise ValueError(f"Invalid state: {state}")
|
|
|
|
try:
|
|
print(f"[DEBUG] Writing state {state_map[state]} to index 0x2024, subindex {pu_number}")
|
|
self.node.sdo[0x2024][0x01].raw = state_map[state]
|
|
except Exception as e:
|
|
print(f"[SDO WRITE ERROR] Failed to write to 0x2024:{pu_number} - {e}")
|
|
raise
|
|
|
|
|
|
def send_thermal_loop_cleaning(self, mode: str):
|
|
if not self.connected:
|
|
raise RuntimeError("Not connected to CAN")
|
|
|
|
mode_map = {
|
|
"IDLE": 0,
|
|
"ACTIVE": 1
|
|
}
|
|
|
|
if mode not in mode_map:
|
|
raise ValueError(f"Invalid thermal loop mode: {mode}")
|
|
|
|
self.node.sdo[0x2024][0x01].raw = mode_map[mode]
|
|
|
|
def shutdown(self):
|
|
self.polling_active = False
|
|
if self.network:
|
|
self.network.disconnect()
|
|
self.connected = False
|