Merge to main for Scrimmage

Reviewed-on: #9

LINEUPS.md

@@ -0,0 +1,15 @@
+# Lineups
+
+## These are the line-up positions for the robot game for various missions.
+
+- Mission Run #1 (Mission #1) [Right/Blue Home] - The left yellow part of the right arm attachment is positioned with its right edge on the 5th thin line from the left. Note that this is NOT positioned from the back of the robot.
+
+- Mission Run #2 (Send Over) [Right/Blue Home] - The robot should be lined up on the vertical edge of the left home. The robot's right side should be positioned on the 1st thin line from the top. Note that the 0th line is the one that is not part of the curve.
+
+- Mission Run #3 (Sand mission) [Right/Blue Home] - The robot should be lined up on the vertical edge of the left home. The robot's left side should be positioned on the 1/2th thin line from the bottom.
+
+- Mission Run #4 (Boat mission) [Left/Red Home] - The robot should be lined up on the vertical edge of the left home. The robot's right side should be positioned on the 2nd thick line from the bottom.
+
+- Mission Run #5 (Bautism) [Left/Red Home] - The robot's left edge should be positioned at the 1st thick, 2nd thin line from the left.
+
+- Mission Run #6 (Not-so-heavy Lifting) [Right/Blue Home] - The robot's right edge should be positioned at the 1st thick from the right.

README.md

@@ -14,7 +14,11 @@ This repository contains the Pybricks code for Team 65266 Lego Dynamics' robot f
 
 ## Code Structure
 
+<<<<<<< HEAD
 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.
+=======
+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.
+>>>>>>> dev
 
 ## How to Use
 
@@ -22,4 +26,11 @@ Load the master file into PyBricks, then send it over to the robot. Then you hol
 
 ## License
 GNU General Public License
-You can take inspiration from our code, but you can't take our exact code.
+<<<<<<< HEAD
+You can take inspiration from our code, but you can't take our exact code.
+=======
+
+You can take inspiration from our code, but you can't take our exact code.
+
+Read LICENSE for more information.
+>>>>>>> dev

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
+}

final/1main.py

@@ -0,0 +1,116 @@
+
+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)
+
+
+async def Run1():
+    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)
+
+async def Run2():
+    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)
+
+
+async def Run3():
+    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)
+
+
+async def Run5():
+    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)
+
+
+async def Run6():
+    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)
+
+
+async def main():
+    if color_sensor.color() == Color.ORANGE:
+        await Run1()
+        
+    if color_sensor.color() == Color.GREEN:
+        await Run2()
+        
+    if color_sensor.color() == Color.WHITE:
+        await Run3()
+        
+    if color_sensor.color() == Color.YELLOW:
+        await Run5()
+        
+    if color_sensor.color() == Color.BLUE:
+        await Run6()
+        
+    
+    print(f'Detected color: {color_sensor.color()}')
+
+# Main execution loop
+while True:
+    run_task(main())
+    wait(100)

members/Johannes

@@ -0,0 +1,7 @@
+I am Johannes
+
+I am the building manager for the team. I like making art.
+
+
+
+Parthiv, Diddy will touch you tonight.

members/Vickram.txt

@@ -0,0 +1 @@
+Hello my name is Vickram and I like coding and video games.

members/atharv.txt

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

missions/Bautism.py

@@ -0,0 +1,45 @@
+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, run_task
+
+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.D, Direction.CLOCKWISE)
+arm_motor_left= Motor(Port.C, Direction.CLOCKWISE)
+# 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=68.8, axle_track=180)
+
+print('The default settings are: ' + str(drive_base.settings()))
+drive_base.settings(300,1000,300,750)
+# Optionally, uncomment the line below to use the gyro for improved accuracy.
+drive_base.use_gyro(True)
+
+async def main():
+    await drive_base.straight(519)
+    await arm_motor_left.run_angle(300, -100)
+    await arm_motor_left.run_angle(300, 500)
+    await drive_base.straight(180)
+    await drive_base.turn(-37)
+    await drive_base.straight(50)
+    await arm_motor.run_angle(300, -400)
+    await drive_base.straight(-150)
+    await drive_base.turn(135)
+    await drive_base.straight(50)
+    await arm_motor.run_angle(300, 400)
+    await drive_base.straight(-75)
+    await arm_motor.run_angle(300, 300)
+    await drive_base.turn(-50)
+    await drive_base.straight(162)
+    await arm_motor.run_angle(100, -200)
+    await drive_base.straight(30)
+    await arm_motor.run_angle(50,-500)
+
+run_task(main())

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/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(277)
+    await drive_base.turn(20)
+    await drive_base.straight(65)
+
+    await drive_base.turn(-30)
+    right_arm.run_angle(50,500)
+    await drive_base.turn(45)
+    await drive_base.straight(-145)
+    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(-600)
+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_old.py

@@ -0,0 +1,44 @@
+# ---JOHANNES---
+# THIS CODE IS NOT USED ANYMORE AND SHOULD NOT BE USED!!!!!!
+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())

missions/tip the scale.py

@@ -0,0 +1,42 @@
+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(300,1000,300,200)
+
+#drive_base.use_gyro(True)
+
+async def main():
+    left_arm.run_angle(500,200)   
+    right_arm.run_angle(500,200)
+    await drive_base.straight(70)
+    
+    await drive_base.turn(-55)
+    await drive_base.straight(900)
+    await drive_base.turn(92.5)
+    
+    await drive_base.straight(75)
+    await drive_base.straight(21)
+    await right_arm.run_angle(500,-250)
+    await right_arm.run_angle(500,250)
+    await drive_base.turn(55)
+    
+    await left_arm.run_angle(300,-400)
+    
+    await drive_base.turn(46.5)
+    await drive_base.turn(-40)
+    await drive_base.straight(900)
+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,134 @@
+# Guys please use the same setup code and put into the imports for consistency
+script_names = ["Run1.py", "Run2.py", "Run3.py", "Run5.py", "Run6.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.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]
+                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("        \n")
+    
+    # Add a default case
+    m.write("    \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")

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)