diff --git a/src/ColorSensorTests.py b/src/ColorSensorTests.py
new file mode 100644
index 0000000..3d5a8d9
--- /dev/null
+++ b/src/ColorSensorTests.py
@@ -0,0 +1,69 @@
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.hubs import PrimeHub
+
+# Initialize hub and devices
+hub = PrimeHub()
+
+color_sensor = ColorSensor(Port.F)
+
+# Color Settings
+# https://docs.pybricks.com/en/latest/parameters/color.html
+print("Default Detected Colors:", color_sensor.detectable_colors())
+
+# Custom color Hue, Saturation, Brightness value for Lego bricks
+Color.MAGENTA = Color(315,100,60)
+Color.BLUE = Color(240,100,100)
+Color.CYAN = Color(180,100,100)
+Color.RED = Color(350, 100, 100)
+LEGO_BRICKS_COLOR = [
+    Color.BLUE, 
+    Color.GREEN, 
+    Color.WHITE,
+    Color.RED, 
+    Color.YELLOW, 
+    Color.MAGENTA, 
+    Color.NONE
+]
+magenta_counter = 0
+stable_color = None
+real_color = None
+#Update Detectable colors
+color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
+print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}')
+print("Updated Detected Colors:", color_sensor.detectable_colors())
+async def main():  
+    while True:
+        global magenta_counter, stable_color, real_color
+        color_reflected_percent = await color_sensor.reflection()
+        print("Reflection: ", color_reflected_percent)
+        if color_reflected_percent > 15:
+            color_detected = await color_sensor.color()
+
+            if color_detected == Color.MAGENTA:
+                magenta_counter += 1
+            else:
+                magenta_counter = 0
+                stable_color = color_detected
+
+            # Only accept magenta if it's been stable for a while - usually triggers before other colors so we gotta do this :|
+            if magenta_counter > 10:
+                stable_color = Color.MAGENTA
+                if stable_color != Color.MAGENTA:
+                    stable_color = await color_sensor.color()
+
+            real_color = stable_color
+            #if(color_detected != Color.NONE):
+            #    return
+            
+            print("Magenta counter: ", magenta_counter)
+            if real_color is not None:
+                print(f'Detected color: {real_color} : {real_color.h}, {real_color.s}, {real_color.v}')
+            else:
+                print("No valid color detected yet.")
+        await wait(50)
+
+run_task(main())
\ No newline at end of file
diff --git a/src/Diagnostics.py b/src/Diagnostics.py
new file mode 100644
index 0000000..07d7766
--- /dev/null
+++ b/src/Diagnostics.py
@@ -0,0 +1,33 @@
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.hubs import PrimeHub
+
+# Initialize hub and devices
+hub = PrimeHub()
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
+left_arm = Motor(Port.C, Direction.CLOCKWISE) # Specify default direction
+right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
+lazer_ranger = UltrasonicSensor(Port.E)
+color_sensor = ColorSensor(Port.F)
+
+# DriveBase configuration
+WHEEL_DIAMETER = 68.8  # mm (adjust for your wheels)
+AXLE_TRACK = 180  # mm (distance between wheels)
+drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
+drive_base.settings(600, 500, 300, 200)
+drive_base.use_gyro(True)
+
+WALL_DISTANCE = 200  # mm
+
+if hub.battery.voltage() > 7800:
+    print(f"Battery voltage is sufficient: {hub.battery.voltage()}")
+elif hub.battery.voltage < 7800 :
+    print(f"Charging needed: {hub.battery.voltage()}")
+
+print(hub.battery.current())
+print(hub.imu.heading())
+print(hub.system.info())
\ No newline at end of file