Hardware Connection fix

Robot.py

@@ -59,8 +59,11 @@ 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'):
-            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()
 
 
 class PyBulletBackend:
@@ -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,12 +209,12 @@ 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()
+        self.step_sim()
 
     def step_kinematic_gait(self, vx: float, vy: float, omega: float) -> None:
         """Procedural Tripod Gait solver."""
@@ -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."""