diff --git a/README.md b/README.md index fc9cc11..e1d3f35 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,3 @@ -# 65266 Lego Dynamics - UNEARTHED Season Repository +# DO NOT MERGE THIS BRANCH!!!!! -## 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. \ No newline at end of file +# MERGING THIS BRANCH WILL CAUSE THE CODE TO BE REMOVED!!! THIS IS ONLY FOR A FILE SYSTEM! DO NOT, UNDER ANY CIRCUMSTANCES, MERGE ANYTHING INTO THIS BRANCH OR MERGE THIS BRANCH INTO ANY OTHER!!!! diff --git a/config.py b/config.py deleted file mode 100644 index f59fbfb..0000000 --- a/config.py +++ /dev/null @@ -1,40 +0,0 @@ -# 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 -} \ No newline at end of file diff --git a/members/atharv.txt b/members/atharv.txt deleted file mode 100644 index 61d22be..0000000 --- a/members/atharv.txt +++ /dev/null @@ -1,3 +0,0 @@ -About Me - -I am Atharv, the Git manager for the team. I like to produce music. \ No newline at end of file diff --git a/missions/Boat.py b/missions/Boat.py deleted file mode 100644 index bb42eb7..0000000 --- a/missions/Boat.py +++ /dev/null @@ -1,27 +0,0 @@ -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()) \ No newline at end of file diff --git a/missions/Heavy lifting.py b/missions/Heavy lifting.py deleted file mode 100644 index 643e47d..0000000 --- a/missions/Heavy lifting.py +++ /dev/null @@ -1,37 +0,0 @@ -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) - diff --git a/missions/Lift.py b/missions/Lift.py deleted file mode 100644 index a5e1773..0000000 --- a/missions/Lift.py +++ /dev/null @@ -1,34 +0,0 @@ -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()) \ No newline at end of file diff --git a/missions/M8_5.py b/missions/M8_5.py deleted file mode 100644 index 69e364d..0000000 --- a/missions/M8_5.py +++ /dev/null @@ -1,49 +0,0 @@ -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()) \ No newline at end of file diff --git a/missions/Sand Mission.py b/missions/Sand Mission.py deleted file mode 100644 index 73e1e16..0000000 --- a/missions/Sand Mission.py +++ /dev/null @@ -1,29 +0,0 @@ -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()) \ No newline at end of file diff --git a/missions/Send_Over.py b/missions/Send_Over.py deleted file mode 100644 index 97bed5f..0000000 --- a/missions/Send_Over.py +++ /dev/null @@ -1,31 +0,0 @@ -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()) \ No newline at end of file diff --git a/missions/mission_09.py b/missions/mission_09.py deleted file mode 100644 index 3337624..0000000 --- a/missions/mission_09.py +++ /dev/null @@ -1,43 +0,0 @@ -# ---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()) \ No newline at end of file diff --git a/utils/base_robot_classes.py b/utils/base_robot_classes.py deleted file mode 100644 index 6d5e52a..0000000 --- a/utils/base_robot_classes.py +++ /dev/null @@ -1,145 +0,0 @@ -# --- 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 deleted file mode 100644 index 54bb542..0000000 --- a/utils/color_sensor_navi.py +++ /dev/null @@ -1,41 +0,0 @@ -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 deleted file mode 100644 index 0d3fab1..0000000 --- a/utils/combine_runs.py +++ /dev/null @@ -1,139 +0,0 @@ -# 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 deleted file mode 100644 index f74d325..0000000 --- a/utils/constants.py +++ /dev/null @@ -1,13 +0,0 @@ -#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/new_setup.py b/utils/new_setup.py deleted file mode 100644 index be7680a..0000000 --- a/utils/new_setup.py +++ /dev/null @@ -1,187 +0,0 @@ -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() \ No newline at end of file diff --git a/utils/robot_control.py b/utils/robot_control.py deleted file mode 100644 index 9bb0e5d..0000000 --- a/utils/robot_control.py +++ /dev/null @@ -1,43 +0,0 @@ -# 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 diff --git a/utils/starter_drivebase_code.py b/utils/starter_drivebase_code.py deleted file mode 100644 index d7cb0b7..0000000 --- a/utils/starter_drivebase_code.py +++ /dev/null @@ -1,36 +0,0 @@ -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) \ No newline at end of file