Hardware Connection fix

ArduinoCommunication.py

@@ -19,6 +19,9 @@ class ArduinoCommunication(Thread):
         self.running = Event()
         self.running.set()
 
+    def send_motion(self, radial_array):
+        self.write(radial_array)
+
     def run(self):
         while self.running.is_set():
             # 1. Befehle senden
@@ -70,5 +73,12 @@ class ArduinoCommunication(Thread):
 
     def stop(self):
         self.running.clear()
-        self.join()
+        try:
+            self.join(timeout=0.5)
+        except RuntimeError:
+            pass
+        if self.serial_conn and self.serial_conn.is_open:
             self.serial_conn.close()
+
+    def close(self):
+        self.stop()

EspCommunication.py

@@ -25,8 +25,12 @@ HEADER_FMT  = "<B B H I" # ID, Ver, Len, Timestamp
 TLV_FMT = "<B H"
 
 class ESP32Communication(Thread):
-    def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port):
+    def __init__(self, host=None, port=None, ip=None):
         super().__init__(daemon=True)
+        if ip is not None:
+            host = ip
+        host = cfg.esp32_ip if host is None else host
+        port = cfg.esp32_port if port is None else port
         self.addr = (host, port)
 
         # UDP Socket - Zero Lag
@@ -126,4 +130,10 @@ class ESP32Communication(Thread):
 
     def stop(self):
         self.running.clear()
+        try:
             self.sock.close()
+        except Exception:
+            pass
+
+    def close(self):
+        self.stop()

Robot.py

@@ -59,7 +59,10 @@ class HardwareBackend:
         return [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]
 
     def cleanup(self) -> None:
-        if self.comm_channel and hasattr(self.comm_channel, 'close'):
+        if self.comm_channel:
+            if hasattr(self.comm_channel, 'stop'):
+                self.comm_channel.stop()
+            elif hasattr(self.comm_channel, 'close'):
                 self.comm_channel.close()
 
 
@@ -123,9 +126,11 @@ class Robot:
                 self.backend: RobotBackend = PyBulletBackend(sim_instance)
             elif backend_type == BackendType.ESP32:
                 comm = ESP32Communication(ip=cfg.esp32_ip, port=cfg.esp32_port)
+                comm.start()
                 self.backend = HardwareBackend(comm)
             elif backend_type == BackendType.ARDUINO:
                 comm = ArduinoCommunication(port=cfg.port, baudrate=cfg.baudrate)
+                comm.start()
                 self.backend = HardwareBackend(comm)
         else:
             self.backend = backend_type
@@ -188,7 +193,11 @@ class Robot:
             self.current_state = STATE_REGISTRY[next_state_key]
             self.current_state.enter(self)
 
-    def tick(self, action: Optional[np.ndarray] = None, physics_substeps: int = 4) -> None:
+    def tick(self, action: Optional[np.ndarray] = None) -> None:
+        """
+        Unified control loop tick.
+        Processes commands through direct RL, residual RL, or State Machine kinematics.
+        """
         vx, vy, omega = self.vector_dirmov
 
         if self.mode == "direct":
@@ -200,11 +209,11 @@ class Robot:
             if action is not None:
                 self.apply_rl_action_delta(action)
 
-        else:  # kinematics mode
-            self.step_kinematic_gait(vx, vy, omega)
+        else:  # "kinematics" / standard State Machine execution
+            next_state_key = self.current_state.execute(self)
+            if next_state_key:
+                self.transition_to(next_state_key)
 
-        # Step PyBullet engine sub-steps to allow physics actuation
-        for _ in range(physics_substeps):
         self.step_sim()
 
     def step_kinematic_gait(self, vx: float, vy: float, omega: float) -> None:
@@ -261,17 +270,9 @@ class Robot:
         self.set_joint_angles(target_rad)
 
     def apply_rl_action(self, action: np.ndarray) -> None:
-        """
-        Applies direct RL joint action deltas to the current joint positions.
-        """
-        action_flat = np.asarray(action, dtype=np.float32).flatten()
-        
-        # Keep internal memory updated in (6, 3) format for Kinematic/IK math
-        self.current_rad = dt.RadArray(data=action_flat.reshape(6, 3))
-
-        # Send target positions to the PyBullet backend
-        if self.backend:
-            self.backend.send_angles(self.current_rad)
+        action = np.asarray(action, dtype=np.float32)
+        new_rad = dt.RadArray(data=action.reshape(self.current_rad.data.shape))
+        self.set_joint_angles(new_rad)
 
     def apply_rl_action_delta(self, action: np.ndarray) -> None:
         """Applies action deltas on top of joint state for Residual RL."""

config.py

@@ -38,6 +38,14 @@ class RobotConfig:
     tick_rate_hz: float = 25.0         # Motion loop execution rate [ticks/sec]
     step_duration: float = 0.8         # Time to complete full gait stride [s]
 
+    @property
+    def standard_tickpersec(self) -> float:
+        return self.tick_rate_hz
+
+    @standard_tickpersec.setter
+    def standard_tickpersec(self, value: float) -> None:
+        self.tick_rate_hz = value
+
     @property
     def tick_duration(self) -> float:
         return 1.0 / self.tick_rate_hz