diff --git a/src/RSIPI/main.py b/src/RSIPI/main.py index 3e39692..13c094e 100644 --- a/src/RSIPI/main.py +++ b/src/RSIPI/main.py @@ -1,36 +1,87 @@ import time +import threading from RSIPI.rsi_api import RSIAPI -from RSIPI.trajectory_planner import generate_trajectory -from RSIPI.rsi_graphing import plot_csv_file + + +def generate_pulsed_trajectory(total_mm=1.0, step_mm=0.01): + """ + Creates a pulsed trajectory that moves a total distance by sending small RKorr deltas + followed by resets to zero. + """ + steps = int(total_mm / step_mm) + trajectory = [] + for _ in range(steps): + trajectory.append({"X": step_mm}) # Pulse + trajectory.append({"X": 0.0}) # Reset + return trajectory + + +def feed_pulsed_trajectory(api, trajectory): + """ + Sends each RKorr step synchronised with IPOC changes. + """ + stop_event = threading.Event() + + def feeder(): + last_ipoc = api.get_ipoc() + index = 0 + + while index < len(trajectory) and not stop_event.is_set(): + current_ipoc = api.get_ipoc() + + if current_ipoc != last_ipoc: + last_ipoc = current_ipoc + val = trajectory[index].get("X", 0.0) + api.update_cartesian(X=val) + index += 1 + + time.sleep(0.0005) + + stop_event.set() + + thread = threading.Thread(target=feeder) + thread.start() + return stop_event, thread + + +def reset_rkorr(api): + for axis in ["X", "Y", "Z", "A", "B", "C"]: + api.update_variable(f"RKorr.{axis}", 0.0) def main(): api = RSIAPI("RSI_EthernetConfig.xml") - print("⚙️ Starting RSI...") api.start_rsi() + time.sleep(1.0) - print("📦 Starting CSV log...") - api.start_logging() # Automatically creates a timestamped file + # Create trajectories + forward = generate_pulsed_trajectory(total_mm=500.0, step_mm=0.01) + backward = generate_pulsed_trajectory(total_mm=-500.0, step_mm=-0.01) - time.sleep(1.0) # Let handshake and logging settle + loop_count = 5 + print(f"🔁 Starting looped motion (±1 mm), {loop_count} cycles") - # ✅ Send a few small RKorr pulses - print("🔁 Sending RKorr pulses...") - for _ in range(5): - api.update_variable("RKorr.X", 0.5) - time.sleep(0.008) # 2 cycles - api.update_variable("RKorr.X", 0.0) - time.sleep(0.008) + for i in range(loop_count): + print(f"➡️ Loop {i+1}: Forward") + fwd_done, _ = feed_pulsed_trajectory(api, forward) + while not fwd_done.is_set(): + time.sleep(0.005) - print("🛑 Stopping CSV log...") - api.stop_logging() + reset_rkorr(api) - print("🛑 Stopping RSI...") - api.stop_rsi() + print(f"⬅️ Loop {i+1}: Backward") + bwd_done, _ = feed_pulsed_trajectory(api, backward) + while not bwd_done.is_set(): + time.sleep(0.005) - print("✅ Log complete. You can now visualise it with rsi_graphing or plot_csv_file().") + reset_rkorr(api) + print("✅ Motion complete. RSI still running.") + + print("✅ Motion complete. RSI still running. Waiting for operator or Ctrl+C...") + while True: + time.sleep(1) if __name__ == "__main__": main() diff --git a/src/RSIPI/rsi_echo_server.py b/src/RSIPI/rsi_echo_server.py index 8ce4a91..146c86f 100644 --- a/src/RSIPI/rsi_echo_server.py +++ b/src/RSIPI/rsi_echo_server.py @@ -1,11 +1,11 @@ + import socket import time import xml.etree.ElementTree as ET import logging import threading -from rsi_config import RSIConfig +from src.RSIPI.rsi_config import RSIConfig -# ✅ Configure Logging LOGGING_ENABLED = True if LOGGING_ENABLED: @@ -17,9 +17,7 @@ if LOGGING_ENABLED: ) class EchoServer: - """A UDP echo server that sends test messages to the RSI client and logs received messages.""" - - def __init__(self, config_file, delay_ms=4): + def __init__(self, config_file, delay_ms=4, mode="relative"): self.config = RSIConfig(config_file) network_settings = self.config.get_network_settings() @@ -27,107 +25,114 @@ class EchoServer: self.client_address = ("127.0.0.1", network_settings["port"]) self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udp_socket.bind(self.server_address) - + self.last_received = None self.ipoc_value = 123456 - self.delay_ms = delay_ms / 1000 # Convert milliseconds to seconds + self.delay_ms = delay_ms / 1000 + self.mode = mode.lower() # relative or absolute - self.send_variables = self.config.get_send_variables() - self.receive_variables = self.config.get_receive_variables() + # Internal simulation state + self.state = { + "RIst": {k: 0.0 for k in ["X", "Y", "Z", "A", "B", "C"]}, + "AIPos": {f"A{i}": 0.0 for i in range(1, 7)}, + "ELPos": {f"E{i}": 0.0 for i in range(1, 7)}, + "DiO": 0, + "DiL": 0 + } - self.running = True # ✅ Control flag for graceful shutdown + self.running = True self.thread = threading.Thread(target=self.send_message, daemon=True) logging.info(f"✅ Echo Server started on {self.server_address}") - logging.info(f"📡 Sending messages to RSI Client on port {network_settings['port']}") - logging.info(f"⏳ Message delay set to {delay_ms}ms") - print("✅ Echo Server Started") - print(f"📡 Sending messages to RSI Client on port {network_settings['port']}") + print(f"✅ Echo Server started in {self.mode.upper()} mode.") + + def receive_and_process(self): + try: + self.udp_socket.settimeout(self.delay_ms) + data, addr = self.udp_socket.recvfrom(1024) + xml_string = data.decode() + root = ET.fromstring(xml_string) + self.last_received = xml_string # ✅ Save for GUI + + for elem in root: + tag = elem.tag + if tag in ["RKorr", "AKorr"]: + for axis, value in elem.attrib.items(): + value = float(value) + if tag == "RKorr" and axis in self.state["RIst"]: + if self.mode == "relative": + self.state["RIst"][axis] += value + else: + self.state["RIst"][axis] = value + elif tag == "AKorr" and axis in self.state["AIPos"]: + if self.mode == "relative": + self.state["AIPos"][axis] += value + else: + self.state["AIPos"][axis] = value + elif tag in ["DiO", "DiL"]: + if tag in self.state: + self.state[tag] = int(elem.text.strip()) + elif tag == "IPOC": + self.ipoc_value = int(elem.text.strip()) + + logging.debug(f"✅ Processed input: {ET.tostring(root).decode()}") + except socket.timeout: + pass # Just ignore and continue + except ConnectionResetError: + print("⚠️ Connection was reset by client. Waiting before retry...") + time.sleep(0.5) + except Exception as e: + print(f"[ERROR] Failed to process input: {e}") def generate_message(self): - """Generate a properly formatted RSI message dynamically based on the config file.""" - root = ET.Element("Rob", Type="KUKA") # ✅ Correct root element + root = ET.Element("Rob", Type="KUKA") - for tag, value in self.send_variables.items(): - if isinstance(value, dict): - element = ET.SubElement(root, tag.split(".")[0]) # Create main element - for sub_key, sub_value in value.items(): - if isinstance(sub_value, bool): - sub_value = int(sub_value) # Convert booleans to 0/1 - element.set(sub_key, f"{sub_value:.1f}" if isinstance(sub_value, float) else str(sub_value)) - else: - element = ET.SubElement(root, tag) - if isinstance(value, bool): - value = int(value) # Convert booleans to 0/1 - element.text = f"{value:.1f}" if isinstance(value, float) else str(value) + for key in ["RIst", "AIPos", "ELPos"]: + element = ET.SubElement(root, key) + for sub_key, value in self.state[key].items(): + element.set(sub_key, f"{value:.2f}") + + for key in ["DiO", "DiL"]: + ET.SubElement(root, key).text = str(self.state[key]) ET.SubElement(root, "IPOC").text = str(self.ipoc_value) - - message = ET.tostring(root, encoding="utf-8").decode() - logging.debug(f"📤 Sent XML Message:\n{message}") - print(f"📤 Sent XML Message:\n{message}") - return message + return ET.tostring(root, encoding="utf-8").decode() def send_message(self): - """Send messages every 4ms or 12ms without waiting for a response.""" while self.running: try: - message = self.generate_message() - print(f"Sending message:\n{message}") - logging.debug(f"📡 Sending message to RSI client with IPOC={self.ipoc_value}...") - print(f"📡 Sending message to RSI client with IPOC={self.ipoc_value}...") - self.udp_socket.sendto(message.encode(), self.client_address) # ✅ Send without waiting - - # ✅ Attempt to receive a response - self.udp_socket.settimeout(self.delay_ms) - try: - response, addr = self.udp_socket.recvfrom(1024) - received_message = response.decode() - - logging.debug(f"📩 Received XML Message:\n{received_message}") - print(f"📩 Received XML Message:\n{received_message}") - - except socket.timeout: - print("⚠️ No response received before next send cycle.") - - time.sleep(self.delay_ms) # ✅ Apply user-defined delay (4ms or 12ms) - self.increment_ipoc() # ✅ Increment IPOC for next message - - except ConnectionResetError: - logging.warning("⚠️ RSI Client is not responding. Retrying...") - print("⚠️ RSI Client is not responding. Retrying in 2 seconds...") - time.sleep(2) # ✅ Instead of crashing, wait and retry - + self.receive_and_process() + response = self.generate_message() + self.udp_socket.sendto(response.encode(), self.client_address) + self.ipoc_value += 4 + time.sleep(self.delay_ms) except Exception as e: - logging.error(f"❌ Unexpected error: {e}") - print(f"❌ Unexpected error: {e}") - - def increment_ipoc(self): - """Increment the IPOC value by 4 milliseconds for the next message.""" - self.ipoc_value += 4 - logging.debug(f"🔄 IPOC incremented to {self.ipoc_value}") - print(f"🔄 IPOC incremented to {self.ipoc_value}") + print(f"[ERROR] EchoServer error: {e}") + time.sleep(1) def start(self): - """Start the Echo Server message loop in a separate thread.""" self.running = True self.thread.start() def stop(self): - """Gracefully stop the Echo Server.""" print("🛑 Stopping Echo Server...") - logging.info("🛑 Stopping Echo Server...") - self.running = False # ✅ Stop the loop - self.thread.join() # ✅ Wait for the thread to finish - self.udp_socket.close() # ✅ Close the socket - logging.info("✅ Echo Server Stopped") - print("✅ Echo Server Stopped") + self.running = False + self.thread.join() + self.udp_socket.close() + print("✅ Echo Server Stopped.") if __name__ == "__main__": - config_file = "RSI_EthernetConfig.xml" - echo_server = EchoServer(config_file, delay_ms=4) + import argparse + parser = argparse.ArgumentParser(description="Run Echo Server for RSI Simulation") + parser.add_argument("--config", type=str, default="RSI_EthernetConfig.xml", help="Path to RSI config file") + parser.add_argument("--mode", type=str, choices=["relative", "absolute"], default="relative", help="Correction mode") + parser.add_argument("--delay", type=int, default=4, help="Delay between messages in ms") + + args = parser.parse_args() + server = EchoServer(config_file=args.config, delay_ms=args.delay, mode=args.mode) + try: - echo_server.start() + server.start() while True: - time.sleep(1) # Keep main thread alive + time.sleep(1) except KeyboardInterrupt: - echo_server.stop() + server.stop()