added Proper initialization sequences, Detailed Logging and Debuging messages

Author: shieldstring

L1/L1_motor.py

@@ -1,57 +1,31 @@
 import gpiozero
-import logging
 from gpiozero.pins.pigpio import PiGPIOFactory
-from gpiozero import Device
-
-# Configure logging for debugging
-logging.basicConfig(level=logging.DEBUG)
+from utils.constants import MOTOR_PINS, PWM_FREQUENCY
+import logging
 
-# Set the pin factory to pigpio (optional, for sharing GPIO pins between processes)
-Device.pin_factory = PiGPIOFactory()
+logger = logging.getLogger(__name__)
 
 class MotorController:
-    def __init__(self, pins):
-        """
-        Initialize the MotorController with a list of GPIO pins.
-        Each pin should be unique to avoid conflicts.
-        """
-        # Ensure that no pin conflicts exist and that each pin is used once
-        if len(set(pins)) != len(pins):
-            raise ValueError("Pins must be unique. Duplicate pins found.")
-
-        logging.debug(f"Initializing MotorController with pins: {pins}")
-
-        try:
-            # Initialize PWMOutputDevice for each motor
-            self.motor_a = gpiozero.PWMOutputDevice(pins[0])
-            self.motor_b = gpiozero.PWMOutputDevice(pins[1])
-            self.motor_c = gpiozero.PWMOutputDevice(pins[2])
-            self.motor_d = gpiozero.PWMOutputDevice(pins[3])
-
-            logging.debug("MotorController initialized successfully.")
-        except gpiozero.GPIOPinInUse as e:
-            logging.error(f"GPIO pin conflict detected: {e}")
-            raise
-        except Exception as e:
-            logging.error(f"An error occurred while initializing motors: {e}")
-            raise
-
-    def set_speed(self, duty_cycle):
-        """
-        Set the speed of all motors using a list of duty cycles.
-        :param duty_cycle: List of duty cycles for each motor (e.g., [0.5, 0.5, 0.5, 0.5])
-        """
-        if len(duty_cycle) != 4:
-            raise ValueError("Duty cycle list must contain exactly 4 values.")
-
-        try:
-            # Set the duty cycle for each motor
-            self.motor_a.value = duty_cycle[0]
-            self.motor_b.value = duty_cycle[1]
-            self.motor_c.value = duty_cycle[2]
-            self.motor_d.value = duty_cycle[3]
-
-            logging.debug(f"Motor speeds set to: {duty_cycle}")
-        except Exception as e:
-            logging.error(f"An error occurred while setting motor speeds: {e}")
-            raise
+    def __init__(self):
+        """Initialize with pigpio for hardware PWM"""
+        gpiozero.Device.pin_factory = PiGPIOFactory()
+        self.motors = [
+            gpiozero.PWMOutputDevice(
+                pin=pin,
+                frequency=PWM_FREQUENCY,
+                initial_value=0
+            ) for pin in MOTOR_PINS
+        ]
+        logger.info(f"Motors ready on pins: {MOTOR_PINS}")
+
+    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"""
+        for motor in self.motors:
+            motor.value = 0
+        logger.warning("Motors forcefully stopped")

L2/L2_kinematics.py

@@ -1,22 +1,46 @@
+from utils.constants import WHEELBASE
 import numpy as np
 import logging
 
-# Configure logging for debugging
-logging.basicConfig(level=logging.DEBUG)
+logger = logging.getLogger(__name__)
 
 class Kinematics:
-    def __init__(self, wheelbase):
+    def __init__(self, wheelbase=WHEELBASE):
+        """Initialize with wheelbase from constants.py"""
         self.wheelbase = wheelbase
+        logger.info(f"Kinematics initialized with wheelbase: {self.wheelbase}m")
 
-    def compute_chassis_movement(self, phiL, phiR):
+    def compute_chassis_speeds(self, phi_l, phi_r):
         """
-        Compute chassis movement based on wheel speeds.
+        Convert wheel speeds to chassis movement
+        phi_l, phi_r: left/right wheel speeds in rad/s
+        Returns: (x_dot, theta_dot) in m/s and rad/s
         """
-        try:
-            x_dot = (phiL + phiR) / 2
-            theta_dot = (phiR - phiL) / self.wheelbase
-            logging.debug(f"Chassis movement: x_dot={x_dot}, theta_dot={theta_dot}")
-            return x_dot, theta_dot
-        except Exception as e:
-            logging.error(f"An error occurred while computing chassis movement: {e}")
-            raise
+        x_dot = (phi_l + phi_r) / 2
+        theta_dot = (phi_r - phi_l) / self.wheelbase
+        logger.debug(f"Computed chassis speeds: x_dot={x_dot:.2f}m/s, theta_dot={theta_dot:.2f}rad/s")
+        return x_dot, theta_dot
+
+    def compute_wheel_speeds(self, x_dot, theta_dot):
+        """
+        Convert chassis commands to wheel speeds
+        x_dot: desired forward speed (m/s)
+        theta_dot: desired angular speed (rad/s)
+        Returns: [left_speed, right_speed] in rad/s
+        """
+        phi_l = x_dot - (theta_dot * self.wheelbase) / 2
+        phi_r = x_dot + (theta_dot * self.wheelbase) / 2
+        logger.debug(f"Computed wheel speeds: left={phi_l:.2f}rad/s, right={phi_r:.2f}rad/s")
+        return [phi_l, phi_r]
+
+    def compute_motor_commands(self, x_dot, theta_dot):
+        """
+        Directly compute motor duty cycles (-1 to 1)
+        Returns: [left_duty, right_duty, left_duty, right_duty]
+        """
+        phi_l, phi_r = self.compute_wheel_speeds(x_dot, theta_dot)
+        # Scale to motor duty cycles (assuming MAX_SPEED from constants)
+        from utils.constants import MAX_SPEED
+        left_duty = phi_l / MAX_SPEED
+        right_duty = phi_r / MAX_SPEED
+        return [left_duty, right_duty, left_duty, right_duty]

L3/L3_drive_mt.py

@@ -1,37 +1,48 @@
-import threading
 from L1.L1_motor import MotorController
 from L2.L2_kinematics import Kinematics
+from utils.constants import WHEELBASE
 import logging
+import time
 
-# Configure logging for debugging
-logging.basicConfig(level=logging.DEBUG)
+logger = logging.getLogger(__name__)
 
 class DriveController:
     def __init__(self):
-        self.motor_controller = MotorController([17, 18, 22, 23])
-        self.kinematics = Kinematics(0.405)  # Wheelbase in meters
+        self.motor = MotorController()
+        self.kinematics = Kinematics(WHEELBASE)
+        self._running = False
 
-    def drive(self, x_dot, theta_dot):
-        """
-        Drive the robot based on desired x_dot and theta_dot.
-        """
+        # safety variables 
+        self._last_update = 0
+        self._command_timeout = 0.5  # seconds
+        
+        logger.info("Drive controller initialized")
+
+    def driving_thread(self):
+        """Main control loop with safety checks"""
+        self._running = True
         try:
-            phiL = x_dot - theta_dot * self.kinematics.wheelbase / 2
-            phiR = x_dot + theta_dot * self.kinematics.wheelbase / 2
-            self.motor_controller.set_speed([phiL, phiR, phiL, phiR])
-            logging.debug(f"Driving with x_dot={x_dot}, theta_dot={theta_dot}")
+            while self._running:
+                 # Auto-stop if no recent commands
+                if time.time() - self._last_update > self._command_timeout:
+                    self.motor.stop()
+                    time.sleep(0.1)
+                    continue
+                
+                # Get commands from L2 systems
+                x_dot, theta_dot = self._get_commands()  # Implement your logic
+                
+                # Convert to wheel speeds
+                speeds = self.kinematics.compute_wheel_speeds(x_dot, theta_dot)
+                         
+                # Drive motors
+                self.motor.set_speed(speeds)
+                                
         except Exception as e:
-            logging.error(f"An error occurred while driving: {e}")
+            logger.error(f"Control error: {e}", exc_info=True)
             raise
-
-# Multithreading example
-drive_controller = DriveController()
-
-def driving_thread():
-    while True:
-        # Get x_dot and theta_dot from gamepad or autonomous logic
-        x_dot, theta_dot = 0.5, 0.1  # Example values
-        drive_controller.drive(x_dot, theta_dot)
-
-thread = threading.Thread(target=driving_thread)
-thread.start()
+        finally:
+            self.motor.stop()
+        
+    def stop(self):
+        self._running = False

utils/constants.py

@@ -1,3 +1,7 @@
 # Constants for the SCUTTLE robot
-WHEELBASE = 0.405  # Wheelbase in meters
-MAX_SPEED = 1.0    # Maximum speed in m/s
+WHEELBASE = 0.405  # Distance between wheels in meters
+MAX_SPEED = 0.5    # Maximum speed in m/s
+
+# Motor Configuration
+MOTOR_PINS = [17, 18, 22, 23]  # GPIO pins for motors 1-4
+PWM_FREQUENCY = 1000            # PWM frequency in Hz

utils/logger.py

@@ -1,6 +1,6 @@
 import csv
 from datetime import datetime
-
+import logging
 class Logger:
     def __init__(self, filename="log.csv"):
         self.filename = filename
@@ -10,7 +10,9 @@ def log_data(self, data):
             writer = csv.writer(file)
             writer.writerow([datetime.now()] + data)
 
-            import logging
 
 def setup_logger():
-    logging.basicConfig(level=logging.DEBUG)
+    logging.basicConfig(
+        level=logging.INFO,
+        format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
+    )