from pybricks.tools import wait, StopWatch, hub_menu
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Stop, Button, Direction
from pybricks.robotics import DriveBase
# Debug flag - set to True to enable console logging
DEBUG = True
# -------------------------
# Hardware initialization
# -------------------------
hub = PrimeHub()
# Drive motors - configured with opposite directions for differential drive
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.C, Direction.CLOCKWISE)
# Arm motors for manipulating game pieces
arm_motor_left = Motor(Port.E)
arm_motor_right = Motor(Port.D)
# DriveBase settings (measure your robot and adjust)
# wheel_diameter: 56mm wheels
# axle_track: 120mm distance between wheels
robot = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=120)
# -------------------------
# Global heading tracker
# -------------------------
# Tracks the robot's intended heading throughout the mission
current_heading = 0 # logical heading in degrees
# Minimum turn speed to overcome motor friction
min_speed = 20
# -------------------------
# Mission Selection Menu
# -------------------------
# Display menu on hub to choose which mission to run
# Options: "0", "1", "2", "3", "C", "B", "A"
selected = hub_menu("0", "1", "2", "3", "C", "B", "A")
# -------------------------
def log(msg):
"""Print debug messages if DEBUG is enabled"""
if DEBUG:
print(msg)
def normalize_angle(angle):
"""
Normalize angle (degrees) into range (-180, 180]
This ensures angles wrap correctly around 360°
Examples:
normalize_angle(190) -> -170
normalize_angle(-190) -> 170
normalize_angle(180) -> 180
normalize_angle(-180) -> -180
"""
a = angle % 360
if a > 180:
a -= 360
return a
# -------------------------
# Helper motion functions
# -------------------------
def realign_to_heading(target_heading, max_speed=60, deadband=0.6, dt=15, Kp=2.0):
"""
Realign robot to a specific absolute heading using proportional control
Args:
target_heading: Desired heading in degrees (0-360)
max_speed: Maximum turn speed
deadband: Acceptable error in degrees before stopping
dt: Control loop delay in milliseconds
Kp: Proportional gain for turn correction
"""
global current_heading
while True:
# Get current heading from IMU
current = hub.imu.heading()
# Calculate shortest path to target (handles wrap-around)
error = (target_heading - current)
if error > 180:
error -= 360
elif error < -180:
error += 360
# Exit if within acceptable error
if abs(error) <= deadband:
break
# Apply proportional control with speed limits
turn_speed = max(-max_speed, min(max_speed, Kp * error))
robot.drive(0, turn_speed)
wait(dt)
robot.stop()
current_heading = target_heading # update logical heading
def gyro_straight(distance_mm, speed=120, Kp=1.2, deadband=1.0, dt=20, timeout=None):
"""
Drive straight for a specific distance using gyro feedback (basic P control)
Args:
distance_mm: Distance to travel in millimeters (negative for backwards)
speed: Drive speed
Kp: Proportional gain for heading correction
deadband: Acceptable heading error in degrees
dt: Control loop delay in milliseconds
timeout: Maximum time in milliseconds (None for no timeout)
"""
global current_speed
start_heading = hub.imu.heading()
robot.reset() # Reset distance tracker
watch = StopWatch()
target = abs(distance_mm)
direction = 1 if distance_mm > 0 else -1
while abs(robot.distance()) < target:
# Check timeout if enabled
if timeout is not None and watch.time() > timeout:
break
# Calculate heading error
angle = hub.imu.heading()
error = angle - start_heading
if error > 180:
error -= 360
elif error < -180:
error += 360
# Apply correction turn to maintain straight line
correction = 0 if abs(error) < deadband else -Kp * error
robot.drive(direction * speed, correction)
wait(dt)
# Log final drift for tuning purposes
final_error = normalize_angle(hub.imu.heading() - start_heading)
log(f"Drift: {final_error:.2f}°")
robot.stop()
current_speed = 0
def dynamic_pid(speed):
"""
Return appropriate PID gains based on speed
Slower speeds need tighter control, faster speeds need damping
Returns:
(Kp, Kd): Proportional and derivative gains
"""
if speed < 80:
return 2.35, 1.0
if speed < 100:
return 1.0, 0.3 # slow + precise
if speed < 200:
return 1.40, 0.35
if speed < 300:
return 1.7, 0.6
return 2.0, 0.9
# Global speed tracker for ramping
current_speed = 0
# Commented out old PD controller - kept for reference
# def gyro_straight_pd(distance_mm, speed=150, deadband=1.0, dt=20, timeout=None):
# ... (old implementation)
def gyro_straight_pd(distance_mm, speed=150, deadband=1.0, dt=20, timeout=None):
"""
Optimized gyro-based straight drive with PD control.
Uses both proportional and derivative terms for smoother, more stable motion.
Key improvements:
- Pre-computed constants for efficiency
- Early exit conditions
- Improved angle normalization
- Better derivative handling on first iteration
- Distance calculation optimization
Args:
distance_mm: Distance to travel in millimeters (negative for backwards)
speed: Target drive speed
deadband: Acceptable heading error in degrees
dt: Control loop delay in milliseconds
timeout: Maximum time in milliseconds (None for no timeout)
"""
global current_speed
# Pre-compute constants
Kp, Kd = dynamic_pid(speed)
Kd_scaled = Kd / (dt / 1000) # Pre-scale Kd by dt
start_heading = hub.imu.heading()
target = abs(distance_mm)
direction = 1 if distance_mm > 0 else -1
# Initialize variables
robot.reset()
last_error = 0
first_iteration = True
# Timeout handling
if timeout is not None:
watch = StopWatch()
while True:
# Distance check (primary exit condition)
traveled = abs(robot.distance())
if traveled >= target:
break
# Timeout check (only if enabled)
if timeout is not None and watch.time() > timeout:
break
# Get current heading and normalize error in one step
error = hub.imu.heading() - start_heading
# Normalize to [-180, 180] - optimized
if error > 180:
error -= 360
elif error < -180:
error += 360
# Calculate correction
if abs(error) < deadband:
correction = 0
derivative = 0 # Reset derivative when in deadband
else:
# Skip derivative on first iteration to avoid spike
if first_iteration:
derivative = 0
first_iteration = False
else:
derivative = (error - last_error) * Kd_scaled
correction = -(Kp * error + derivative)
# Apply ramping and drive
current_speed = ramp_speed(current_speed, speed)
robot.drive(direction * current_speed, correction)
last_error = error
wait(dt)
# Final drift logging
final_error = hub.imu.heading() - start_heading
if final_error > 180:
final_error -= 360
elif final_error < -180:
final_error += 360
log(f"Drift: {final_error:.2f}°, Distance: {robot.distance():.1f}mm")
robot.stop()
current_speed = 0
# Additional optimization: Cache PID values if calling frequently with same speed
_pid_cache = {}
def dynamic_pid_cached(speed):
"""Cached version of dynamic_pid for repeated calls with same speed."""
if speed not in _pid_cache:
_pid_cache[speed] = dynamic_pid(speed)
return _pid_cache[speed]
def gyro_straight_pdt(distance_mm, speed=150, deadband=1.0, dt=20, timeout=3000):
"""
PD straight drive with default timeout of 3000ms
Same as gyro_straight_pd but with timeout always enabled
"""
global current_speed
Kp, Kd = dynamic_pid(speed)
start_heading = hub.imu.heading()
robot.reset()
last_error = 0
watch = StopWatch()
target = abs(distance_mm)
direction = 1 if distance_mm > 0 else -1
while abs(robot.distance()) < target:
if timeout is not None and watch.time() > timeout:
break
angle = hub.imu.heading()
error = angle - start_heading
if error > 180:
error -= 360
elif error < -180:
error += 360
if abs(error) < deadband:
derivative = 0
correction = 0
else:
derivative = (error - last_error) / (dt / 1000)
correction = -(Kp * error + Kd * derivative)
current_speed = ramp_speed(current_speed, speed)
robot.drive(direction * current_speed, correction)
last_error = error
wait(dt)
final_error = normalize_angle(hub.imu.heading() - start_heading)
log(f"Drift: {final_error:.2f}°")
robot.stop()
current_speed = 0
def gyro_turnt(target_angle, max_speed=80, Kp=2.0, deadband=0.6, timeout=3000, dt=5):
"""
Turn to an absolute heading using proportional control
Implements minimum speed to overcome friction
Args:
target_angle: Desired heading in degrees (0-360)
max_speed: Maximum turn speed
Kp: Proportional gain
deadband: Acceptable error in degrees
timeout: Maximum time in milliseconds
dt: Control loop delay in milliseconds
"""
global current_heading
watch = StopWatch()
while True:
current = hub.imu.heading()
# Calculate shortest path to target
error = target_angle - current
if error > 180:
error -= 360
elif error < -180:
error += 360
# Exit if within deadband
if abs(error) <= deadband:
break
# Exit on timeout
if watch.time() > timeout:
log("⚠️ gyro_turnt timeout — continuing")
break
# Apply proportional control with minimum speed threshold
# Ensures motor has enough power to actually turn
turn_speed = max(min_speed, min(max_speed, abs(Kp*error))) * (1 if error > 0 else -1)
robot.drive(0, turn_speed)
wait(dt)
robot.stop()
current_heading = target_angle # update logical heading
def gyro_turntlong(target_angle, max_speed=80, Kp=2.0, deadband=0.6, timeout=5000, dt=5):
"""
Turn to absolute heading with longer timeout (5000ms vs 3000ms)
Useful for larger turns that need more time
"""
global current_heading
watch = StopWatch()
while True:
current = hub.imu.heading()
error = target_angle - current
if error > 180:
error -= 360
elif error < -180:
error += 360
if abs(error) <= deadband:
break
if watch.time() > timeout:
log("⚠️ gyro_turnt timeout — continuing")
break
turn_speed = max(min_speed, min(max_speed, abs(Kp*error))) * (1 if error > 0 else -1)
robot.drive(0, turn_speed)
wait(dt)
robot.stop()
current_heading = target_angle # update logical heading
# -------------------------
# Optim helpers
# -------------------------
def ramp_speed(current, target, step=10):
"""
Smoothly move current speed toward the target speed.
Prevents sudden acceleration/deceleration that can cause slipping.
Args:
current: Current speed
target: Desired speed
step: Maximum speed change per call (default 10)
Returns:
New speed value between current and target
"""
if current < target:
return min(current + step, target)
else:
return max(current - step, target)
# -------------------------
# Arm helpers
# -------------------------
def rotate_leftarm(degrees=90, speed=150, max_time=3000):
"""
Rotate left arm and wait for completion (blocking)
Args:
degrees: Rotation angle (positive or negative)
speed: Rotation speed
max_time: Maximum wait time in milliseconds
"""
watch = StopWatch()
arm_motor_left.run_angle(speed, degrees, Stop.HOLD, wait=False)
while not arm_motor_left.control.done():
if watch.time() > max_time:
log("⚠️ Left arm timeout reached — stopping")
arm_motor_left.stop()
break
wait(10)
arm_motor_left.stop()
def rotate_leftarmnoblock(degrees=90, speed=150):
"""
Start left arm rotation without waiting (non-blocking)
Useful for simultaneous arm+drive movements
"""
arm_motor_left.run_angle(speed, degrees, Stop.HOLD, wait=False)
def rotate_leftarmcoast(degrees=90, speed=100, max_time=3000):
"""
Rotate left arm and coast (cut power) immediately when target reached
Prevents the motor from holding position, allowing for smoother motion
"""
start_angle = arm_motor_left.angle()
target = start_angle + degrees
watch = StopWatch()
arm_motor_left.run_target(speed, target, wait=False)
while True:
# Angle reached? (Ignore PID settling)
if abs(arm_motor_left.angle() - target) <= 2:
log("Left arm angle reached — cutting motor immediately")
arm_motor_left.dc(0) # << key change - cuts power
break
# Timeout reached?
if watch.time() > max_time:
log("⚠️ Left arm TIMEOUT — stopping early")
arm_motor_left.dc(0)
break
wait(5)
log(f"Arm moved {arm_motor_left.angle()-start_angle}° (requested {degrees}°)")
def rotate_rightarmnoblock(degrees=90, speed=100, max_time=3000):
"""
Start right arm rotation without waiting (non-blocking)
"""
arm_motor_right.run_angle(speed, degrees, Stop.HOLD, wait=False)
def rotate_rightarm(degrees=90, speed=100, max_time=3000):
"""
Rotate right arm and wait for completion (blocking)
"""
watch = StopWatch()
arm_motor_right.run_angle(speed, degrees, Stop.HOLD, wait=False)
while not arm_motor_right.control.done():
if watch.time() > max_time:
log("⚠️ Right arm timeout reached — stopping")
arm_motor_right.stop()
break
wait(10)
arm_motor_right.stop()
log(f"Right arm angle = {arm_motor_right.angle()}")
def rotate_rightarmcoast(degrees=90, speed=100, max_time=3000):
"""
Rotate right arm and coast (cut power) immediately when target reached
"""
start_angle = arm_motor_right.angle()
target = start_angle + degrees
watch = StopWatch()
arm_motor_right.run_target(speed, target, wait=False)
while True:
# Angle reached? (Ignore PID settling)
if abs(arm_motor_right.angle() - target) <= 2:
log("Right arm angle reached — cutting motor immediately")
arm_motor_right.dc(0) # << key change - cuts power
break
# Timeout reached?
if watch.time() > max_time:
log("⚠️ Right arm TIMEOUT — stopping early")
arm_motor_right.dc(0)
break
wait(5)
log(f"Arm moved {arm_motor_right.angle()-start_angle}° (requested {degrees}°)")
def hammer_arm(strike_angle=180, speed=1200, repeats=5, pause=100):
"""
Repeatedly strike/hammer with left arm
Useful for hitting buttons or knocking objects
Args:
strike_angle: How far to push forward
speed: Strike speed (high for impact)
repeats: Number of strikes
pause: Delay between strikes in milliseconds
"""
for _ in range(repeats):
# Push forward relative
arm_motor_left.run_angle(speed, -strike_angle, Stop.HOLD, wait=True)
wait(pause)
# Return back relative
arm_motor_left.run_angle(speed, strike_angle, Stop.HOLD, wait=True)
wait(pause)
def run_mission(steps):
"""
Execute a mission defined as a list of action steps
Each step is a tuple: (action, value, speed)
- action: String describing what to do
- value: Action parameter (distance, angle, time, etc.)
- speed: Motor speed for the action
Supported actions:
- 'straight': Basic straight drive
- 'straightnotimeout': Straight drive with no timeout
- 'straightimeout': Straight drive with 4s timeout
- 'straightpd': Straight drive with PD control
- 'straightpdt': Straight drive with PD and default timeout
- 'turn': Relative turn (adds to current heading)
- 'turnt': Absolute turn to specific heading
- 'turntpd': Turn with PD control
- 'turntpdlargedeadband': Turn with larger acceptable error
- 'turntpdfast': Fast turn
- 'turntpdlong': Turn with longer timeout
- 'larm': Left arm blocking
- 'noblocklarm': Left arm non-blocking
- 'noblockrarm': Right arm non-blocking
- 'rarm': Right arm blocking
- 'rarmcoast': Right arm with coast
- 'larmcoast': Left arm with coast
- 'rarmhightimeout': Right arm with 10s timeout
- 'hammerstrike': Hammer motion
- 'wait': Pause for specified milliseconds
- 'heading': Realign to absolute heading
"""
global current_heading
# Reset IMU and heading at mission start
hub.imu.reset_heading(0)
current_heading = 0
mission_timer = StopWatch()
mission_timer.reset()
log("Starting mission; IMU heading reset to 0")
# Execute each step in sequence
for i, (action, value, speed) in enumerate(steps):
step_timer = StopWatch()
log(f"Step {i+1}/{len(steps)}: {action}, value={value}, speed={speed}")
if action == 'straight':
gyro_straight(value, speed=speed, Kp=1.5)
elif action == 'straightnotimeout':
gyro_straight(value, speed=speed, Kp=1.5, timeout=None)
elif action == 'straightimeout':
gyro_straight(value, speed=speed, Kp=1.5, timeout=4000)
elif action == 'straightpd':
gyro_straight_pd(value, speed=speed)
elif action == 'straightpdt':
gyro_straight_pdt(value, speed=speed)
elif action == 'turn':
# Relative turn: add to current heading
target = (current_heading + value) % 360
gyro_turnt(target, max_speed=speed)
elif action == 'turnt':
# Absolute turn to specific heading
gyro_turnt(value, max_speed=speed)
elif action == 'turntpd':
target = (current_heading + value) % 360
gyro_turnt(target, max_speed=speed)
elif action == 'turntpdlargedeadband':
# Turn with larger acceptable error (3° vs 0.6°)
target = (current_heading + value) % 360
gyro_turnt(target, max_speed=speed, deadband=3.0)
elif action == 'turntpdfast':
target = (current_heading + value) % 360
gyro_turn_fast(target, max_speed=speed)
elif action == 'turntpdlong':
# Turn with longer timeout
target = (current_heading + value) % 360
gyro_turntlong(target, max_speed=speed)
elif action == 'larm':
# Left arm with realignment after
pre_heading = current_heading
rotate_leftarm(degrees=value, speed=speed)
realign_to_heading(pre_heading)
elif action == 'noblocklarm':
# Non-blocking left arm with realignment
pre_heading = current_heading
rotate_leftarmnoblock(degrees=value, speed=speed)
realign_to_heading(pre_heading)
elif action == 'noblockrarm':
# Non-blocking right arm with realignment
pre_heading = current_heading
rotate_rightarmnoblock(degrees=value, speed=speed)
realign_to_heading(pre_heading)
elif action == 'rarm':
# Right arm with realignment after
pre_heading = current_heading
rotate_rightarm(degrees=value, speed=speed)
realign_to_heading(pre_heading)
elif action == 'rarmcoast':
# Right arm coast (no realignment)
pre_heading = current_heading
rotate_rightarmcoast(degrees=value, speed=speed)
elif action == 'larmcoast':
# Left arm coast (no realignment)
pre_heading = current_heading
rotate_leftarmcoast(degrees=value, speed=speed)
elif action == 'rarmhightimeout':
# Right arm with extended 10s timeout
pre_heading = current_heading
rotate_rightarm(degrees=value, speed=speed, max_time=10000)
elif action =="hammerstrike":
# Hammer motion (value = number of strikes)
hammer_arm(strike_angle=300, speed=speed, repeats=value, pause=200)
elif action == 'wait':
# Pause for specified milliseconds
wait(value)
elif action == "heading":
# Realign to specific heading
realign_to_heading(value)
else:
log(f"Unknown action: {action}")
log(f" ✓ Step completed in {step_timer.time()/1000:.2f}s")
robot.stop()
log(f"Mission completed in {mission_timer.time()/1000:.2f}s")
wait(400)
# -------------------------
# Missions (lists of actions)
# -------------------------
if selected == "B":
# Mission B: Green heavy lift test
# Likely manipulates heavy game pieces on green area
mission_green_heavylifttest = [
("straightpd", 440, 180),
("turntpd", 41, 40),
("larmcoast", -270, 250),
("turntpd", 0, 20),
("straightpd", 12, 60),
("larm", 260, 300),
("rarm", -50, 200),
("straightpd", -4, 20),
("turntpd", -27, 40),
("straightpd", 15, 60),
("turntpdlong", -40, 70),
("straightpd", -35, 100),
("turntpd", 25, 70),
("noblocklarm", 130, 150),
("straightpd", -450, 320),
]
run_mission(mission_green_heavylifttest)
if selected == "C":
# Mission C: Yellow silo
# Uses hammer strike, likely for hitting buttons or knocking objects
mission_yellow_silo = [
("straight", 150, 200),
("hammerstrike", 4, 1200), # 4 hammer strikes at high speed
("turntpd", -20, 60),
("straight", 310, 200),
("turntpd", -60, 40),
("straight", 160, 200),
("turntpd", 70, 40),
("straight", 10, 80),
# Commented out return path
# ("wait", 500, 0),
# ("straight", -20, 80),
# ("turntpd", -38, 40),
# ("straight", 490, 200),
# ("turntpd", -26, 40),
# ("straight", -166, 160),
]
run_mission(mission_yellow_silo)
if selected == "A":
# Mission A: Red tip on sale
# Complex arm movements for precise manipulation
mission_red_tiponsale5 = [
("straightpd", 40, 100),
("turntpd", -44, 70),
("straightpd", 130, 160),
("rarm", -140, 250),
("straightpd", 95, 100),
("noblocklarm", -300, 250), # Non-blocking for speed
("rarm", 80, 80),
("wait", 150, 0),
("rarm", -70, 120),
("straightpd", -100, 100),
("turntpdlargedeadband", -15, 60), # Large deadband for speed
("noblocklarm", 320, 250),
("straight", -200, 300),
]
run_mission(mission_red_tiponsale5)
if selected == "1":
# Mission 1: Mineshaft combined aligned
# Navigates to and manipulates mineshaft area
mission_mineshaftcombined_aligned = [
("straightpd", 180, 200),
("turntpd", 90, 50),
("straightpd", 30, 60),
("turntpd", 90, 50),
("straightpd", -405, 200),
("turntpd", -90, 35),
("straightpd", 70, 60),
("noblocklarm", -180, 300), # Both arms move simultaneously
("noblockrarm", -55, 500),
("straightpd", -70, 40),
("turntpdlargedeadband", 100, 60),
("straightpd", 450, 300),
]
run_mission(mission_mineshaftcombined_aligned)
# Additional mission definitions (not currently selected)
# From blue first dinosaur/seal lift
mission_green_liftartifactsonly3 = [
("straightpd", 535, 180),
("turntpd", 37, 50),
('larm', 360, 150),
("straightpd", 115, 100),
('larm', -200, 180),
('larm', -50, 180),
('larm', -30, 150),
("wait", 2000, 0), # 2 second wait for settling
("turntpd", -37, 50),
("straightpd", -40, 100),
('noblocklarm', -30, 250),
("turntpd", -90, 50),
("straightpd", 100, 100),
("turntpd", -12, 20),
("rarmhightimeout", 4000, 800), # Extended timeout for difficult movement
("turntpd", 10, 40),
("straightpd", -30, 120),
("turntpd", 95, 20),
("straightpd", 450, 300),
]
# Working green lift artifacts in reverse order from blue
mission_green_liftartifactsonly = [
("straightpd", -485, 200),
("turntpd", 100, 50),
("straightpd", 20, 100),
("turntpd", -16, 10),
("rarmhightimeout", 4000, 800),
("turntpd", 10, 40),
("straightpd", -40, 120),
("turntpd", 80, 20),
("straightpd", 10, 100),
("turntpd", 43, 50),
('larm', 380, 150),
("straightpd", 32, 100),
('larm', -200, 180),
('larm', -50, 180),
('larm', -30, 150),
("wait", 2000, 0),
("straightpd", -20, 150),
("turntpd", -45, 50),
("straightpd", 250, 300),
]
if selected == "2":
# Mission 2: Ship push
# Simple forward and back mission, possibly to push a ship game piece
mission_shippush = [
("straightpd", 330, 150),
# Commented out arm movement
#('larm', -90),
("straightpd", -330 , 150)
]
run_mission(mission_shippush)
if selected == "0":
# Mission 0: Blue plants
# Manipulates plants/objects in blue area
mission_blue_plants = [
("straightpd", 350, 220),
("turntpd", -34, 50),
("straightpdt", 110, 100), # Using timeout version
("larmcoast", 150, 350), # Coast for smoother motion
("straightpd", -60, 60),
("rarmcoast", -200, 600),
("rarmcoast", 250, 700),
("turntpdlargedeadband", 45, 80),
("straightpd", -380, 220),
]
run_mission(mission_blue_plants)
if selected == "3":
# Mission 3: Aligned reverse mission lift artifact
# Complex mission for lifting artifacts with precise positioning
# Commented out old version
# alignedreversemission_liftartifact = [
# ...
# ]
alignedreversemission_liftartifact = [
("straightpd", 120, 200),
("turntpd", 90, 50),
("straightpd", 280, 150),
("turntpd", -105, 50),
('larm', 330, 200),
("straightpd", 35, 50),
("larmcoast", -270, 180),
("wait", 500, 0),
("straightpd", -10, 70),
("turntpd", 105, 60),
('noblocklarm', -100, 250),
("straightpd", 150, 150),
("turntpd", 45, 50),
("straightpd", -50, 50),
("turntpd", 45, 40),
("wait", 300, 0),
("straightpd", 35, 70),
("wait", 300, 0),
("turntpd", -10, 10),
("rarmhightimeout", 4000, 800), # High timeout for reliable completion
("turntpd", 10, 40),
("straightpd", -35, 120),
("turntpdlargedeadband", -70, 90),
("straightpd", 500, 350),
]
run_mission(alignedreversemission_liftartifact)
# Commented out mission - kept for reference
# if selected == "2":
# alignedreversemission_liftartifactflag = [
# ...
# ]
# run_mission(alignedreversemission_liftartifactflag)