From 3ddaf7e8d0098cea2b511a16d228233652868859 Mon Sep 17 00:00:00 2001 From: Atharv <30nagava@elmbrookstudents.org> Date: Wed, 10 Sep 2025 13:31:27 +0000 Subject: [PATCH] Uploaded files from Bitbucket --- utils/base_robot_classes.py | 145 ++++++++++++++++++++++++++++++++++++ utils/color_sensor_navi.py | 41 ++++++++++ utils/combine_runs.py | 139 ++++++++++++++++++++++++++++++++++ utils/constants.py | 13 ++++ utils/robot_control.py | 43 +++++++++++ 5 files changed, 381 insertions(+) create mode 100644 utils/base_robot_classes.py create mode 100644 utils/color_sensor_navi.py create mode 100644 utils/combine_runs.py create mode 100644 utils/constants.py create mode 100644 utils/robot_control.py diff --git a/utils/base_robot_classes.py b/utils/base_robot_classes.py new file mode 100644 index 0000000..6d5e52a --- /dev/null +++ b/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()) \ No newline at end of file diff --git a/utils/color_sensor_navi.py b/utils/color_sensor_navi.py new file mode 100644 index 0000000..54bb542 --- /dev/null +++ b/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() diff --git a/utils/combine_runs.py b/utils/combine_runs.py new file mode 100644 index 0000000..0d3fab1 --- /dev/null +++ b/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']})") \ No newline at end of file diff --git a/utils/constants.py b/utils/constants.py new file mode 100644 index 0000000..f74d325 --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/utils/robot_control.py b/utils/robot_control.py new file mode 100644 index 0000000..9bb0e5d --- /dev/null +++ b/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") \ No newline at end of file