Upload files to "/"

Test new

README.md

@@ -1,3 +1,25 @@
-# solutions_season_unearthed
+# 65266 Lego Dynamics - UNEARTHED Season Repository
 
-Pybricks code solving missions in Season UNEARTHED
+## Project Overview
+
+This repository contains the Pybricks code for Team 65266 Lego Dynamics' robot for the UNEARTHED season.
+
+## Robot Hardware
+
+*   **Robot Name:** Optimus Prime III
+*   **Robot Firmware:** PyBricks firmware
+*   **Motors:** Two large motors for attachments, C left, D right, Two small motors for drive, A left, B right
+*   **Sensors:** visible up-facing color sensor for quick starts
+*   **Attachments:** Lots of 'em
+
+## Code Structure
+
+Files are the different runs we do, with each run consisting of one or multiple mission completions. Another script combines these files into a "master" file, which then adds the color-sensor-to-start logic.
+
+## How to Use
+
+Load the master file into PyBricks, then send it over to the robot. Then you hold a color LEGO up to the sensor to start it.
+
+## License
+GNU General Public License
+You can take inspiration from our code, but you can't take our exact code.

config.py

@@ -0,0 +1,40 @@
+# config.py - Robot configuration shared across all modules
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
+from pybricks.parameters import Port
+
+# Initialize hub
+hub = PrimeHub()
+
+# Robot hardware configuration
+ROBOT_CONFIG = {
+    # Drive motors
+    'left_motor': Motor(Port.A),
+    'right_motor': Motor(Port.B),
+    
+    # Attachment motors
+    'attachment_motor': Motor(Port.C),
+    'lift_motor': Motor(Port.D),
+    
+    # Sensors
+    #'color_left': ColorSensor(Port.E),
+    'color_back': ColorSensor(Port.F),
+    'ultrasonic': UltrasonicSensor(Port.E),
+    
+    # Hub reference
+    'hub': hub
+}
+
+# Speed and distance constants
+SPEEDS = {
+    'FAST': 400,
+    'NORMAL': 250,
+    'SLOW': 100,
+    'PRECISE': 50
+}
+
+DISTANCES = {
+    'TILE_SIZE': 300,  # mm per field tile
+    'ROBOT_LENGTH': 180,  # mm
+    'ROBOT_WIDTH': 140    # mm
+}

members/atharv.txt

@@ -0,0 +1,3 @@
+About Me
+
+I am Atharv, the Git manager for the team. I like to produce music.

missions/Boat.py

@@ -0,0 +1,27 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+from pybricks.tools import run_task,multitask
+
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
+right_arm = Motor(Port.D)
+
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
+
+drive_base.settings(550,700,100,100)
+
+drive_base.use_gyro(True)
+
+first_run = False
+
+async def main():
+    await drive_base.straight(750)
+    await drive_base.straight(-650)
+    
+run_task(main())

missions/Heavy lifting.py

@@ -0,0 +1,37 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+from pybricks.tools import run_task, multitask
+
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+
+left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
+right_arm = Motor(Port.D)
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
+drive_base.settings(600,500,300,200)
+drive_base.use_gyro(True)
+
+async def main():
+    await right_arm.run_angle(2000,1000)
+
+    await drive_base.straight(200)
+    await drive_base.turn(-20)
+    await drive_base.straight(525)
+    await drive_base.turn(60)
+    await drive_base.straight(30)
+
+    await right_arm.run_angle(2000,-1000)
+    await drive_base.straight(30)
+    await right_arm.run_angle(3000,1000)
+    await drive_base.straight(-60)
+
+    await drive_base.turn(-60)
+    await drive_base.straight(-525)
+    await drive_base.turn(20)
+    await drive_base.straight(-200)
+    

missions/Lift.py

@@ -0,0 +1,34 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+from pybricks.tools import run_task, multitask
+
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+
+left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
+right_arm = Motor(Port.D)
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
+drive_base.settings(600,500,300,200)
+drive_base.use_gyro(True)
+
+async def main():
+    await drive_base.straight(200)
+    await drive_base.turn(-20)
+    await drive_base.straight(525)
+    await drive_base.turn(60)
+
+    await drive_base.straight(50)
+    await right_arm.run_angle(2000,1000)
+    await drive_base.straight(-50)
+    await drive_base.turn(45)
+    await drive_base.straight(50)
+    await right_arm.run_angle(350,-1000)
+
+    await drive_base.turn(-100)
+    await drive_base.straight(-600)
+run_task(main())

missions/M8_5.py

@@ -0,0 +1,49 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+from pybricks.tools import run_task, multitask
+
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+
+left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
+right_arm = Motor(Port.D)
+
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
+
+drive_base.settings(600,500,300,200)
+drive_base.use_gyro(True)
+
+async def main():
+    left_arm.run_angle(1000, 300)
+    right_arm.run_angle(1000,500)
+    await drive_base.straight(320)
+
+    await right_arm.run_angle(5000,-500, Stop.HOLD)
+    await right_arm.run_angle(5000,500, Stop.HOLD)
+    await right_arm.run_angle(5000,-500, Stop.HOLD)
+    await right_arm.run_angle(5000,500, Stop.HOLD)
+    await right_arm.run_angle(5000,-500, Stop.HOLD)
+
+    await drive_base.turn(-20)
+    await drive_base.straight(275)
+    await drive_base.turn(20)
+    await drive_base.straight(63)
+
+    await drive_base.turn(-30)
+    right_arm.run_angle(50,500)
+    await drive_base.turn(45)
+    await drive_base.straight(-135)
+    await drive_base.turn(-60)
+    await drive_base.straight(90)
+    await left_arm.run_angle(1000,-450)
+    await drive_base.straight(-145)
+    await left_arm.run_angle(1000,450)
+    await drive_base.straight(10)
+    await drive_base.turn(35)
+    await drive_base.straight(-500)
+run_task(main())

missions/Sand Mission.py

@@ -0,0 +1,29 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+from pybricks.tools import run_task,multitask
+
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
+right_arm = Motor(Port.D)
+
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
+
+drive_base.settings(400,500,100,100)
+
+drive_base.use_gyro(True)
+
+
+async def main():
+    await drive_base.straight(420)
+    await right_arm.run_angle(300,-100)
+    await drive_base.straight(-100)
+    await right_arm.run_angle(300, 100)
+    await drive_base.straight(-350)
+    
+run_task(main())

missions/Send_Over.py

@@ -0,0 +1,31 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+from pybricks.tools import run_task, multitask
+
+hub = PrimeHub()
+
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+
+left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
+right_arm = Motor(Port.D)
+
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
+
+drive_base.settings(600,500,300,200)
+drive_base.use_gyro(True)
+
+async def main():
+
+    await drive_base.straight(915)
+    await drive_base.turn(-90)
+    await drive_base.straight(60)
+    await left_arm.run_angle(10000,-15000)
+    await drive_base.straight(-60)
+    await drive_base.turn(85)
+    await drive_base.straight(2000)
+    
+run_task(main())

missions/mission_09.py

@@ -0,0 +1,43 @@
+# ---JOHANNES---
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+
+hub = PrimeHub()
+
+# Initialize both motors. In this example, the motor on the
+# left must turn counterclockwise to make the robot go forward.
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+
+arm_motor = Motor(Port.E, Direction.CLOCKWISE)
+arm_motor.run_angle(299,90, Stop.HOLD)
+# Initialize the drive base. In this example, the wheel diameter is 56mm.
+# The distance between the two wheel-ground contact points is 112mm.
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=140)
+
+print('The default settings are: ' + str(drive_base.settings()))
+drive_base.settings(100,1000,166,750)
+# Optionally, uncomment the line below to use the gyro for improved accuracy.
+drive_base.use_gyro(True)
+
+async def solveM9():
+    print("Solving Mission 9")
+    await drive_base.turn(45)
+    await drive_base.straight(260)
+    await arm_motor.run_angle(500,-500, Stop.HOLD)
+    await drive_base.straight(-40)
+    await drive_base.turn(92)
+    await drive_base.straight(-120)
+    await drive_base.straight(220)
+    await arm_motor.run_angle(500,100, Stop.HOLD)
+    await drive_base.turn(-50)
+    await drive_base.straight(-600)
+async def main():
+    await drive_base.straight(50)
+    print("Hello, Robot is starting to run.")
+    await solveM9()
+
+run_task(main())

utils/base_robot_classes.py

@@ -0,0 +1,145 @@
+# --- VICKRAM's CODE ---
+# To do - make another version of this using DriveBase
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor
+from pybricks.parameters import Port, Stop
+from pybricks.tools import wait, StopWatch, multitask
+from umath import pi, sin, cos, radians
+import asyncio
+
+# Keep these---------------------------------------------------
+class Tracker:
+    def __init__(self, starting_pos_x, starting_pos_y, starting_angle):
+        self.position = [starting_pos_x, starting_pos_y]
+        self.angle = starting_angle
+    def update(self, straight_distance, delta_angle):
+        delta_angle = radians(delta_angle) # Convert to radians
+        self.position[0] += straight_distance * cos(delta_angle) # Calculate x coordinate
+        self.position[1] += straight_distance * sin(delta_angle) # Calculate y coordinate
+        self.angle += delta_angle
+
+    def get_state():
+        return self.position, self. angle
+
+class Attachment:
+    def __init__(self, port, start_angle=0):
+        self.motor = Motor(port)
+        self.start_angle = start_angle
+    def move(self, degrees, speed):
+        self.motor.reset_angle(0)
+        target_angle = degrees
+        tolerance = 2
+        # Movement with timeout
+        movement_timer = StopWatch()
+        movement_timeout = 3000  # 3 seconds timeout
+        while movement_timer.time() < movement_timeout:
+            current_angle = self.motor.angle()
+            error = target_angle - current_angle
+            if abs(error) <= tolerance:
+                self.motor.stop()
+                break
+            if error > 0:
+                self.motor.run(speed)
+            else:
+                self.motor.run(-speed)
+            wait(5)
+        self.motor.stop()
+        self.motor.reset_angle(0)
+
+    def reset(self):
+        self.motor.reset_angle(0)
+        self.move(self.start_angle)
+
+# Initialize hub and motors
+hub = PrimeHub()
+left_motor = Motor(Port.C)
+right_motor = Motor(Port.A)
+hub.imu.reset_heading(0)
+tracker = Tracker(0, 0, 0)
+
+# Make sure to measure robot
+def straight(distance, speed):  # Distance in millimeters
+    target_heading = hub.imu.heading()
+    # Reset distance tracking
+    left_motor.reset_angle(0)
+    right_motor.reset_angle(0)
+    # Calculate target distance in motor degrees
+    wheel_circumference = pi * 62.4
+    target_degrees = abs(distance) / wheel_circumference * 360
+    while True:
+        # Check current distance traveled - Fixed: Make left_motor abs negative
+        left_angle = -abs(left_motor.angle())
+        right_angle = abs(right_motor.angle())
+        average_angle = (left_angle + right_angle) / 2
+        # Stop if it reached the target
+        if abs(average_angle) >= target_degrees:
+            break
+        # Get heading error for correction
+        current_heading = hub.imu.heading()
+        heading_error = target_heading - current_heading
+        # Handle wraparound at 0°/360°
+        if heading_error > 180:
+            heading_error -= 360
+        if heading_error < -180:
+            heading_error += 360
+        # Calculate motor speeds with correction
+        direction = 1 if distance > 0 else -1
+        correction = heading_error * 2.0
+        left_speed = -direction * (speed + correction)
+        right_speed = direction * (speed - correction)
+        # Limit speeds to prevent excessive values
+        left_speed = max(-200, min(200, left_speed))
+        right_speed = max(-200, min(200, right_speed))
+        # Apply speeds to motors
+        left_motor.run(left_speed)
+        right_motor.run(right_speed)
+        wait(10)
+    # Stop motors
+    left_motor.stop()
+    right_motor.stop()
+    wait(20)
+    tracker.update(distance, 0)
+    print(tracker.get_position())
+
+def turn(theta, speed):  # Negative value is left and positive is right
+    start_angle = hub.imu.heading()
+    target_angle = theta + start_angle
+    # Normalize target angle to -180 to 180 range
+    if target_angle > 180:
+        target_angle -= 360
+    if target_angle < -180:
+        target_angle += 360
+    while True:
+        current_angle = hub.imu.heading()
+        # Calculate angle error (how much we still need to turn)
+        angle_error = target_angle - current_angle
+        # Handle wraparound for shortest path
+        if angle_error > 180:
+            angle_error -= 360
+        if angle_error < -180:
+            angle_error += 360
+        # Stop if we're close enough (within 2 degrees)
+        if abs(angle_error) < 2:
+            break
+        # Determine turn direction and speed based on error
+        if angle_error > 0:
+            # Turn right
+            left_motor.run(-speed)
+            right_motor.run(-speed)
+        else:
+            # Turn left
+            left_motor.run(speed)
+            right_motor.run(speed)
+        wait(20)
+    # Stop both motors
+    left_motor.stop()
+    right_motor.stop()
+    trcker.update(0, theta)
+    print(tracker.get_position())
+# Setup attachments here-------------------------------
+attachment1 = Attachment(Port.D)
+attachment2 = Attachment(Port.B)
+# Run code goes here-----------------------------------
+async def main():
+#------------------------------------------------------
+run_task(main())

utils/color_sensor_navi.py

@@ -0,0 +1,41 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+
+hub = PrimeHub()
+
+# Robot hardware configuration
ROBOT_CONFIG = {
    # Drive motors
    'left_motor': Motor(Port.A),
    'right_motor': Motor(Port.B),
    
    # Attachment motors
    'attachment_motor': Motor(Port.C),
    'lift_motor': Motor(Port.D),
    
    # Sensors
    'color_left': ColorSensor(Port.E),
    'color_right': ColorSensor(Port.F),
    'ultrasonic': UltrasonicSensor(Port.S1),
    
    # Hub reference
    'hub': hub
}

# Speed and distance constants
SPEEDS = {
    'FAST': 400,
    'NORMAL': 250,
    'SLOW': 100,
    'PRECISE': 50
}

DISTANCES = {
    'TILE_SIZE': 300,  # mm per field tile
    'ROBOT_LENGTH': 180,  # mm
    'ROBOT_WIDTH': 140    # mm
}
+
+
+def mission_run_1():
+    print('Running missions in Run 1')
+
+def mission_run_2():
+    print('Running missions in Run 2')
+
+def mission_run_3():
+    print('Running missions in Run 3')
+
+# In main.py - sensor-based navigation
+def main(self):
+    """Use color sensor to select runs"""
+    print("Place colored object in front of sensor:")
+    print("RED=Run1, GREEN=Run2, BLUE=Run3, YELLOW=Test")
+    while True:
+        color = ROBOT_CONFIG['color_left'].color()
+        if color == Color.RED:
+            mission_run_1()
+            break
+        elif color == Color.GREEN:
+            mission_run_2()
+            break
+        elif color == Color.BLUE:
+            mission_run_3()
+            break
+        elif color == Color.YELLOW:
+            self.test_mode()
+            break
+        wait(1000)
+main()

utils/combine_runs.py

@@ -0,0 +1,139 @@
+# Guys please use the same setup code and put into the imports for consistency
+script_names = ["untitled14.py", "untitled13.py"]  # This is a list of the files of the mission runs
+content = ""
+imports = """
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor
+from pybricks.parameters import Port, Stop, Color, Direction
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch, multitask, run_task
+import asyncio
+
+hub = PrimeHub()
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+atarm1 = Motor(Port.E, Direction.COUNTERCLOCKWISE)
+atarm2 = Motor(Port.F)
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112)
+color_sensor = ColorSensor(Port.C)
+
+drive_base.settings(300, 500, 300, 200)
+Color.ORANGE = Color(10, 100, 100)
+Color.MAGENTA = Color(321, 100, 86)
+
+"""
+
+def extract_main_function(content):
+    lines = content.split('\n')
+    main_content = []
+    in_main_function = False
+    main_indent = 0
+    is_async = False
+    
+    for line in lines:
+        stripped_line = line.strip()
+        
+        # Find the start of main function
+        if stripped_line.startswith('def main') or stripped_line.startswith('async def main'):
+            in_main_function = True
+            is_async = stripped_line.startswith('async def main')
+            continue
+        
+        if in_main_function:
+            # If we hit another function or class definition at the same level, we're done
+            if stripped_line and not line.startswith(' ') and not line.startswith('\t'):
+                if stripped_line.startswith('def ') or stripped_line.startswith('class '):
+                    break
+            
+            # Skip the first line after def main() if it's empty
+            if not stripped_line and not main_content:
+                continue
+                
+            # If this is the first content line, determine the indent level
+            if main_content == [] and stripped_line:
+                main_indent = len(line) - len(line.lstrip())
+            
+            # Remove the main function's indentation
+            if line.strip():  # Don't process empty lines
+                if len(line) - len(line.lstrip()) >= main_indent:
+                    main_content.append(line[main_indent:])
+                else:
+                    main_content.append(line)
+            else:
+                main_content.append('')  # Keep empty lines
+    
+    return '\n'.join(main_content), is_async
+
+# Clear the main.py file and write the required imports
+with open("main.py", 'w') as required_imports:
+    required_imports.write(imports)
+
+function_calls = []
+
+# Define colors properly - one per script
+colors = [
+    'Color.ORANGE', 'Color.GREEN', 'Color.BLACK', 'Color.WHITE', 
+    'Color.YELLOW', 'Color.BLUE', 'Color.MAGENTA', 'Color.RED', 'Color.BROWN'
+]
+
+# Process each script file and create individual functions
+for i, f_name in enumerate(script_names):
+    try:
+        with open(f_name, 'r') as f:
+            content = f.read()
+            # Extract only the main function content
+            main_function_content, is_async = extract_main_function(content)
+            
+            if main_function_content.strip():  # Only proceed if it found main function content
+                func_name = f_name.replace('.py', '').replace('-', '_')
+                
+                func_def = f"\n{'async ' if is_async else ''}def {func_name}():\n"
+                
+                indented_content = '\n'.join(['    ' + line if line.strip() else line for line in main_function_content.split('\n')])
+                func_def += indented_content + "\n"
+                
+                with open("main.py", 'a') as m:
+                    m.write(func_def)
+                
+                # Assign one color per script
+                color_condition = colors[i % len(colors)]
+                function_calls.append({
+                    'name': func_name,
+                    'is_async': is_async,
+                    'color': color_condition,
+                    'filename': f_name
+                })
+                
+            else:
+                print(f"Warning: No main() function found in {f_name}")
+    except FileNotFoundError:
+        print(f"Warning: File {f_name} not found")
+
+# Write the main function that checks colors and calls appropriate functions
+with open("main.py", 'a') as m:
+    m.write("\nasync def main():\n")
+    
+    for func_info in function_calls:
+        m.write(f"    if color_sensor.color() == {func_info['color']}:\n")
+        
+        if func_info['is_async']:
+            m.write(f"        await {func_info['name']}()\n")
+        else:
+            m.write(f"        {func_info['name']}()\n")
+        m.write("        return  # Exit after running one function\n")
+    
+    # Add a default case
+    m.write("    # Default case - no matching color detected\n")
+    m.write("    print(f'Detected color: {color_sensor.color()}')\n")
+
+# Write the main loop
+with open("main.py", 'a') as m:
+    m.write("\n# Main execution loop\n")
+    m.write("while True:\n")
+    m.write("    run_task(main())\n")
+    m.write("    wait(100)\n")
+
+print("Script merger completed successfully!")
+print("Functions created:")
+for func_info in function_calls:
+    print(f"  - {func_info['name']}() triggered by {func_info['color']} (from {func_info['filename']})")

utils/constants.py

@@ -0,0 +1,13 @@
+#Speed and distance constants
+SPEEDS = {
+    'FAST': 400,
+    'NORMAL': 250,
+    'SLOW': 100,
+    'PRECISE': 50
+}
+
+DISTANCES = {
+    'TILE_SIZE': 300,  # mm per field tile
+    'ROBOT_LENGTH': 180,  # mm
+    'ROBOT_WIDTH': 140    # mm
+}

utils/new_setup.py

@@ -0,0 +1,187 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor
+from pybricks.parameters import Port, Stop
+from pybricks.tools import wait, StopWatch, multitask
+from umath import pi
+
+class Attachment:
+    def __init__(self, port, start_angle=0):
+        self.motor = Motor(port)
+        self.start_angle = start_angle
+    
+    def move_attachment(self, degrees, speed):
+        self.motor.reset_angle(0)
+        target_angle = degrees
+        tolerance = 2
+        movement_timer = StopWatch()
+        movement_timeout = 3000  # 3 seconds timeout
+        
+        stuck_threshold = 50  # ms without significant movement
+        last_angle = 0
+        stuck_timer = StopWatch()
+        
+        while movement_timer.time() < movement_timeout:
+            current_angle = self.motor.angle()
+            error = target_angle - current_angle
+            
+            if abs(error) <= tolerance:
+                self.motor.stop()
+            
+            # Check if motor is stuck
+            if abs(current_angle - last_angle) < 1:  # Less than 1 degree movement
+                if stuck_timer.time() > stuck_threshold:
+                    print("Motor appears stuck")
+                    self.motor.stop()
+            else:
+                stuck_timer.reset()
+                last_angle = current_angle
+            
+            if error > 0:
+                self.motor.run(speed)
+            else:
+                self.motor.run(-speed)
+            wait(10)  # Consistent timing
+        
+        self.motor.stop()
+        print("Movement timeout reached")
+
+    def reset_attachment(self):
+        self.motor.reset_angle(0)
+        return self.move_attachment(self.start_angle, 100)
+
+class Robot:
+    def __init__(self, left_port, right_port, wheel_diameter):
+        # Initialize hub and motors
+        self.hub = PrimeHub()
+        try:
+            self.left_motor = Motor(left_port)
+            self.right_motor = Motor(right_port)
+        except Exception as e:
+            print(f"Failed to initialize motors: {e}")
+            
+        # Reset and calibrate IMU
+        try:
+            self.hub.imu.reset_heading(0)
+            wait(1000)  # Give IMU time to calibrate
+        except Exception as e:
+            print(f"Failed to initialize IMU: {e}")
+            
+        # Robot specs
+        self.wheel_diameter = wheel_diameter
+        self.wheel_circumference = pi * self.wheel_diameter
+        
+        # Attachments list
+        self.attachments = {}
+    
+    def add_attachment(self, name, port, start_angle=0):
+        try:
+            self.attachments[name] = Attachment(port, start_angle)
+        except Exception as e:
+            print(f"Failed to add attachment '{name}': {e}")
+    
+    def move_attachment(self, attachment_name, degrees, speed):
+        if attachment_name in self.attachments:
+            return self.attachments[attachment_name].move_attachment(degrees, speed)
+        else:
+            print(f"Attachment '{attachment_name}' not found")
+    
+    def reset_attachment(self, attachment_name):
+        if attachment_name in self.attachments:
+            return self.attachments[attachment_name].reset_attachment()
+        else:
+            print(f"Attachment '{attachment_name}' not found")
+    
+    def straight(self, distance, speed):
+        if not (-200 <= speed <= 200):
+            print(f"Invalid speed: {speed}. Must be between -200 and 200.")
+            
+        target_heading = self.hub.imu.heading()
+        # Reset distance tracking
+        self.left_motor.reset_angle(0)
+        self.right_motor.reset_angle(0)
+        
+        # Calculate target distance in motor degrees
+        target_degrees = abs(distance) / self.wheel_circumference * 360
+        
+        while True:
+            # Check current distance traveled (assuming both motors should be positive for forward)
+            left_angle = abs(self.left_motor.angle())
+            right_angle = abs(self.right_motor.angle())
+            average_angle = (left_angle + right_angle) / 2
+            
+            # Stop if it reached the target
+            if average_angle >= target_degrees:
+                break
+                
+            # Get heading error for correction
+            current_heading = self.hub.imu.heading()
+            heading_error = target_heading - current_heading
+            
+            # Handle wraparound at 0°/360°
+            if heading_error > 180:
+                heading_error -= 360
+            elif heading_error < -180:
+                heading_error += 360
+                
+            # Calculate motor speeds with correction
+            direction = 1 if distance > 0 else -1
+            correction = heading_error * 1.5  # Reduced gain for stability
+            left_speed = direction * (speed + correction)
+            right_speed = direction * (speed - correction)
+            
+            # Limit speeds to prevent excessive values
+            left_speed = max(-200, min(200, left_speed))
+            right_speed = max(-200, min(200, right_speed))
+            
+            # Apply speeds to motors
+            self.left_motor.run(left_speed)
+            self.right_motor.run(right_speed)
+            wait(10)  # Consistent timing
+        
+        # Stop motors
+        self.left_motor.stop()
+        self.right_motor.stop()
+        wait(50)  # Allow motors to fully stop
+    
+    def turn(self, theta, speed):
+        if not (-200 <= speed <= 200):
+            print(f"Invalid speed: {speed}. Must be between -200 and 200.")
+            
+        start_angle = self.hub.imu.heading()
+        target_angle = start_angle + theta
+        
+        # Normalize target angle to -180 to 180 range
+        while target_angle > 180:
+            target_angle -= 360
+        while target_angle < -180:
+            target_angle += 360
+            
+        while True:
+            current_angle = self.hub.imu.heading()
+            # Calculate angle error
+            angle_error = target_angle - current_angle
+            
+            # Handle wraparound for shortest path
+            if angle_error > 180:
+                angle_error -= 360
+            elif angle_error < -180:
+                angle_error += 360
+                
+            # Stop if we're close enough
+            if abs(angle_error) < 2:
+                break     
+				
+            # Positive angle_error means we need to turn clockwise (right)
+            if angle_error > 0:
+                # Turn right: left motor forward, right motor backward
+                self.left_motor.run(speed)
+                self.right_motor.run(-speed)
+            else:
+                # Turn left: left motor backward, right motor forward
+                self.left_motor.run(-speed)
+                self.right_motor.run(speed)
+            wait(10)  # Consistent timing
+        
+        # Stop both motors
+        self.left_motor.stop()
+        self.right_motor.stop()

utils/robot_control.py

@@ -0,0 +1,43 @@
+# utils/robot_control.py - Shared driving and control functions
+from pybricks.robotics import DriveBase
+from config import ROBOT_CONFIG
+import time
+
+# Initialize drive base (done once)
+drive_base = DriveBase(
+    ROBOT_CONFIG['left_motor'], 
+    ROBOT_CONFIG['right_motor'], 
+    wheel_diameter=56, 
+    axle_track=114
+)
+
+def drive_straight(distance, speed=200):
+    """Drive straight for a given distance in mm"""
+    drive_base.straight(distance, speed=speed)
+
+def turn_angle(angle, speed=100):
+    """Turn by a given angle in degrees"""
+    drive_base.turn(angle, speed=speed)
+
+def drive_until_line(speed=100, sensor='color_left'):
+    """Drive until line is detected"""
+    sensor_obj = ROBOT_CONFIG[sensor]
+    drive_base.drive(speed, 0)
+    
+    while sensor_obj.color() != Color.BLACK:
+        time.sleep(0.01)
+    
+    drive_base.stop()
+
+def return_to_base():
+    """Return robot to launch area - implement based on your strategy"""
+    print("Returning to base...")
+    # Add your return-to-base logic here
+    pass
+
+def reset_robot():
+    """Reset all motors and sensors to default state"""
+    drive_base.stop()
+    for motor in ROBOT_CONFIG.get('attachment_motors', []):
+        motor.stop()
+    print("Robot reset completed")

utils/starter_drivebase_code.py

@@ -0,0 +1,36 @@
+from pybricks.hubs import PrimeHub
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+
+hub = PrimeHub()
+
+# Initialize both motors. In this example, the motor on the
+# left must turn counterclockwise to make the robot go forward.
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B)
+
+arm_motor = Motor(Port.E, Direction.CLOCKWISE)
+arm_motor.run_angle(299,90, Stop.HOLD)
+# Initialize the drive base. In this example, the wheel diameter is 56mm.
+# The distance between the two wheel-ground contact points is 112mm.
+drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=112)
+
+print('The default settings are: ' + str(drive_base.settings()))
+drive_base.settings(100,1000,166,750)
+# Optionally, uncomment the line below to use the gyro for improved accuracy.
+drive_base.use_gyro(True)
+
+# Drive forward by 500mm (half a meter).
+drive_base.straight(500)
+
+# Turn around clockwise by 180 degrees.
+drive_base.turn(180)
+
+# Drive forward again to get back to the start.
+drive_base.straight(500)
+
+# Turn around counterclockwise.
+drive_base.turn(-180)
+arm_motor.run_angle(299,-90, Stop.HOLD)