From cea616f13e659f1b38aa1bbd7bfb63c456eb068f Mon Sep 17 00:00:00 2001 From: nifrali Date: Sat, 31 Oct 2026 13:27:07 -0500 Subject: [PATCH] Added object oriented version of codes --- .DS_Store | Bin 0 -> 6148 bytes regionals/arm.py | 134 ++++++++ regionals/arm_calibrate.py | 30 ++ regionals/bioglow_helpers.py | 165 +++++++++ regionals/bioglow_main.py | 121 +++++++ regionals/bioglow_mission_1.py | 53 +++ regionals/bioglow_mission_11_experimental.py | 55 +++ regionals/bioglow_mission_12.py | 51 +++ regionals/bioglow_mission_13.py | 51 +++ regionals/bioglow_mission_2.py | 67 ++++ .../bioglow_mission_3_4_5_experimental.py | 55 +++ regionals/bioglow_mission_8.py | 46 +++ regionals/bioglow_missions_11_6_7.py | 70 ++++ regionals/bioglow_missions_15_14_ant.py | 67 ++++ regionals/bioglow_missions_3_4_5.py | 51 +++ ...bioglow_missions_6_7_13_14_experimental.py | 92 +++++ .../bioglow_missions_6_7_experimental.py | 78 +++++ regionals/movement.py | 317 ++++++++++++++++++ regionals/robot.py | 116 +++++++ 19 files changed, 1619 insertions(+) create mode 100644 .DS_Store create mode 100644 regionals/arm.py create mode 100644 regionals/arm_calibrate.py create mode 100644 regionals/bioglow_helpers.py create mode 100644 regionals/bioglow_main.py create mode 100644 regionals/bioglow_mission_1.py create mode 100644 regionals/bioglow_mission_11_experimental.py create mode 100644 regionals/bioglow_mission_12.py create mode 100644 regionals/bioglow_mission_13.py create mode 100644 regionals/bioglow_mission_2.py create mode 100644 regionals/bioglow_mission_3_4_5_experimental.py create mode 100644 regionals/bioglow_mission_8.py create mode 100644 regionals/bioglow_missions_11_6_7.py create mode 100644 regionals/bioglow_missions_15_14_ant.py create mode 100644 regionals/bioglow_missions_3_4_5.py create mode 100644 regionals/bioglow_missions_6_7_13_14_experimental.py create mode 100644 regionals/bioglow_missions_6_7_experimental.py create mode 100644 regionals/movement.py create mode 100644 regionals/robot.py diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..335da9fb8458983350547ecaee878bb94091701a GIT binary patch literal 6148 zcmeHKK~4iP3>-rbMZNUMF<;;ZQB_{h4-_b>kXE}=D>(0sZ!>m`)M|^=69*Ve&LoZ} zQRc8-1F+Tm?g3Z=nA08c>BH1~-+f{?8F92YV?d85bU5yglkCd@=icFfSM2eI6aGv0 zJPaL=y|n$YIrQDo@44$4GASShq<|EV0#e{=1+sL`7gsZsCEIA80CC1}80XPT5Q_(hHF0dn2+bOm*r=8*hBZ3lCG%?H*s#%IaWl`!n=LyOi`yA5 zkq)a3HA(>~aIU~@TQ0r+Khl5b|IbNUNdYPFrxdW+db3{fNm*MbkMmkv=v(@?F*njV lL@6dlDdxgU@%>3&@j3Tv;@B|gj0c^l9|6}zCIx;&fiHu691H*e literal 0 HcmV?d00001 diff --git a/regionals/arm.py b/regionals/arm.py new file mode 100644 index 0000000..1ed1cf1 --- /dev/null +++ b/regionals/arm.py @@ -0,0 +1,134 @@ +""" +arm.py — easy arm moves with % speed, named positions and homing. + +Set up each arm ONCE (in robot.py): + from arm import Arm + left_arm = Arm(Motor(Port.C, ...), speed=100, + positions={"down": 0, "up": 270}, + home_backward=True) + +Use it in any mission (always with await): + await robot.left_arm.home() # push to the hard stop, call it 0 + await robot.left_arm.move_to("up") # go to a named position + await robot.left_arm.move_to(135) # go to an exact angle + await robot.left_arm.move_by(-45) # move 45 degrees from where it is + await robot.left_arm.move_for_seconds(1) # run for 1 second + await robot.left_arm.move_until_stalled() # run until it's blocked + robot.left_arm.reset() # call where it is now 0 + +Speed is a PERCENT of this arm's top speed (it's geared, so that's slow). +Leave speed out to use the arm's default speed. +""" + +from pybricks.parameters import Stop +from pybricks.tools import wait, multitask + + +class Arm: + def __init__(self, motor, speed=50, positions=None, + home_backward=True, home_angle=0, home_speed=30, home_power=40): + """ + motor: the Motor(...) for this arm. + speed: default % speed when a move gives no speed. + positions: names for angles, like {"down": 0, "up": 270}. + Angles are measured from the home spot. + home_backward: which way the arm turns to reach its hard stop. + Check this on the robot before using home()! + home_angle: what to call the angle at the hard stop (usually 0). + home_speed: % speed for homing. Slow is gentle. + home_power: % power limit for homing, so it pushes softly. + """ + self.motor = motor + self.top_speed = motor.control.limits()[0] # deg/s after the gears = 100% + self.positions = positions or {} + self.home_backward = home_backward + self.home_angle = home_angle + self.home_speed = home_speed + self.home_power = home_power + self.set_default_speed(speed) + + # ------------------------------------------------------------ helpers + def set_default_speed(self, speed): + """Change this arm's default % speed from now on.""" + self._deg_s(speed) # just checks it's 1-100 + self.default_speed = speed + + def _deg_s(self, percent): + if percent is None: + percent = self.default_speed + if not 1 <= percent <= 100: + raise ValueError("arm speed is a percent from 1 to 100, but got %s" % percent) + return max(1, int(self.top_speed * percent / 100)) + + def _target(self, target): + if isinstance(target, str): + if target not in self.positions: + raise ValueError("no arm position named '%s'. Known: %s" + % (target, ", ".join(self.positions))) + return self.positions[target] + return target + + # ------------------------------------------------------------ moves + async def move_to(self, target, speed=None): + """Go to a named position ("up") or an exact angle (135), then hold there.""" + await self.motor.run_target(self._deg_s(speed), self._target(target)) + + async def move_by(self, degrees, speed=None): + """Move this many degrees from where it is now (negative = other way).""" + await self.motor.run_angle(self._deg_s(speed), degrees) + + async def move_for_seconds(self, seconds, speed=None, backward=False): + """Run for a set time, then hold.""" + deg_s = self._deg_s(speed) + await self.motor.run_time(-deg_s if backward else deg_s, int(seconds * 1000)) + + async def move_until_stalled(self, speed=None, backward=False, power=None, max_seconds=3): + """Run until something blocks the arm, then let it relax. + + power: % power limit (1-100), so it pushes softly. None = full power. + max_seconds: give up after this long. + Returns True if it got blocked, False if it gave up. + """ + deg_s = self._deg_s(speed) + results = await multitask( + self.motor.run_until_stalled(-deg_s if backward else deg_s, + then=Stop.COAST, duty_limit=power), + wait(int(max_seconds * 1000)), + race=True, + ) + return results[0] is not None # the stall finished first + + async def home(self): + """Push gently against the hard stop and call that spot home_angle (usually 0). + + Do this at the start, so "up" and "down" mean the same thing every run. + Returns True if it found the stop. + """ + found = await self.move_until_stalled(speed=self.home_speed, + backward=self.home_backward, + power=self.home_power) + self.reset(self.home_angle) + if not found: + print("Arm never reached its hard stop. Check home_backward in robot.py.") + return found + + # ------------------------------------------------------------ info and stopping + def reset(self, angle=0): + """Say "the arm is at this angle right now" (default 0). + + Always give a number: Pybricks' own reset_angle() with no number + does something different (it uses the motor's built-in marker). + """ + self.motor.reset_angle(angle) + + def angle(self): + """Where the arm is now, in degrees.""" + return self.motor.angle() + + def stop(self): + """Let the arm go limp.""" + self.motor.stop() + + def hold(self): + """Lock the arm where it is.""" + self.motor.hold() diff --git a/regionals/arm_calibrate.py b/regionals/arm_calibrate.py new file mode 100644 index 0000000..530a4d4 --- /dev/null +++ b/regionals/arm_calibrate.py @@ -0,0 +1,30 @@ +""" +arm_calibrate.py — find the real angles for "up", "down", and any other arm position. + +How to use: + 1. Open this file in Pybricks Code and press Run. + 2. Each arm pushes gently to its hard stop (that spot becomes 0). + If an arm spins the wrong way, stop the program and flip home_backward in robot.py. + 3. The arms go limp. Move them BY HAND to each position you want. + 4. Read the angles printed below and copy them into positions={...} in robot.py. + 5. Press the hub's center button (or Stop) to finish. +""" + +import robot + + +async def calibrate(): + print("Homing arms...") + left_ok, right_ok = await robot.multitask(robot.left_arm.home(), robot.right_arm.home()) + print("Left arm found its stop:", left_ok, " Right arm found its stop:", right_ok) + + robot.left_arm.stop() # go limp so you can move them by hand + robot.right_arm.stop() + print("Move the arms by hand. Angles:") + + while True: + print("left_arm:", robot.left_arm.angle(), " right_arm:", robot.right_arm.angle()) + await robot.wait(500) + + +robot.run_task(calibrate()) diff --git a/regionals/bioglow_helpers.py b/regionals/bioglow_helpers.py new file mode 100644 index 0000000..7fc9dcf --- /dev/null +++ b/regionals/bioglow_helpers.py @@ -0,0 +1,165 @@ +######################################################## +# bioglow_helpers.py +# - This file contain all the helper functions and tools +# - This is where we add special or custom functionality codes +######################################################## + +""" +PID Straight Drive — heading-corrected forward / backward movement. + +Improvements over baseline: + • Derivative on measurement (not error) — no derivative kick on setpoint change + • Fixed-interval dt — loop sleeps a constant 20 ms, so /dt is dropped from the + derivative term (dividing by 0.02 was silently multiplying KD by 50) + • prev_error seeded from first real heading — eliminates first-tick spike + • Per-motor speed clamping — prevents one side pivoting at low speeds + • Deceleration ramp — smooth stop in the last RAMP_MM millimetres + • Dual-motor exit + timeout — survives stall on either wheel + • Integral freeze near target — stops windup right where it matters most +""" + +from robot import * # hub, motors, sensors, drive_base, wait, umath, ... all come from robot.py +import robot + +LOOP_MS = 20 # fixed control loop period +RAMP_MM = 50 # begin decelerating this far from target +MIN_SPEED = 80 # floor speed during ramp (avoid stall) +TIMEOUT_MS = 8_000 # safety abort +# Tuning sequence + +KP = 4.8 # Wide chassis = stable platform, can afford higher P gain + # without oscillation. Pushes harder through corrections. + +KI = 0.012 # Slightly lower than default — large wheels cover distance + # fast so the integrator has less time to build up per run. + # Raise to 0.012 if you see consistent sideways drift at end. + +KD = 7.5 # Higher than default. Big wheels = more rotational momentum, + # needs stronger damping to prevent overshoot on corrections. + +def _mm_to_deg(mm: float) -> float: + return (abs(mm) / (umath.pi * WHEEL_DIAMETER)) * 360.0 + + +def _motor_avg_angle(m1, m2) -> float: + return (abs(m1.angle()) + abs(m2.angle())) / 2.0 + + +async def pid_straight(distance_mm: float, speed: int = 600): + """ + Drive straight for distance_mm at speed (deg/s). + Positive = forward, negative = reverse. + """ + direction = 1 if distance_mm >= 0 else -1 + degrees_need = _mm_to_deg(distance_mm) + target_hdg = hub.imu.heading() + + left_motor.reset_angle(0) + right_motor.reset_angle(0) + + # ── seed integral and prev_error from the real opening heading ── + first_error = target_hdg - hub.imu.heading() # 0.0 on a perfect start, + integral = 0.0 # non-zero if robot is tilted + prev_heading = hub.imu.heading() # for derivative-on-measurement + + watchdog = StopWatch() + + while True: + traveled = _motor_avg_angle(left_motor, right_motor) + + # ── exit: distance reached or timeout ── + if traveled >= degrees_need: + break + if watchdog.time() > TIMEOUT_MS: + break + + # ── heading error with wrap ── + heading = hub.imu.heading() + error = target_hdg - heading + if error > 180: error -= 360 + if error < -180: error += 360 + + # ── derivative on measurement — no kick when target changes ── + d_heading = heading - prev_heading # raw delta (deg / loop) + if d_heading > 180: d_heading -= 360 + if d_heading < -180: d_heading += 360 + prev_heading = heading + + # ── integral with freeze near target ── + near_target = (degrees_need - traveled) < _mm_to_deg(RAMP_MM / 2) + if not near_target: + integral = max(-50.0, min(50.0, integral + error)) # dt=const → no *dt + + # ── PID output ── + # derivative term: -KD * d_heading (measurement, not error difference) + correction = KP * error + KI * integral - KD * d_heading + correction = max(-200.0, min(200.0, correction)) + + # ── ramp speed near end ── + remaining = degrees_need - traveled + ramp_degrees = _mm_to_deg(RAMP_MM) + if remaining < ramp_degrees and ramp_degrees > 0: + ramp_factor = max(float(MIN_SPEED) / speed, + remaining / ramp_degrees) + run_speed = int(speed * ramp_factor) + else: + run_speed = speed + + # ── per-motor clamping — prevents pivot at low speeds ── + l_cmd = max(-speed, min(speed, direction * run_speed + int(correction))) + r_cmd = max(-speed, min(speed, direction * run_speed - int(correction))) + + left_motor.run(l_cmd) + right_motor.run(r_cmd) + await wait(LOOP_MS) + + left_motor.brake() + right_motor.brake() + + +""" +Debugging helps +""" +DEBUG = 1 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 + +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,0) + +async def drive_backward(): + """Drive backward continuously using DriveBase.""" + drive_base.drive(-400, 0) # FIXED: was +400, which drove forward + + +async def monitor_distance(stop_mm: int = WALL_DISTANCE): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...', distance) + + if distance is None: + await wait(50) + continue + if distance < stop_mm: + drive_base.stop() + print(f"Wall detected at {distance}mm!") + break + + # Small delay to prevent overwhelming the sensor + await wait(50) + +# Speeds: use speed= and acceleration= on each move, e.g. robot.move(12, speed=18, acceleration=200). +# Both arms together: robot.lift_up(degrees, speed=...) and robot.lift_down(degrees, speed=...). diff --git a/regionals/bioglow_main.py b/regionals/bioglow_main.py new file mode 100644 index 0000000..1eafcd3 --- /dev/null +++ b/regionals/bioglow_main.py @@ -0,0 +1,121 @@ +##################################### +# Main.py # +# File for running all MISSIONS # +# together # +# # +##################################### + +import bioglow_helpers as helper +import robot +import bioglow_missions_3_4_5 as Run_1 # import here the name of files containing mission runs Run_1 for run 1, +import bioglow_mission_2 as Run_2 # Run_2 for run 2, and so on.. +import bioglow_mission_1 as Run_3 +import bioglow_mission_11_experimental as Run_4 +import bioglow_mission_13 as Run_5 +import bioglow_missions_6_7_13_14_experimental as Run_6 # FIXED: was bioglow_mission_6_7_13_14 (no such file) +import bioglow_missions_15_14_ant as Run_7 # FIXED: was bioglow_missions_14_15 (no such file) +import bioglow_mission_8 as Run_8 + +async def start(run): + """Run one mission, starting from the normal speeds in robot.py. + + Some runs change speeds with robot.set_default_speed(). Resetting first + means each run drives exactly like it did when tested on its own. + """ + robot.reset_settings() + await run.solve_missions() + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if 1 < h < 11 and s <50: # brown + return "Brown" + elif 11 < h < 24 and s > 80: # orange + return "Orange" + elif 30 < h < 60 and v > 50: # yellow + return "Yellow" + elif 70 < h < 90 and s > 60: # neon green + return "Lime" + elif 100 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 195 < h < 198: # light blue + return "Cyan" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350 and s > 80: # purple + return "Purple" + elif 350 < h and s > 50: #red + return "Red" + elif s < 15 and v > 70: # white + return "White" + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = robot.hub.buttons.pressed() + h, s, v = await robot.color_sensor.hsv() + reflected = await robot.color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if helper.DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") + +#TODO: Add the solve_missions() function of the run number for each color here + if color == "Green": + print('Running Mission 1, Detect Green') + await start(Run_1) + elif color == "Red": + print('Running Mission 2, Detect Red') + await start(Run_2) + elif color == "Yellow": + print('Running Mission 3, Detect Yellow') + await start(Run_3) + elif color == "Blue": + print('Running Mission 4, Detect Blue') + await start(Run_4) + elif color == "Purple": + print('Running Mission 5, Detect Purple') + await start(Run_5) + elif color == "Orange": + print('Running Mission 6, Detect Orange') + await start(Run_6) + elif color == "Cyan": + print('Running Mission 7, Detect Cyan') + await start(Run_7) + elif color == "Lime": + print('Running Mission 8, Detect Lime') + await start(Run_8) + elif color == "Brown": + print('Running Mission Experimental, Detect Brown') + await robot.wait(500) + elif color == "White": + print('Detect White') + await robot.wait(500) + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if robot.Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = robot.hub.battery.voltage() + # Use the function with your voltage reading + percentage = await helper.get_battery_percentage(float(battery_voltage_mV)) + if helper.DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await robot.hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await robot.wait(10) +# Run the main function +robot.run_task(main()) \ No newline at end of file diff --git a/regionals/bioglow_mission_1.py b/regionals/bioglow_mission_1.py new file mode 100644 index 0000000..6c5b402 --- /dev/null +++ b/regionals/bioglow_mission_1.py @@ -0,0 +1,53 @@ +##################################### +# Solution for BioGlow Mission 1 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Drone Survey - Johannes + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + await robot.multitask( + robot.left_arm.move_by(-300, speed=93), # left arm -300°, 93% arm speed + robot.right_arm.move_by(100, speed=93), # right arm 100°, 93% arm speed + robot.move(90) # 90 cm forward + ) + await robot.turn(-25) # 25° left + await robot.move(11) # 11 cm forward + await robot.turn(-35) # 35° left + await robot.turn(35) # 35° right + await robot.move(-3) # 3 cm backward + await robot.turn(25) # 25° right + await robot.move(-20) # 20 cm backward + await robot.turn(-45) # 45° left + await robot.move(50) # 50 cm forward + await robot.turn(75) # 75° right + await robot.move(70) # 70 cm forward + robot.stop() # FIXED: was `await drive_base.stop` (crashed) + + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_mission_11_experimental.py b/regionals/bioglow_mission_11_experimental.py new file mode 100644 index 0000000..ef4985a --- /dev/null +++ b/regionals/bioglow_mission_11_experimental.py @@ -0,0 +1,55 @@ +##################################### +# Solution for BioGlow Mission 11 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Windows to the past + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + robot.reset() # always reset drivebase for heading reset especially with turns + + await robot.multitask( + robot.right_arm.move_by(-90, speed=100), # right arm -90°, 100% arm speed + robot.move(10) # 10 cm forward + ) + await robot.turn(-45) # 45° left + await robot.move(42) # 42 cm forward + await robot.multitask( + robot.left_arm.move_by(45, speed=100), # left arm 45°, 100% arm speed + robot.right_arm.move_by(140, speed=100) # right arm 140°, 100% arm speed + ) + + #return home + await robot.move(-50) # 50 cm backward + await robot.multitask( + robot.left_arm.move_by(-45, speed=23), # left arm -45°, 23% arm speed + robot.right_arm.move_by(-90, speed=100) # right arm -90°, 100% arm speed + ) + #helper.reset_arms() + robot.stop() + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_mission_12.py b/regionals/bioglow_mission_12.py new file mode 100644 index 0000000..b79273a --- /dev/null +++ b/regionals/bioglow_mission_12.py @@ -0,0 +1,51 @@ +##################################### +# Solution for BioGlow Mission 12 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Forest Elder - Parthiv + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + await robot.move(47.5) # 47.5 cm forward + await robot.right_arm.move_by(25, speed=70) # right arm 25°, 70% arm speed + await robot.turn(-20) # 20° left + await robot.left_arm.move_by(245, speed=47) # left arm 245°, 47% arm speed + await robot.move(6) # 6 cm forward + await robot.turn(25) # 25° right + await robot.left_arm.move_by(-56.5, speed=23) # left arm -56.5°, 23% arm speed + await robot.move(-5) # 5 cm backward + await robot.move(5) # 5 cm forward + await robot.turn(-25) # 25° left + await robot.left_arm.move_by(-200, speed=47) # left arm -200°, 47% arm speed + await robot.move(-24) # 24 cm backward + await robot.turn(15) # 15° right + await robot.right_arm.move_by(-200, speed=47) # right arm -200°, 47% arm speed + await robot.move(-20) # 20 cm backward + robot.stop() # FIXED: was `await drive_base.stop` (crashed) + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_mission_13.py b/regionals/bioglow_mission_13.py new file mode 100644 index 0000000..14c60cd --- /dev/null +++ b/regionals/bioglow_mission_13.py @@ -0,0 +1,51 @@ +##################################### +# Solution for BioGlow Mission 13 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Interchangeable Mission - Parthiv + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + await robot.left_arm.move_by(-80, speed=47) # left arm -80°, 47% arm speed + await robot.arc(-35, degrees=90) # arc: circle on the robot's LEFT (35 cm radius), FORWARD 90° (nose turns left) + await robot.turn(50) # 50° right + await robot.move(23.8) # 23.8 cm forward + await robot.turn(-33) # 33° left + await robot.move(8.5) # 8.5 cm forward + await robot.move(-3.3) # 3.3 cm backward + await robot.multitask( + robot.left_arm.move_by(80, speed=47), # left arm 80°, 47% arm speed + robot.right_arm.move_by(-100, speed=47) # right arm -100°, 47% arm speed + ) + await robot.move(-6) # 6 cm backward + await robot.right_arm.move_by(100, speed=47) # right arm 100°, 47% arm speed + await robot.turn(30) # 30° right + await robot.move(-77.5) # 77.5 cm backward + robot.stop() # FIXED: was `await drive_base.stop` (crashed) + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_mission_2.py b/regionals/bioglow_mission_2.py new file mode 100644 index 0000000..d7d2f0a --- /dev/null +++ b/regionals/bioglow_mission_2.py @@ -0,0 +1,67 @@ +##################################### +# Solution for BioGlow Mission 2 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Exploding Seeds - Johannes + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + robot.stop() + robot.reset() + await robot.move(2, speed=100, acceleration=400, then=robot.Stop.NONE) # 2 cm forward, 100% speed, 400 mm/s² accel + await robot.arc(15, cm=11.5, speed=100, turn_speed=48, acceleration=400, turn_acceleration=100, then=robot.Stop.NONE) # arc: circle on the robot's RIGHT (15 cm radius), FORWARD 11.5 cm along it (nose turns right) | 100% speed, 48% turn speed, 400 mm/s² accel, 100 °/s² turn accel + await robot.move(27, speed=100, acceleration=400, then=robot.Stop.HOLD) # 27 cm forward, 100% speed, 400 mm/s² accel + await robot.left_arm.move_by(70, speed=70) # left arm 70°, 70% arm speed + await robot.move(-3, speed=100, acceleration=400) # 3 cm backward, 100% speed, 400 mm/s² accel + await robot.left_arm.move_by(20, speed=23) # left arm 20°, 23% arm speed + await robot.move(-30, speed=100, acceleration=400, then=robot.Stop.COAST), # 30 cm backward, 100% speed, 400 mm/s² accel + await robot.multitask( + robot.right_arm.move_by(-110, speed=100), # right arm -110°, 100% arm speed + robot.left_arm.move_by(-100, speed=100), # left arm -100°, 100% arm speed + robot.turn(-40, speed=48, acceleration=100, then=robot.Stop.HOLD) # 40° left, 48% turn speed, 100 °/s² turn accel + ) + await robot.arc(61, degrees=70, speed=100, turn_speed=48, acceleration=400, turn_acceleration=100, then=robot.Stop.HOLD) # arc: circle on the robot's RIGHT (61 cm radius), FORWARD 70° (nose turns right) | 100% speed, 48% turn speed, 400 mm/s² accel, 100 °/s² turn accel + await robot.turn(-78, speed=48, acceleration=100, then=robot.Stop.HOLD) # 78° left, 48% turn speed, 100 °/s² turn accel + await robot.move(12, speed=18, acceleration=200, then=robot.Stop.COAST) # 12 cm forward, 18% speed, 200 mm/s² accel + await robot.right_arm.move_by(70, speed=100) # right arm 70°, 100% arm speed + await robot.move(-13, speed=18, acceleration=200, then=robot.Stop.COAST) # 13 cm backward, 18% speed, 200 mm/s² accel + await robot.multitask( + robot.turn(95, speed=48, acceleration=100, then=robot.Stop.HOLD), # 95° right, 48% turn speed, 100 °/s² turn accel + robot.right_arm.move_by(-40, speed=100) # right arm -40°, 100% arm speed + ) + await robot.multitask( + robot.move(16, speed=100, acceleration=400, then=robot.Stop.COAST), # 16 cm forward, 100% speed, 400 mm/s² accel + robot.right_arm.move_by(110, speed=100) # right arm 110°, 100% arm speed + ) + await robot.move(-40, speed=100, acceleration=400, then=robot.Stop.COAST) # 40 cm backward, 100% speed, 400 mm/s² accel + await robot.turn(100, speed=48, acceleration=100, then=robot.Stop.NONE) # 100° right, 48% turn speed, 100 °/s² turn accel + await robot.move(70, speed=100, acceleration=400, then=robot.Stop.COAST) # 70 cm forward, 100% speed, 400 mm/s² accel + robot.stop() # FIXED: was `await drive_base.stop` (crashed) + + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_mission_3_4_5_experimental.py b/regionals/bioglow_mission_3_4_5_experimental.py new file mode 100644 index 0000000..6a4701e --- /dev/null +++ b/regionals/bioglow_mission_3_4_5_experimental.py @@ -0,0 +1,55 @@ +##################################### +# Solution for BioGlow Mission 3,4,5 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Flip the rock,etc + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + robot.reset() # always reset drivebase for heading reset especially with turns + + print (robot.left_arm.angle(),robot.right_arm.angle()) + await robot.move(40, speed=100, acceleration=200, then=robot.Stop.COAST) # 40 cm forward, 100% speed, 200 mm/s² accel + + await robot.move(10, speed=100, acceleration=300) # 10 cm forward, 100% speed, 300 mm/s² accel + + await robot.multitask( + robot.right_arm.move_by(-60, speed=23), # right arm -60°, 23% arm speed + robot.left_arm.move_by(60, speed=23), # left arm 60°, 23% arm speed + robot.move(-6, speed=18, acceleration=100) # 6 cm backward, 18% speed, 100 mm/s² accel + ) + + await robot.multitask( + robot.right_arm.move_by(50, speed=12), # right arm 50°, 12% arm speed + robot.left_arm.move_by(-50, speed=12), # left arm -50°, 12% arm speed + robot.move(-4, speed=18, acceleration=100) # 4 cm backward, 18% speed, 100 mm/s² accel + ) + await robot.move(-30) # 30 cm backward + await robot.move(-10) # 10 cm backward + + robot.stop() +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_mission_8.py b/regionals/bioglow_mission_8.py new file mode 100644 index 0000000..cec78ee --- /dev/null +++ b/regionals/bioglow_mission_8.py @@ -0,0 +1,46 @@ +##################################### +# Solution for BioGlow Mission 8 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Research Platform - Johannes + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + robot.reset() + await robot.arc(128, cm=34) # arc: circle on the robot's RIGHT (128 cm radius), FORWARD 34 cm along it (nose turns right) + await robot.arc(-140, cm=35.5) # arc: circle on the robot's LEFT (140 cm radius), FORWARD 35.5 cm along it (nose turns left) + await robot.move(14.5, speed=18, acceleration=400) # 14.5 cm forward, 18% speed, 400 mm/s² accel + await robot.lift_up(900, speed=93) + await robot.lift_down(300, speed=100) + await robot.move(-15, speed=18, acceleration=400) # 15 cm backward, 18% speed, 400 mm/s² accel + await robot.turn(20, speed=100, acceleration=500) # 20° right, 100% turn speed, 500 °/s² turn accel + await robot.move(6.3, speed=100) # 6.3 cm forward, 100% speed + await robot.arc(-100, cm=-110, speed=100) # arc: circle on the robot's LEFT (100 cm radius), BACKWARD 110 cm along it (back end swings left, nose turns right) | 100% speed + robot.stop() # FIXED: was `await drive_base.stop` (crashed) + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_missions_11_6_7.py b/regionals/bioglow_missions_11_6_7.py new file mode 100644 index 0000000..b10cbea --- /dev/null +++ b/regionals/bioglow_missions_11_6_7.py @@ -0,0 +1,70 @@ +##################################### +# Solution for BioGlow Missions 11,6,7 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Window to the Past, Lead Cutter, Humungous Fungus - Johannes + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + await robot.move(11) # 11 cm forward + await robot.turn(-45) # 45° left + await robot.move(28) # 28 cm forward + await robot.right_arm.move_by(120, speed=70) # right arm 120°, 70% arm speed + await robot.left_arm.move_by(200, speed=93) # left arm 200°, 93% arm speed + await robot.move(-7) # 7 cm backward + await robot.left_arm.move_by(-110, speed=100) # left arm -110°, 100% arm speed + await robot.move(37) # 37 cm forward + await robot.turn(50) # 50° right + await robot.multitask( + robot.move(25), # 25 cm forward + robot.left_arm.move_by(-60, speed=100) # left arm -60°, 100% arm speed + ) + await robot.turn(40) # 40° right + await robot.move(17, speed=18, acceleration=200) # 17 cm forward, 18% speed, 200 mm/s² accel + await robot.move(-13.5, speed=18, acceleration=200) # 13.5 cm backward, 18% speed, 200 mm/s² accel + await robot.turn(80) # 80° right + await robot.move(8.5, speed=18, acceleration=200) # 8.5 cm forward, 18% speed, 200 mm/s² accel + await robot.multitask( + robot.turn(-36), # 36° left + robot.right_arm.move_by(-140, speed=47) # right arm -140°, 47% arm speed + ) + await robot.move(23, speed=18, acceleration=200) # 23 cm forward, 18% speed, 200 mm/s² accel + await robot.multitask( + robot.right_arm.move_by(160, speed=70), # right arm 160°, 70% arm speed + robot.left_arm.move_by(150, speed=70) # left arm 150°, 70% arm speed + ) + await robot.wait(200) + await robot.move(3, speed=18, acceleration=200) # 3 cm forward, 18% speed, 200 mm/s² accel + await robot.turn(-90) # 90° left + await robot.move(-17, speed=18, acceleration=200) # 17 cm backward, 18% speed, 200 mm/s² accel + await robot.move(10, speed=100) # 10 cm forward, 100% speed + await robot.arc(80, cm=-90, speed=100) # arc: circle on the robot's RIGHT (80 cm radius), BACKWARD 90 cm along it (back end swings right, nose turns left) | 100% speed + robot.stop() # FIXED: was `await drive_base.stop` (crashed) + + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_missions_15_14_ant.py b/regionals/bioglow_missions_15_14_ant.py new file mode 100644 index 0000000..58c2298 --- /dev/null +++ b/regionals/bioglow_missions_15_14_ant.py @@ -0,0 +1,67 @@ +##################################### +# Solution for BioGlow Mission 14,15 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Seed of Renewal and Keystone Species - Parthiv + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + # BLUE COLOR CODE + robot.stop() + robot.reset() + + await robot.left_arm.move_by(-25, speed=100) # left arm -25°, 100% arm speed + await robot.arc(-31.5, degrees=90, speed=74, turn_speed=48, acceleration=200, turn_acceleration=100, then=robot.Stop.HOLD) # arc: circle on the robot's LEFT (31.5 cm radius), FORWARD 90° (nose turns left) | 74% speed, 48% turn speed, 200 mm/s² accel, 100 °/s² turn accel + await robot.move(15, speed=74, acceleration=200, then=robot.Stop.HOLD) # 15 cm forward, 74% speed, 200 mm/s² accel + await robot.turn(90, speed=48, acceleration=100, then=robot.Stop.HOLD) # 90° right, 48% turn speed, 100 °/s² turn accel + await robot.move(23.5, speed=74, acceleration=200, then=robot.Stop.NONE) # 23.5 cm forward, 74% speed, 200 mm/s² accel + await robot.turn(-85, then=robot.Stop.HOLD) # 85° left + await robot.move(9, speed=18, acceleration=200, then=robot.Stop.COAST) # 9 cm forward, 18% speed, 200 mm/s² accel + await robot.move(-9.5, speed=18, acceleration=200) # 9.5 cm backward, 18% speed, 200 mm/s² accel + await robot.turn(-85) # 85° left + await robot.move(-25, speed=18, acceleration=200) # 25 cm backward, 18% speed, 200 mm/s² accel + await robot.turn(60) # 60° right + await robot.move(-17.5, speed=9, acceleration=200) # 17.5 cm backward, 9% speed, 200 mm/s² accel + await robot.move(10, speed=9, acceleration=200) # 10 cm forward, 9% speed, 200 mm/s² accel + await robot.turn(-125, speed=60) # 125° left, 60% turn speed + await robot.left_arm.move_by(21, speed=47) # left arm 21°, 47% arm speed + await robot.turn(-21, speed=60) # 21° left, 60% turn speed + await robot.move(2.5, speed=9, acceleration=200) # 2.5 cm forward, 9% speed, 200 mm/s² accel + await robot.left_arm.move_by(-25, speed=100) # left arm -25°, 100% arm speed + await robot.move(25, speed=92, acceleration=200) # 25 cm forward, 92% speed, 200 mm/s² accel + await robot.turn(-42, speed=48, acceleration=100) # 42° left, 48% turn speed, 100 °/s² turn accel + await robot.move(15, speed=92, acceleration=200) # 15 cm forward, 92% speed, 200 mm/s² accel + await robot.right_arm.move_by(-40, speed=70) # right arm -40°, 70% arm speed + await robot.move(-15, speed=92, acceleration=200, then=robot.Stop.NONE) # 15 cm backward, 92% speed, 200 mm/s² accel + await robot.turn(93, speed=48, acceleration=100, then=robot.Stop.HOLD) # 93° right, 48% turn speed, 100 °/s² turn accel + await robot.move(55, speed=92, acceleration=200, then=robot.Stop.NONE) # 55 cm forward, 92% speed, 200 mm/s² accel + + + + + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_missions_3_4_5.py b/regionals/bioglow_missions_3_4_5.py new file mode 100644 index 0000000..8680526 --- /dev/null +++ b/regionals/bioglow_missions_3_4_5.py @@ -0,0 +1,51 @@ +##################################### +# Solution for BioGlow Missions 3,4,5 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Rishabh + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + await robot.move(45.5) # 45.5 cm forward + await robot.move(-18) # 18 cm backward + await robot.turn(45) # 45° right + await robot.move(50.8) # 50.8 cm forward + await robot.turn(-80) # 80° left + await robot.multitask( + robot.left_arm.move_by(100, speed=47), # left arm 100°, 47% arm speed + robot.right_arm.move_by(-120, speed=47) # right arm -120°, 47% arm speed + ) + + await robot.move(25) # 25 cm forward + await robot.left_arm.move_by(-300, speed=47) # left arm -300°, 47% arm speed + await robot.move(-3.8) # 3.8 cm backward + await robot.turn(30) # 30° right + await robot.right_arm.move_by(120, speed=100) # right arm 120°, 100% arm speed | Was 5000 + await robot.move(4) # 4 cm forward + await robot.arc(-70, degrees=-150) # arc: circle on the robot's LEFT (70 cm radius), BACKWARD 150° (back end swings left, nose turns right) + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_missions_6_7_13_14_experimental.py b/regionals/bioglow_missions_6_7_13_14_experimental.py new file mode 100644 index 0000000..b2217e5 --- /dev/null +++ b/regionals/bioglow_missions_6_7_13_14_experimental.py @@ -0,0 +1,92 @@ +##################################### +# Solution for BioGlow Mission 6,7,13,14 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Leaf cutter frenzy, humungous fungus, etc + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + robot.reset() # always reset drivebase for heading reset especially with turns + + # Forward while raising arms + await robot.multitask( + robot.move(50), # 50 cm forward + robot.right_arm.move_by(20, speed=100), # right arm 20°, 100% arm speed + robot.left_arm.move_by(-10, speed=100) # left arm -10°, 100% arm speed + + ) + # Turn left and lower right arm + await robot.multitask( + robot.turn(-45, speed=48, acceleration=100), # 45° left, 48% turn speed, 100 °/s² turn accel + robot.right_arm.move_by(-70, speed=100) # right arm -70°, 100% arm speed + ) + # approach leaf cutter mission + await robot.multitask( + robot.move(35, speed=100, acceleration=200), # 35 cm forward, 100% speed, 200 mm/s² accel + robot.right_arm.move_by(65, speed=100) # right arm 65°, 100% arm speed + ) + await robot.turn(45, speed=48, acceleration=100) # 45° right, 48% turn speed, 100 °/s² turn accel + await robot.move(10, speed=100, acceleration=200) # 10 cm forward, 100% speed, 200 mm/s² accel + + # solving mycelium + # await multitask( + # right_arm.run_angle(800,70), + # drive_base.straight(10) + # ) + await robot.right_arm.move_by(70, speed=100) # right arm 70°, 100% arm speed + + print('Moving back') + await robot.move(-26) # 26 cm backward + await robot.move(4) # 4 cm forward + await robot.multitask( + robot.turn(55), # 55° right + robot.right_arm.move_by(-200, speed=100) # right arm -200°, 100% arm speed + ) + + #approach Elder tree + await robot.move(22) # 22 cm forward + #await drive_base.turn(-40) + #await drive_base.straight(140) + + #attemp to raise cane + await robot.multitask( + #drive_base.turn(5), + robot.right_arm.move_by(200, speed=100), # right arm 200°, 100% arm speed + robot.move(3), # 3 cm forward + ) + await robot.move(6) # 6 cm forward + await robot.left_arm.move_by(200, speed=100) # left arm 200°, 100% arm speed + + await robot.move(-5) # 5 cm backward + await robot.turn(-45) # 45° left + await robot.move(-60) # 60 cm backward + + #helper.reset_arms() + + robot.stop() + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/bioglow_missions_6_7_experimental.py b/regionals/bioglow_missions_6_7_experimental.py new file mode 100644 index 0000000..9a37a45 --- /dev/null +++ b/regionals/bioglow_missions_6_7_experimental.py @@ -0,0 +1,78 @@ +##################################### +# Solution for BioGlow Mission 6,7 +##################################### + +import robot # drive base, arms, sensors and moves all come from robot.py + +# UNITS used in this file ----------------------------------------------------- +# robot.move(cm) distance in cm (+ forward, - backward) +# robot.turn(degrees) degrees (+ right / clockwise, - left) +# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left), +# then degrees= around the circle or cm= along it (+ forward, - backward) +# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn) +# acceleration= how fast it speeds up and slows down: +# mm/s² for move/arc (normal 500), °/s² for turn (normal 200) +# (² means "per second": 200 mm/s² = 200 mm/s faster every second) +# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed +# ------------------------------------------------------------------------------ + +async def solve_missions(): # Windows to the past + + # --- ARM HOMING (optional) ------------------------------------------- + # Uncomment the next line to push both arms gently against their hard stops + # and call that spot 0, so every arm move starts from the same place each run. + # Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py). + # await robot.home_arms() + + # --- LET THE GYRO SETTLE ---------------------------------------------- + # Wait half a second (and until the hub is perfectly still) before moving, + # so turns are measured from a steady heading. + await robot.settle_gyro() + + robot.reset() # always reset drivebase for heading reset especially with turns + + # Forward while raising arms + await robot.multitask( + robot.move(50), # 50 cm forward + robot.right_arm.move_by(20, speed=100) # right arm 20°, 100% arm speed + ) + # Turn left and lower arms + await robot.multitask( + robot.turn(-45, speed=48, acceleration=100), # 45° left, 48% turn speed, 100 °/s² turn accel + robot.right_arm.move_by(-70, speed=100) # right arm -70°, 100% arm speed + ) + # approach leaf cutter mission + await robot.move(31.5, speed=92, acceleration=200) # 31.5 cm forward, 92% speed, 200 mm/s² accel + await robot.right_arm.move_by(65, speed=100) # right arm 65°, 100% arm speed + await robot.turn(45, speed=48, acceleration=100) # 45° right, 48% turn speed, 100 °/s² turn accel + await robot.move(10, speed=92, acceleration=200) # 10 cm forward, 92% speed, 200 mm/s² accel + + # solving mycelium + # await multitask( + # right_arm.run_angle(800,70), + # drive_base.straight(10) + # ) + await robot.right_arm.move_by(70, speed=100) # right arm 70°, 100% arm speed + + print('Moving back') + await robot.move(-26, speed=92, acceleration=200) # 26 cm backward, 92% speed, 200 mm/s² accel + await robot.move(4, speed=92, acceleration=200) # 4 cm forward, 92% speed, 200 mm/s² accel + await robot.multitask( + robot.turn(90, speed=48, acceleration=100), # 90° right, 48% turn speed, 100 °/s² turn accel + robot.right_arm.move_by(-200, speed=100) # right arm -200°, 100% arm speed + ) + await robot.move(10, speed=92, acceleration=200) # 10 cm forward, 92% speed, 200 mm/s² accel + await robot.turn(-45, speed=48, acceleration=100) # 45° left, 48% turn speed, 100 °/s² turn accel + await robot.move(15, speed=92, acceleration=200) # 15 cm forward, 92% speed, 200 mm/s² accel + await robot.right_arm.move_by(100, speed=100) # right arm 100°, 100% arm speed + + await robot.move(-5, speed=92, acceleration=200) # 5 cm backward, 92% speed, 200 mm/s² accel + await robot.turn(-45, speed=48, acceleration=100) # 45° left, 48% turn speed, 100 °/s² turn accel + await robot.move(-60, speed=92, acceleration=200) # 60 cm backward, 92% speed, 200 mm/s² accel + + #helper.reset_arms() + + robot.stop() + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + robot.run_task(solve_missions()) diff --git a/regionals/movement.py b/regionals/movement.py new file mode 100644 index 0000000..7f642fe --- /dev/null +++ b/regionals/movement.py @@ -0,0 +1,317 @@ +""" +movement.py — easy robot moves in centimeters, degrees and % speed. + +Set it up ONCE (in robot.py, after drive_base is made): + import movement + movement.initialize(drive_base, speed=80, turn_speed=72, + acceleration=500, turn_acceleration=200) + +Use it in any mission (always with await): + await robot.move(30) # forward 30 cm, normal speed + await robot.move(-10, speed=40) # backward 10 cm at 40% + await robot.move(12, speed=18, acceleration=200) # slow and gentle + await robot.turn(90) # turn right 90 degrees + await robot.turn(-45, speed=30) # turn left 45 degrees, slowly + await robot.arc(20, degrees=90) # quarter circle to the right, 20 cm radius + await robot.arc(-35, cm=34) # curve left for 34 cm + await robot.move(2, then=robot.Stop.NONE) # don't stop, flow into the next move + await robot.move(12, speed=18, max_seconds=2) # push a model, but give up after 2 s + +SPEED is a PERCENT of the robot's top speed: 1 = crawl, 100 = as fast as it goes. + move/arc: % of about 544 mm/s. turn: % of about 416 degrees/s. +ACCELERATION is how quickly it speeds up and slows down: + move/arc: mm/s per second. turn: degrees/s per second. + Lower = gentler (less slipping), higher = snappier. +Leave any of them out to use the normal values from robot.py. +A value you give is for THAT move only. The next move is back to normal. + +It's speed, not raw power: the hub keeps the wheels at that speed and +still stops at exactly the right distance, even when the battery is low. + +max_seconds= is a safety time limit for move, turn and arc. If a model or wall +blocks the robot, the move gives up after that many seconds instead of getting +stuck, and the run carries on. The move returns True if it finished, False if +it ran out of time. Leave it out for no time limit. + +then= says what to do at the end of a move (same as Pybricks): + Stop.HOLD (default) stop and hold the position + Stop.COAST stop and roll freely + Stop.NONE don't stop, flow straight into the next move +""" + +from pybricks.parameters import Stop +from pybricks.tools import wait, StopWatch, multitask + +# The DriveBase. Set by initialize(), not here. +drive_base = None + +# Normal values, used when a move doesn't give its own (set in robot.py). +_default_speed = 80 # % for move/arc +_default_turn = 72 # % for turn +_default_acc = 500 # mm/s per second for move/arc +_default_turn_acc = 200 # deg/s per second for turn +_max_speed = None # mm/s, the fastest this robot can drive straight (= 100%) +_max_turn = None # deg/s, the fastest it can turn (= 100%) +_start = None # the values from robot.py, for reset_settings() + + +def initialize(drive, speed=80, turn_speed=72, acceleration=500, turn_acceleration=200): + """Connect the movement library to the robot's DriveBase and set the normal values.""" + global drive_base, _max_speed, _max_turn, _start + drive_base = drive + # Top speeds come from the motors and wheel size, measured by Pybricks itself. + _max_speed = drive.distance_control.limits()[0] + _max_turn = drive.heading_control.limits()[0] + _start = (speed, turn_speed, acceleration, turn_acceleration) + set_default_speed(*_start) + + +def set_default_speed(speed=None, turn_speed=None, acceleration=None, turn_acceleration=None): + """Change the NORMAL values for the rest of this run. Usually not needed: + give speed= or acceleration= on the move itself instead. + """ + global _default_speed, _default_turn, _default_acc, _default_turn_acc + if speed is not None: + _from_percent(speed, 1) # just checks it's 1-100 + _default_speed = speed + if turn_speed is not None: + _from_percent(turn_speed, 1) + _default_turn = turn_speed + if acceleration is not None: + _default_acc = acceleration + if turn_acceleration is not None: + _default_turn_acc = turn_acceleration + _apply() + + +def reset_settings(): + """Go back to the normal values from robot.py. The menu does this before every run.""" + _check() + set_default_speed(*_start) + + +def _apply(speed=None, turn_speed=None, acceleration=None, turn_acceleration=None): + """Send the given values (or the normal ones) to the drive base.""" + drive_base.settings( + straight_speed=_from_percent(_default_speed if speed is None else speed, _max_speed), + straight_acceleration=_default_acc if acceleration is None else acceleration, + turn_rate=_from_percent(_default_turn if turn_speed is None else turn_speed, _max_turn), + turn_acceleration=_default_turn_acc if turn_acceleration is None else turn_acceleration, + ) + + +def _check(): + if drive_base is None: + raise RuntimeError( + "movement isn't set up. Add movement.initialize(drive_base) " + "in robot.py, after drive_base.settings(...)" + ) + + +def _mm(cm): + # Pybricks drops decimals, so round instead: 57.5 cm -> 575 mm + return int(round(cm * 10)) + + +def _from_percent(percent, top, allow_zero=False): + """Turn a percent (like 50) into a real speed (like 272 mm/s).""" + low = 0 if allow_zero else 1 + if not low <= abs(percent) <= 100: + raise ValueError( + "speed is a percent from %d to 100, but got %s" % (low, percent) + ) + return int(top * percent / 100) + + +async def _limited(motion, max_seconds): + """Wait for a drive base move, or give up after max_seconds. True = finished.""" + if max_seconds is None: + await motion + return True + + async def finish(): + await motion + return True + + done, _ = await multitask(finish(), wait(int(max_seconds * 1000)), race=True) + if done is True: + return True + drive_base.stop() # ran out of time: stop pushing and carry on + return False + + +async def move(cm, speed=None, acceleration=None, then=Stop.HOLD, max_seconds=None): + """Drive straight. + + cm: distance in centimeters. Positive = forward, negative = backward. + speed: % of top speed (1-100). Leave out for normal. + acceleration: mm/s per second. Leave out for normal. + then: Stop.HOLD (default), Stop.COAST, or Stop.NONE to flow into the next move. + max_seconds: give up after this long (for pushes that might get blocked). + """ + _check() + _apply(speed=speed, acceleration=acceleration) + return await _limited(drive_base.straight(_mm(cm), then=then), max_seconds) + + +async def turn(degrees, speed=None, acceleration=None, then=Stop.HOLD, max_seconds=None): + """Turn in place. + + degrees: positive = right (clockwise), negative = left. + speed: % of top turning speed (1-100). Leave out for normal. + acceleration: degrees/s per second. Leave out for normal. + then: Stop.HOLD (default), Stop.COAST, or Stop.NONE. + max_seconds: give up after this long. + """ + _check() + _apply(turn_speed=speed, turn_acceleration=acceleration) + return await _limited(drive_base.turn(degrees, then=then), max_seconds) + + +async def arc(radius_cm, degrees=None, cm=None, speed=None, turn_speed=None, + acceleration=None, turn_acceleration=None, then=Stop.HOLD, max_seconds=None): + """Drive along part of a circle. Give degrees OR cm, not both. + + radius_cm: size of the circle in cm. + Positive = circle to the RIGHT, negative = circle to the LEFT. + degrees: how far around the circle to go. + Positive = forward, negative = backward. + cm: or: how far to drive along the circle, in cm. + Positive = forward, negative = backward. + speed, acceleration: limits for driving along the circle. + turn_speed, turn_acceleration: limits for how fast it turns while curving. + then: Stop.HOLD (default), Stop.COAST, or Stop.NONE. + max_seconds: give up after this long. + + Examples: + await robot.arc(20, degrees=90) # quarter circle forward, curving right + await robot.arc(-35, degrees=90) # quarter circle forward, curving left + await robot.arc(128, cm=34) # 34 cm along a big right-hand curve + """ + _check() + if (degrees is None) == (cm is None): + raise ValueError("arc needs degrees=... or cm=..., but not both") + _apply(speed, turn_speed, acceleration, turn_acceleration) + if cm is None: + motion = drive_base.arc(_mm(radius_cm), angle=degrees, then=then) + else: + motion = drive_base.arc(_mm(radius_cm), distance=_mm(cm), then=then) + return await _limited(motion, max_seconds) + + +async def move_for_seconds(seconds, speed=None, backward=False, acceleration=None): + """Drive straight for a set time, then stop. + + seconds: how long to drive, like 1.5 + speed: % of top speed (1-100). Leave out for normal. + backward: True to drive backward. + acceleration: mm/s per second. Leave out for normal. + + Good for pushing a mission model. For exact distances, use move() instead. + """ + _check() + _apply(acceleration=acceleration) + mm_s = _from_percent(_default_speed if speed is None else speed, _max_speed) + drive_base.drive(-mm_s if backward else mm_s, 0) + try: + await wait(int(seconds * 1000)) + finally: + drive_base.brake() # also stops if a multitask race cancels us + + +async def move_until_stalled(speed=None, backward=False, max_seconds=5, acceleration=None): + """Drive straight until something blocks the robot, then stop. + + "Blocked" means EITHER Pybricks says the wheels are stalled, OR the robot + has almost stopped moving forward (so it also works on light, springy + models that never make the motors work at full power). + It can't notice wheels spinning in place, so max_seconds always applies. + + speed: % of top speed (1-100). Slower is gentler: try 20-40. + backward: True to drive backward. + max_seconds: give up after this long. + acceleration: mm/s per second. Leave out for normal. + + Returns True if it got blocked, False if it gave up. + Example: + hit = await move_until_stalled(speed=30, backward=True) + if not hit: + print("Never reached the wall!") + """ + _check() + _apply(acceleration=acceleration) + mm_s = _from_percent(_default_speed if speed is None else speed, _max_speed) + drive_base.drive(-mm_s if backward else mm_s, 0) + + acc = _default_acc if acceleration is None else acceleration + ramp_ms = int(1000 * mm_s / acc) + 100 # time to reach full speed, plus a moment + window_ms = 200 # check progress every 0.2 s + min_progress = max(1, mm_s * window_ms / 1000 / 4) # under 1/4 of normal = stuck + + timer = StopWatch() + window = StopWatch() + last = drive_base.distance() + try: + await wait(100) # give the wheels a moment to start rolling + while timer.time() < max_seconds * 1000: + if drive_base.stalled(): + return True + if window.time() >= window_ms: + now = drive_base.distance() + if timer.time() > ramp_ms and abs(now - last) < min_progress: + return True # barely moving: something is in the way + last = now + window.reset() + await wait(10) + return False + finally: + drive_base.stop() # stop pushing, even if a race cancels us + + +def drive(speed, steer=0): + """Start driving and keep going until stop() is called. + + speed: percent of top speed, -100 to 100. Negative = backward. + steer: percent of top turning speed, -100 to 100. Positive = steer right. + Doesn't wait, so don't use await on it. Example: + drive(40) + while await lazer_ranger.distance() > 100: + await wait(10) + stop() + """ + _check() + _apply() + drive_base.drive( + _from_percent(speed, _max_speed, allow_zero=True), + _from_percent(steer, _max_turn, allow_zero=True), + ) + + +def stop(): + """Stop the wheels and let the robot roll to a stop.""" + _check() + drive_base.stop() + + +def brake(): + """Stop the wheels quickly.""" + _check() + drive_base.brake() + + +def reset(): + """Set the distance and angle counters back to 0 (same as drive_base.reset(0, 0)).""" + _check() + drive_base.reset() + + +def distance_cm(): + """How far the robot has driven since the last reset(), in cm.""" + _check() + return drive_base.distance() / 10 + + +def angle(): + """How far the robot has turned since the last reset(), in degrees.""" + _check() + return drive_base.angle() diff --git a/regionals/robot.py b/regionals/robot.py new file mode 100644 index 0000000..04afad6 --- /dev/null +++ b/regionals/robot.py @@ -0,0 +1,116 @@ +""" +robot.py — everything about OUR robot, in one place. +(Replaces bioglow_robot_settings.py.) + +Every mission starts with: + import robot + +Then: + await robot.move(55) # forward 55 cm + await robot.turn(-90) # left 90 degrees + await robot.arc(-35, degrees=90) # curve left + await robot.left_arm.move_by(100, speed=47) # arms (see arm.py) + robot.color_sensor, robot.lazer_ranger, robot.hub + +RULE: motors and sensors are created ONLY in this file. +Creating Motor(Port.A) anywhere else gives a "port already in use" error. + +Important: test all code with the battery at 100%, and make all updates +with the robot at full charge. +""" + +from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor +from pybricks.parameters import Button, Color, Direction, Port, Side, Stop +from pybricks.tools import run_task, multitask, wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub +import umath + +# ---------------------------------------------------------------- hub +hub = PrimeHub() + +# ---------------------------------------------------------------- wheels +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B, Direction.CLOCKWISE) + +WHEEL_DIAMETER = 62.4 # mm (measure your wheels) +AXLE_TRACK = 150 # mm (distance between the two wheels) + +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.use_gyro(True) + +# ---------------------------------------------------------------- arms +# Gear trains: both trains go in ONE list. 2.33:1 reduction, so top speed is ~430 deg/s. +left_arm_motor = Motor(Port.C, Direction.CLOCKWISE, [[20, 28], [12, 20]]) +right_arm_motor = Motor(Port.D, Direction.CLOCKWISE, [[20, 28], [12, 20]]) + +# Each arm gets % speeds, named positions and homing (see arm.py). +# Before using home() or move_to(), on the robot: +# 1. set home_backward: which way each arm turns to reach its hard stop +# 2. add positions, measured from that stop (run arm_calibrate.py), +# for example positions={"down": 0, "up": 120} +from arm import Arm +left_arm = Arm(left_arm_motor, speed=50, positions={}, home_backward=True) +right_arm = Arm(right_arm_motor, speed=50, positions={}, home_backward=False) + +# ---------------------------------------------------------------- sensors +lazer_ranger = UltrasonicSensor(Port.E) # mounted at the BACK of the robot +color_sensor = ColorSensor(Port.F) + +# ---------------------------------------------------------------- moves +# movement.py does the work, so robot.move(55) means "drive 55 cm". +# +# NORMAL SPEEDS AND ACCELERATIONS. Used whenever a move doesn't give its own: +# await robot.move(30) -> normal values +# await robot.move(30, speed=40, acceleration=200) -> just for this move +# Speeds are % of top speed: 100% straight = about 544 mm/s, 100% turning = about 416 deg/s. +DEFAULT_SPEED = 80 # % for move() and arc() (about 435 mm/s) +DEFAULT_TURN_SPEED = 72 # % for turn() (about 300 deg/s) +DEFAULT_ACCELERATION = 500 # mm/s per second, move() and arc() +DEFAULT_TURN_ACCELERATION = 200 # deg/s per second, turn() + +import movement +movement.initialize(drive_base, speed=DEFAULT_SPEED, turn_speed=DEFAULT_TURN_SPEED, + acceleration=DEFAULT_ACCELERATION, + turn_acceleration=DEFAULT_TURN_ACCELERATION) +from movement import (move, turn, arc, move_for_seconds, move_until_stalled, + drive, stop, brake, reset, distance_cm, angle, + set_default_speed, reset_settings) + + +async def settle_gyro(ms=500, max_seconds=2): + """Let the gyro settle before a run starts. + + Waits ms milliseconds (default half a second), then keeps waiting until the + hub says it's perfectly still, so the first turn starts from a steady heading. + Gives up after max_seconds, so a run never gets stuck here. + """ + await wait(ms) + timer = StopWatch() + while timer.time() < max_seconds * 1000: + still = hub.imu.stationary() + ready = hub.imu.ready() if hasattr(hub.imu, "ready") else True + if still and ready: + return + await wait(10) + + +async def home_arms(): + """Find both arms' starting positions. Check home_backward first!""" + await multitask(left_arm.home(), right_arm.home()) + + +async def lift_up(degrees, speed=None): + """Raise both arms together (they face opposite ways). speed is in %.""" + await multitask( + right_arm.move_by(degrees, speed), + left_arm.move_by(-degrees, speed), + ) + + +async def lift_down(degrees, speed=None): + """Lower both arms together. speed is in %.""" + await multitask( + right_arm.move_by(-degrees, speed), + left_arm.move_by(degrees, speed), + )