updated logging and gamepad

Author: shieldstring

L1/L1_gamepad.py

@@ -1,26 +1,63 @@
 import pygame
 import logging
-
-# Configure logging for debugging
-logging.basicConfig(level=logging.DEBUG)
+import time
 
 class Gamepad:
-    def __init__(self):
+    def __init__(self, retry_interval=1, max_retries=5):
         pygame.init()
         pygame.joystick.init()
-        self.joystick = pygame.joystick.Joystick(0)
-        self.joystick.init()
+        self.joystick = None
+        self.retry_interval = retry_interval
+        self.max_retries = max_retries
+        self._initialize_gamepad()
+
+    def _initialize_gamepad(self):
+        """Initialize gamepad with retry logic"""
+        for attempt in range(self.max_retries):
+            try:
+                if pygame.joystick.get_count() > 0:
+                    self.joystick = pygame.joystick.Joystick(0)
+                    self.joystick.init()
+                    logging.info(f"Gamepad connected: {self.joystick.get_name()}")
+                    return
+                else:
+                    logging.warning(f"No gamepad detected (attempt {attempt + 1}/{self.max_retries})")
+            except Exception as e:
+                logging.error(f"Initialization error: {str(e)}")
+            
+            time.sleep(self.retry_interval)
+        
+        raise RuntimeError("Failed to initialize gamepad after multiple attempts")
 
     def get_input(self):
-        """
-        Get input from the gamepad.
-        """
+        """Get normalized input from gamepad with deadzone handling"""
+        if not self.joystick:
+            raise RuntimeError("Gamepad not initialized")
+        
         try:
             pygame.event.pump()
-            x_axis = self.joystick.get_axis(0)  # Left stick X-axis
-            y_axis = self.joystick.get_axis(1)  # Left stick Y-axis
-            logging.debug(f"Gamepad input: x_axis={x_axis}, y_axis={y_axis}")
+            
+            # Add deadzone threshold (adjust as needed)
+            deadzone = 0.1
+            
+            # Get axis values with deadzone filtering
+            x_axis = self._apply_deadzone(self.joystick.get_axis(0), deadzone)
+            y_axis = self._apply_deadzone(self.joystick.get_axis(1), deadzone)
+            
+            logging.debug(f"Gamepad input - X: {x_axis:.2f}, Y: {y_axis:.2f}")
             return x_axis, y_axis
+            
         except Exception as e:
-            logging.error(f"An error occurred while reading gamepad input: {e}")
-            raise
+            logging.error(f"Input error: {str(e)}")
+            raise
+
+    def _apply_deadzone(self, value, threshold):
+        """Apply deadzone to axis values"""
+        if abs(value) < threshold:
+            return 0.0
+        return value
+
+    def __del__(self):
+        if self.joystick:
+            self.joystick.quit()
+        pygame.quit()

L1/L1_motor.py

@@ -17,15 +17,31 @@ def __init__(self):
             ) for pin in MOTOR_PINS
         ]
         logger.info(f"Motors ready on pins: {MOTOR_PINS}")
+        self._is_emergency_stopped = False
 
     def set_speed(self, speeds):
         """Set speeds between -1.0 (full reverse) and 1.0 (full forward)"""
         for motor, speed in zip(self.motors, speeds):
             motor.value = max(-1.0, min(1.0, speed))
         logger.debug(f"Motor speeds set: {speeds}")
 
-    def stop(self):
-        """Emergency stop all motors"""
+    def stop(self, emergency=False):
+        
+        """Stop all motors
+        Args:
+            emergency: If True, logs as warning (for unexpected stops)
+        """
+        self._is_emergency_stopped = emergency
+
         for motor in self.motors:
             motor.value = 0
-        logger.warning("Motors forcefully stopped")
+        if emergency:
+            logger.warning("Motors forcefully stopped")
+        else:
+            logger.info("Motors stopped normally")
+            
+    def status(self):
+        return {
+            "emergency_stop": self._is_emergency_stopped,
+            "speeds": [m.value for m in self.motors]
+        }

L3/L3_drive_mt.py

@@ -55,8 +55,12 @@ def start(self):
         self.control_thread.start()
         logger.info("Drive system started")
 
+    # In DriveController:
     def stop(self):
-        """Stop the drive system"""
-        self.controller.stop()
-        self.control_thread.join()
-        logger.info("Drive system stopped")
+        self._running = False
+        self.motor.stop(emergency=False)  # Normal expected stop
+
+        # In timeout check:
+        if time.time() - self._last_update > self._command_timeout:
+            self.motor.stop(emergency=True)  # Unexpected stop
+            time.sleep(0.1)