upadted branch

Author: shieldstring

.gitignore

@@ -0,0 +1,3 @@
+.venv/
+*.pyc
+__pycache__/

L1/L1_encoder.py

@@ -1,11 +1,23 @@
 import smbus2
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
 
 class Encoder:
     def __init__(self, i2c_address):
         self.bus = smbus2.SMBus(1)
         self.address = i2c_address
 
     def read_position(self):
-        # Read encoder position from I2C
-        data = self.bus.read_i2c_block_data(self.address, 0, 2)
-        return (data[0] << 8) | data[1]
+        """
+        Read the encoder position from I2C.
+        """
+        try:
+            data = self.bus.read_i2c_block_data(self.address, 0, 2)
+            position = (data[0] << 8) | data[1]
+            logging.debug(f"Encoder position: {position}")
+            return position
+        except Exception as e:
+            logging.error(f"An error occurred while reading encoder position: {e}")
+            raise

L1/L1_gamepad.py

@@ -1,4 +1,8 @@
 import pygame
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
 
 class Gamepad:
     def __init__(self):
@@ -8,7 +12,15 @@ def __init__(self):
         self.joystick.init()
 
     def get_input(self):
-        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
-        return x_axis, y_axis
+        """
+        Get input from the gamepad.
+        """
+        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}")
+            return x_axis, y_axis
+        except Exception as e:
+            logging.error(f"An error occurred while reading gamepad input: {e}")
+            raise

L1/L1_lidar.py

@@ -1,9 +1,21 @@
 import pysicktim as lidar
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
 
 class Lidar:
     def __init__(self):
         self.lidar = lidar.TiM561()
 
     def scan(self):
-        # Get LIDAR scan data
-        return self.lidar.scan()
+        """
+        Get LIDAR scan data.
+        """
+        try:
+            scan_data = self.lidar.scan()
+            logging.debug(f"LIDAR scan data: {scan_data}")
+            return scan_data
+        except Exception as e:
+            logging.error(f"An error occurred while scanning with LIDAR: {e}")
+            raise

L1/L1_motor.py

@@ -1,14 +1,57 @@
 import gpiozero
+import logging
+from gpiozero.pins.pigpio import PiGPIOFactory
+from gpiozero import Device
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
+
+# Set the pin factory to pigpio (optional, for sharing GPIO pins between processes)
+Device.pin_factory = PiGPIOFactory()
 
 class MotorController:
     def __init__(self, pins):
-        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])
+        """
+        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):
-        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]
+        """
+        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

L2/L2_kinematics.py

@@ -1,11 +1,22 @@
 import numpy as np
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
 
 class Kinematics:
     def __init__(self, wheelbase):
         self.wheelbase = wheelbase
 
     def compute_chassis_movement(self, phiL, phiR):
-        # Compute chassis movement based on wheel speeds
-        x_dot = (phiL + phiR) / 2
-        theta_dot = (phiR - phiL) / self.wheelbase
-        return x_dot, theta_dot
+        """
+        Compute chassis movement based on wheel speeds.
+        """
+        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

L2/L2_speed_control.py

@@ -1,12 +1,28 @@
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
+
 class SpeedControl:
     def __init__(self):
         self.target_speed = 0.0
 
     def set_target_speed(self, speed):
+        """
+        Set the target speed for the robot.
+        """
         self.target_speed = speed
+        logging.debug(f"Target speed set to: {speed}")
 
     def compute_duty_cycle(self, current_speed):
-        # Simple proportional control
-        error = self.target_speed - current_speed
-        duty_cycle = error * 0.5  # Proportional gain
-        return duty_cycle
+        """
+        Compute the duty cycle based on the current speed.
+        """
+        try:
+            error = self.target_speed - current_speed
+            duty_cycle = error * 0.5  # Proportional gain
+            logging.debug(f"Duty cycle computed: {duty_cycle}")
+            return duty_cycle
+        except Exception as e:
+            logging.error(f"An error occurred while computing duty cycle: {e}")
+            raise

L3/L3_drive_mt.py

@@ -1,17 +1,28 @@
 import threading
 from L1.L1_motor import MotorController
 from L2.L2_kinematics import Kinematics
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
 
 class DriveController:
     def __init__(self):
-        self.motor_controller = MotorController([11, 12, 15, 16])
+        self.motor_controller = MotorController([17, 18, 22, 23])
         self.kinematics = Kinematics(0.405)  # Wheelbase in meters
 
     def drive(self, x_dot, theta_dot):
-        # Compute wheel speeds and set motor duty cycles
-        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])
+        """
+        Drive the robot based on desired x_dot and theta_dot.
+        """
+        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}")
+        except Exception as e:
+            logging.error(f"An error occurred while driving: {e}")
+            raise
 
 # Multithreading example
 drive_controller = DriveController()

main.py

@@ -1,8 +1,15 @@
 from L3.L3_mission_control import MissionControl
+import logging
+
+# Configure logging for debugging
+logging.basicConfig(level=logging.DEBUG)
 
 def main():
-    mission_control = MissionControl()
-    mission_control.start_mission()
+    try:
+        mission_control = MissionControl()
+        mission_control.start_mission()
+    except Exception as e:
+        logging.error(f"An error occurred: {e}")
 
 if __name__ == "__main__":
     main()

utils/logger.py

@@ -8,4 +8,9 @@ def __init__(self, filename="log.csv"):
     def log_data(self, data):
         with open(self.filename, "a") as file:
             writer = csv.writer(file)
-            writer.writerow([datetime.now()] + data)
+            writer.writerow([datetime.now()] + data)
+
+            import logging
+
+def setup_logger():
+    logging.basicConfig(level=logging.DEBUG)