import time import matplotlib.pyplot as plt import matplotlib.animation as animation from collections import deque from .rsi_client import RSIClient class RSIGraphing: """Handles real-time and CSV-based graphing for RSI analysis with alerts and threshold monitoring.""" def __init__(self, client, mode="position", overlay=False, plan_file=None): """Initialize graphing for real-time plotting.""" self.client = client self.mode = mode self.overlay = overlay # Enable/Disable planned vs. actual overlay self.alerts_enabled = True # Default to alerts on self.deviation_threshold = 5.0 # Default deviation threshold (mm) self.force_threshold = 10.0 # Default force threshold (Nm) self.fig, self.ax = plt.subplots(figsize=(10, 6)) # ✅ Data storage self.time_data = deque(maxlen=100) # Store last 100 data points self.position_data = {axis: deque(maxlen=100) for axis in ["X", "Y", "Z"]} self.velocity_data = {axis: deque(maxlen=100) for axis in ["X", "Y", "Z"]} self.acceleration_data = {axis: deque(maxlen=100) for axis in ["X", "Y", "Z"]} self.force_data = {axis: deque(maxlen=100) for axis in ["A1", "A2", "A3", "A4", "A5", "A6"]} self.previous_positions = {"X": 0, "Y": 0, "Z": 0} self.previous_velocities = {"X": 0, "Y": 0, "Z": 0} self.previous_time = time.time() # ✅ Planned movement overlay storage self.planned_data = {axis: deque(maxlen=100) for axis in ["X", "Y", "Z"]} self.deviation_data = {axis: deque(maxlen=100) for axis in ["X", "Y", "Z"]} if plan_file: self.load_plan(plan_file) self.ani = animation.FuncAnimation(self.fig, self.update_graph, interval=100, cache_frame_data=False) plt.show() def update_graph(self, frame): """Update the live graph based on selected mode and check for alerts.""" current_time = time.time() dt = current_time - self.previous_time self.previous_time = current_time # ✅ Fetch live data from RSI client position = self.client.receive_variables.get("RIst", {"X": 0, "Y": 0, "Z": 0}) force = self.client.receive_variables.get("MaCur", {"A1": 0, "A2": 0, "A3": 0, "A4": 0, "A5": 0, "A6": 0}) # ✅ Compute velocity and acceleration for axis in ["X", "Y", "Z"]: velocity = (position[axis] - self.previous_positions[axis]) / dt if dt > 0 else 0 acceleration = (velocity - self.previous_velocities[axis]) / dt if dt > 0 else 0 self.previous_positions[axis] = position[axis] self.previous_velocities[axis] = velocity self.position_data[axis].append(position[axis]) self.velocity_data[axis].append(velocity) self.acceleration_data[axis].append(acceleration) for axis in ["A1", "A2", "A3", "A4", "A5", "A6"]: self.force_data[axis].append(force[axis]) self.time_data.append(time.strftime("%H:%M:%S")) # ✅ Planned vs. Actual movement overlay if self.overlay and self.planned_data: for axis in ["X", "Y", "Z"]: planned_value = self.planned_data[axis][-1] if len(self.planned_data[axis]) > 0 else position[axis] self.planned_data[axis].append(planned_value) deviation = abs(position[axis] - planned_value) self.deviation_data[axis].append(deviation) # ✅ Trigger deviation alert if self.alerts_enabled and deviation > self.deviation_threshold: print(f"⚠️ Deviation Alert! {axis} exceeds {self.deviation_threshold} mm (Deviation: {deviation:.2f} mm)") # ✅ Trigger force spike alert if self.alerts_enabled: for axis in ["A1", "A2", "A3", "A4", "A5", "A6"]: if self.force_data[axis][-1] > self.force_threshold: print(f"⚠️ Force Spike Alert! {axis} exceeds {self.force_threshold} Nm (Force: {self.force_data[axis][-1]:.2f} Nm)") # ✅ Clear the plot self.ax.clear() if self.mode == "position": self.ax.plot(self.time_data, self.position_data["X"], label="X Position") self.ax.plot(self.time_data, self.position_data["Y"], label="Y Position") self.ax.plot(self.time_data, self.position_data["Z"], label="Z Position") self.ax.set_title("Live Position Tracking with Alerts") self.ax.set_ylabel("Position (mm)") if self.overlay: self.ax.plot(self.time_data, self.planned_data["X"], label="Planned X", linestyle="dashed") self.ax.plot(self.time_data, self.planned_data["Y"], label="Planned Y", linestyle="dashed") self.ax.plot(self.time_data, self.planned_data["Z"], label="Planned Z", linestyle="dashed") self.ax.legend() self.ax.set_xlabel("Time") self.ax.tick_params(axis='x', rotation=45) def change_mode(self, mode): """Change the graphing mode dynamically.""" if mode in ["position", "velocity", "acceleration", "force"]: self.mode = mode print(f"✅ Graphing mode changed to: {mode}") else: print("❌ Invalid mode. Available: position, velocity, acceleration, force") def set_alert_threshold(self, alert_type, threshold): """Set threshold for deviation or force alerts.""" if alert_type == "deviation": self.deviation_threshold = threshold elif alert_type == "force": self.force_threshold = threshold print(f"✅ {alert_type.capitalize()} alert threshold set to {threshold}") def enable_alerts(self, enable): """Enable or disable alerts.""" self.alerts_enabled = enable print(f"✅ Alerts {'enabled' if enable else 'disabled'}.") def stop(self): """Stop live plotting loop by closing the figure.""" plt.close(self.fig) if __name__ == "__main__": import argparse parser = argparse.ArgumentParser(description="RSI Graphing Utility") parser.add_argument("--mode", choices=["position", "velocity", "acceleration", "force"], default="position", help="Graphing mode") parser.add_argument("--overlay", action="store_true", help="Enable planned vs. actual overlay") parser.add_argument("--plan", type=str, help="CSV file with planned trajectory") parser.add_argument("--alerts", action="store_true", help="Enable real-time alerts") args = parser.parse_args() client = RSIClient("RSI_EthernetConfig.xml") graphing = RSIGraphing(client, mode=args.mode, overlay=args.overlay, plan_file=args.plan) if not args.alerts: graphing.enable_alerts(False)