Uploaded files from Bitbucket

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/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")