1 Commits

Author SHA1 Message Date
56c24cdb8c Update discovery-phase/main.py 2026-10-09 20:40:07 +00:00
6 changed files with 491 additions and 370 deletions

View File

@@ -1,6 +1,416 @@
''' """
PID Straight Drive — heading-corrected forward / backward movement.
This is where you write the main code to start all mission codes Improvements over baseline:
Combine all call here • 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 pybricks.tools import StopWatch, wait
import umath
WHEEL_DIAMETER = 68.8 # mm — adjust to your wheel (Done)
AXLE_TRACK = 180 # mm — used only if you add turning later (Done)
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()
#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
import umath
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
from pybricks.tools import wait, StopWatch
from pybricks.robotics import DriveBase
from pybricks.hubs import PrimeHub
# Initialize hub and devices
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F)
# DriveBase configuration
WHEEL_DIAMETER = 62.4 # mm (adjust for your wheels)
AXLE_TRACK = 150 # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True)
"""
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 forward continuously using DriveBase."""
drive_base.drive(400, 0)
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)
# Use this to set default
def set_default_speed():
drive_base.settings(600, 500, 300, 200)
# Use this to change drive base movement
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
async def multilift_up(rotation_angle):
await multitask(
right_arm.run_angle(400,rotation_angle),
left_arm.run_angle(400,-1*rotation_angle)
)
async def multilift_down(rotation_angle):
await multitask(
right_arm.run_angle(1000,-1*rotation_angle),
left_arm.run_angle(1000,rotation_angle)
)
async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes
await set_speed(600,200,300,200)
await drive_base.straight(500)
await multitask(
drive_base.straight(-200),
right_arm.run_angle(400,-150)
)
await set_speed(600,500,100,200)
await drive_base.turn(45)
await drive_base.straight(346)
await drive_base.turn(-68)
await multitask(
left_arm.run_angle(600,240),
set_speed(300,200,300,200),
drive_base.straight(280)
)
await left_arm.run_angle(600,-100)
await drive_base.turn(22)
await drive_base.straight(-100)
await right_arm.run_angle(5000,45)
await set_speed(600,500,100,200)
await drive_base.straight(200)
await drive_base.arc(-1500,None,-1000)
async def Run2(): # Exploding Seeds - Johannes
await multitask(
right_arm.run_angle(400,-100)
left_arm.run_angle(300,100)
drive_base.arc(150,None,115)
)
await drive_base.straight(270)
await left_arm.run_angle(300,-100)
await drive_base.straight(-100)
await left_arm.run_angle(300,-100)
await drive_base.arc(-1000,None,-800)
async def Run2_1(): # Drone Survey, Experimental - Johannes
await multitask(
right_arm.run_angle(400,100),
set_speed(600,200,300,200),
drive_base.straight(300)
)
set_speed(600,500,300,200)
await drive_base.straight(470)
await drive_base.turn(-15)
await drive_base.straight(55)
await drive_base.straight(-280)
await drive_base.turn(15)
await right_arm.run_angle(400,-100)
await drive_base.straight(40)
await drive_base.turn(-15)
await drive_base.straight(50)
await drive_base.turn(-25)
async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes
await drive_base.straight(110)
await drive_base.turn(-45)
await drive_base.straight(300)
await right_arm.run_angle(300,120)
await drive_base.straight(290)
await drive_base.turn(50)
await drive_base.straight(250)
await drive_base.turn(40)
set_speed(100,200,300,200)
await drive_base.straight(170)
await drive_base.straight(-145)
await drive_base.turn(80)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-138)
await multitask(
drive_base.turn(-36),
right_arm.run_angle(200,-140)
)
set_speed(100,200,300,200)
await drive_base.straight(230)
await right_arm.run_angle(300,105)
await wait(200)
await drive_base.turn(-90)
await drive_base.straight(-170)
set_speed(600,500,300,200)
await drive_base.straight(100)
await drive_base.arc(800,None,-900)
async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh
#Run
await drive_base.straight(550)#Works (600(Previous))
await drive_base.straight(-200)
await drive_base.turn(45)
await drive_base.straight(575)
await drive_base.turn(-90)#-90 for Coach Cisco table
await left_arm.run_angle(200,270)
await drive_base.straight(300)
await left_arm.run_angle(200,-265)
await right_arm.run_angle(200, -200)
await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
await right_arm.run_angle(1500, 180)
await drive_base.turn(30)
await drive_base.straight(-1000)
async def Run11(): # Interchangable Mission - Parthiv
await left_arm.run_angle(200,-80)
await right_arm.run_angle(200,-70)
await drive_base.arc(-400,angle=90)
await drive_base.turn(55)
await drive_base.straight(240)
await drive_base.turn(-40)
await drive_base.straight(85)
await drive_base.straight(-32)
await run_both_arms()
await drive_base.straight(-53)
await right_arm.run_angle(200,100)
await drive_base.turn(30)
await drive_base.straight(-775)
await drive_base.stop
async def Run12(): # Experimental Research Platform Lifter
await multilift_up(1200)
await multilift_down(300)
await drive_base.straight(50)
# Function to classify color based on HSV
def detect_color(h, s, v, reflected):
if reflected > 4:
if h < 4 or h > 350: # red
return "Red"
elif 3 < h < 40 and s > 70: # orange
return "Orange"
elif 47 < h < 56: # yellow
return "Yellow"
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
return "Green"
elif 195 < h < 198: # light blue
return "Light_Blue"
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
return "Blue"
elif 260 < h < 350: # purple
return "Purple"
else:
return "Unknown"
return "Unknown"
async def main():
while True:
pressed = hub.buttons.pressed()
h, s, v = await color_sensor.hsv()
reflected = await color_sensor.reflection()
color = detect_color(h, s, v, reflected)
if DEBUG :
#print(color_sensor.color())
#print(h,s,v)
#print(color)
print(f"button pressed: {pressed}")
if color == "Green":
print('Running Mission 1')
await Run1()
elif color == "Red":
print('Running Mission 2')
await Run2()
elif color == "Yellow":
print('Running Mission 3')
await Run3()
elif color == "Blue":
print('Running Mission 4')
await Run4()
elif color == "Orange":
print('Running Mission 5')
await Run3()
elif color == "Purple":
print('Running Mission 11')
await Run11()
elif color == "Light_Blue":
print("Running Mission 12")
await Run12()
else:
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
#pass
# Show battery % for debugging
if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor
# Get the battery voltage in millivolts (mV)
battery_voltage_mV = hub.battery.voltage()
# Use the function with your voltage reading
percentage = await get_battery_percentage(float(battery_voltage_mV))
if DEBUG:
print(f"Battery voltage: {battery_voltage_mV} mV")
print(f"Battery level: {percentage:.3f}%")
print("FLL Robot System Ready!")
await hub.display.text(f"{percentage:.0f}")
break
elif pressed == None:
continue
await wait(10)
# Run the main function
run_task(main())

View File

@@ -1,35 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
# Initialize hub and devices
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F)
# DriveBase configuration
WHEEL_DIAMETER = 62.4 # mm (adjust for your wheels)
AXLE_TRACK = 150 # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True)
def set_default_speed():
drive_base.settings(600, 500, 300, 200)
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
left_arm.run_angle(300,100)
drive_base.arc(150,None,115)
drive_base.straight(270)
left_arm.run_angle(300,-90)
drive_base.straight(-80)
left_arm.run_angle(300,-100)
drive_base.arc(-1000,None,-800)

View File

@@ -31,31 +31,30 @@ def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
#Run code #Run code
drive_base.straight(50)#Was 50 drive_base.straight(50)#Was 50
drive_base.turn(-49)#42 Mostly works drive_base.turn(-43.75)#42 Mostly works
drive_base.straight(700)#Was 735 drive_base.straight(735)#Was 750
#left_arm.run_angle(1500,-100)#Was 1000,And was -30 #left_arm.run_angle(1500,-100)#Was 1000,And was -30
#wait(1000) #wait(1000)
#drive_base.straight(330) #drive_base.straight(330)
drive_base.turn(40)#Was 80 drive_base.turn(80)#Was 80
set_speed(100,300,300,200) set_speed(100,300,300,200)
drive_base.straight(100) drive_base.straight(350)#Was 400
drive_base.turn(47)#Was 37
drive_base.straight(320)#Was 300-350
drive_base.straight(-110)#Was -125 drive_base.straight(-110)#Was -125
set_default_speed set_default_speed
drive_base.turn(85)#Was 87 drive_base.turn(87)#Was 90
#right_arm.run_angle(1000, -30)#Was -47 #right_arm.run_angle(1000, -30)#Was -47
drive_base.straight(50)#150 drive_base.straight(100)
drive_base.turn(-67)#Was -73 drive_base.turn(-70)
print("Mycellium") left_arm.run_angle(1000,180)
left_arm.run_angle(1000,180)#Towards Mycellium drive_base.straight(55)
drive_base.straight(75)#Was 75 drive_base.turn(-30)#Was Positive 30
drive_base.turn(-10)#Was Positive 30 #left_arm.run_angle(2000,-180)
drive_base.straight(3) drive_base.straight(-20)
set_speed(1000,300,300,200) drive_base.straight(-200)
left_arm.run_angle(102020,-180) drive_base.turn(120)
drive_base.turn(70)#Orignal 120 #Going back
drive_base.straight(300) drive_base.straight(700)

View File

@@ -15,8 +15,8 @@ Improvements over baseline:
from pybricks.tools import StopWatch, wait from pybricks.tools import StopWatch, wait
import umath import umath
WHEEL_DIAMETER = 62.4 # mm — adjust to your wheel (Done) WHEEL_DIAMETER = 68.8 # mm — adjust to your wheel (Done)
AXLE_TRACK = 150 # mm — used only if you add turning later (Done) AXLE_TRACK = 180 # mm — used only if you add turning later (Done)
LOOP_MS = 20 # fixed control loop period LOOP_MS = 20 # fixed control loop period
RAMP_MM = 50 # begin decelerating this far from target RAMP_MM = 50 # begin decelerating this far from target
MIN_SPEED = 80 # floor speed during ramp (avoid stall) MIN_SPEED = 80 # floor speed during ramp (avoid stall)
@@ -202,15 +202,8 @@ async def multilift_down(rotation_angle):
) )
<<<<<<< HEAD
async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Parthiv
=======
<<<<<<< HEAD
async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes
>>>>>>> 3cd1849 (Update discovery-phase/missions/Showcase_codes.py)
<<<<<<< HEAD
<<<<<<< HEAD
await set_speed(600,200,300,200) await set_speed(600,200,300,200)
await drive_base.straight(500) await drive_base.straight(500)
await multitask( await multitask(
@@ -237,8 +230,8 @@ async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes
async def Run2(): # Exploding Seeds - Johannes async def Run2(): # Exploding Seeds - Johannes
await multitask( await multitask(
right_arm.run_angle(400,-100), right_arm.run_angle(400,-100)
left_arm.run_angle(300,100), left_arm.run_angle(300,100)
drive_base.arc(150,None,115) drive_base.arc(150,None,115)
) )
await drive_base.straight(270) await drive_base.straight(270)
@@ -270,7 +263,7 @@ async def Run2_1(): # Drone Survey, Experimental - Johannes
async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes
await drive_base.straight(98) await drive_base.straight(110)
await drive_base.turn(-45) await drive_base.turn(-45)
await drive_base.straight(300) await drive_base.straight(300)
await right_arm.run_angle(300,120) await right_arm.run_angle(300,120)
@@ -279,44 +272,19 @@ async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
await drive_base.straight(250) await drive_base.straight(250)
await drive_base.turn(40) await drive_base.turn(40)
set_speed(100,200,300,200) set_speed(100,200,300,200)
<<<<<<< HEAD
await drive_base.straight(170) await drive_base.straight(170)
<<<<<<< HEAD
await drive_base.straight(-145) await drive_base.straight(-145)
await drive_base.turn(80) await drive_base.turn(80)
await drive_base.straight(230) await drive_base.straight(230)
set_speed(600, 500, 300, 200) set_speed(600, 500, 300, 200)
await drive_base.straight(-138) await drive_base.straight(-138)
=======
await drive_base.straight(-135)
await drive_base.turn(80)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-145)
>>>>>>> 0e1e580 (Update discovery-phase/missions/Showcase_codes.py)
=======
await drive_base.straight(177)
await drive_base.straight(-145)
await drive_base.turn(85)
await drive_base.straight(232)
set_speed(600, 500, 300, 200)
await drive_base.straight(-138)
>>>>>>> 788745f (Update discovery-phase/missions/Showcase_codes.py)
await multitask( await multitask(
drive_base.turn(-36), drive_base.turn(-36),
right_arm.run_angle(200,-140) right_arm.run_angle(200,-140)
) )
set_speed(100,200,300,200) set_speed(100,200,300,200)
await drive_base.straight(230) await drive_base.straight(230)
<<<<<<< HEAD
<<<<<<< HEAD
await right_arm.run_angle(300,105) await right_arm.run_angle(300,105)
=======
await right_arm.run_angle(300,160)
>>>>>>> 0e1e580 (Update discovery-phase/missions/Showcase_codes.py)
=======
await right_arm.run_angle(300,105)
>>>>>>> 788745f (Update discovery-phase/missions/Showcase_codes.py)
await wait(200) await wait(200)
await drive_base.turn(-90) await drive_base.turn(-90)
await drive_base.straight(-170) await drive_base.straight(-170)
@@ -328,250 +296,43 @@ async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh
#Run #Run
=======
>>>>>>> c15d8f0 (Update discovery-phase/missions/Showcase_codes.py)
await drive_base.straight(550)#Works (600(Previous)) await drive_base.straight(550)#Works (600(Previous))
await drive_base.straight(-200) await drive_base.straight(-200)
=======
await drive_base.straight(415)
await drive_base.straight(-150)
>>>>>>> a9d3cd9 (Update discovery-phase/missions/Showcase_codes.py)
await drive_base.turn(45) await drive_base.turn(45)
await drive_base.straight(600) await drive_base.straight(575)
await drive_base.turn(-90) await drive_base.turn(-90)#-90 for Coach Cisco table
await multitask( await left_arm.run_angle(200,270)
left_arm.run_angle(200,300),
right_arm.run_angle(200,-290)
)
set_speed(100,500,300,200)
await drive_base.straight(300) await drive_base.straight(300)
await left_arm.run_angle(200,-300) await left_arm.run_angle(200,-265)
await drive_base.straight(-95) await right_arm.run_angle(200, -200)
await drive_base.turn(22) await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
await right_arm.run_angle(5000,100) await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
await drive_base.straight(90) await right_arm.run_angle(1500, 180)
set_speed(700,500,300,200) await drive_base.turn(30)
await drive_base.arc(-700,angle=-150) await drive_base.straight(-1000)
<<<<<<< HEAD
async def Run11(): # Interchangable Mission - Parthiv async def Run11(): # Interchangable Mission - Parthiv
=======
async def Run2(): # Exploding Seeds - Johannes
await multitask(
right_arm.run_angle(400,-100),
drive_base.arc(150,None,115),
)
await drive_base.straight(270)
await left_arm.run_angle(300,-80)
await drive_base.straight(-100)
await left_arm.run_angle(300,-100)
await drive_base.arc(-1000,None,-800)
async def Run3(): # Drone Survey - Johannes
await multitask(
left_arm.run_angle(400,-300),
right_arm.run_angle(400,100),
drive_base.straight(900)
)
await drive_base.turn(-25)
await drive_base.straight(110)
await drive_base.turn(-35)
await drive_base.turn(35)
await drive_base.straight(-30)
await drive_base.turn(25)
await drive_base.straight(-200)
await drive_base.turn(-45)
await drive_base.straight(500)
await drive_base.turn(75)
await drive_base.straight(700)
async def Run4(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes
await drive_base.straight(110)
await drive_base.turn(-45)
<<<<<<< HEAD
<<<<<<< HEAD
await drive_base.straight(280)
=======
await drive_base.straight(300)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
await right_arm.run_angle(300,120)
await left_arm.run_angle(400,200)
await drive_base.straight(-70)
await left_arm.run_angle(600,-110)
await drive_base.straight(370)
await drive_base.turn(50)
<<<<<<< HEAD
await multitask(
drive_base.straight(250),
left_arm.run_angle(600,-60)
)
await drive_base.turn(40)
set_speed(100,200,300,200)
await drive_base.straight(170)
await drive_base.straight(-135)
await drive_base.turn(80)
await drive_base.straight(85)
=======
await drive_base.straight(250)
await drive_base.turn(40)
set_speed(100,200,300,200)
await drive_base.straight(170)
=======
await drive_base.straight(300)
await right_arm.run_angle(300,120)
await drive_base.straight(290)
await drive_base.turn(50)
await drive_base.straight(250)
await drive_base.turn(40)
set_speed(100,200,300,200)
await drive_base.straight(170)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
await drive_base.straight(-145)
await drive_base.turn(80)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-138)
<<<<<<< HEAD
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
=======
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
await multitask(
drive_base.turn(-36),
right_arm.run_angle(200,-140)
)
set_speed(100,200,300,200)
<<<<<<< HEAD
<<<<<<< HEAD
await drive_base.straight(235)
await multitask(
right_arm.run_angle(300,100),
left_arm.run_angle(200,140)
)
await wait(200)
await drive_base.straight(30)
=======
await drive_base.straight(230)
await right_arm.run_angle(300,105)
await wait(200)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
=======
await drive_base.straight(230)
await right_arm.run_angle(300,105)
await wait(200)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
await drive_base.turn(-90)
await drive_base.straight(-170)
set_speed(600,500,300,200)
await drive_base.straight(100)
await drive_base.arc(800,None,-900)
async def Run5(): # Forest Elder - Parthiv
await drive_base.straight(475)
await right_arm.run_angle(300,25)
await drive_base.turn(-20)
await left_arm.run_angle(200,245)
await drive_base.straight(60)
await drive_base.turn(25)
await left_arm.run_angle(100,-56.5)
await drive_base.straight(-50)
await drive_base.straight(50)
await drive_base.turn(-25)
await left_arm.run_angle(200,-200)
await drive_base.straight(-240)
await drive_base.turn(15)
await right_arm.run_angle(200,-200)
await drive_base.straight(-200)
<<<<<<< HEAD
await drive_base.stop
=======
=======
async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Parthiv
await drive_base.straight(415)
await drive_base.straight(-150)
>>>>>>> a9d3cd9 (Update discovery-phase/missions/Showcase_codes.py)
await drive_base.turn(45)
await drive_base.straight(600)
await drive_base.turn(-90)
await multitask(
left_arm.run_angle(200,300),
right_arm.run_angle(200,-290)
)
set_speed(100,500,300,200)
await drive_base.straight(300)
await left_arm.run_angle(200,-300)
await drive_base.straight(-95)
await drive_base.turn(22)
await right_arm.run_angle(5000,100)
await drive_base.straight(90)
set_speed(700,500,300,200)
await drive_base.arc(-700,angle=-150)
>>>>>>> 3cd1849 (Update discovery-phase/missions/Showcase_codes.py)
<<<<<<< HEAD
<<<<<<< HEAD
async def Run6(): # Interchangable Mission - Parthiv
<<<<<<< HEAD
>>>>>>> c15d8f0 (Update discovery-phase/missions/Showcase_codes.py)
=======
=======
=======
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
async def Run11(): # Interchangable Mission - Parthiv
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
>>>>>>> 0cd478f (resolving)
await left_arm.run_angle(200,-80) await left_arm.run_angle(200,-80)
await drive_base.arc(-350,angle=90) await right_arm.run_angle(200,-70)
await drive_base.turn(50) await drive_base.arc(-400,angle=90)
await drive_base.straight(238) await drive_base.turn(55)
await drive_base.turn(-33) await drive_base.straight(240)
await drive_base.turn(-40)
await drive_base.straight(85) await drive_base.straight(85)
await drive_base.straight(-33) await drive_base.straight(-32)
await multitask( await run_both_arms()
left_arm.run_angle(200,80), await drive_base.straight(-53)
right_arm.run_angle(200,-100)
)
await drive_base.straight(-60)
await right_arm.run_angle(200,100) await right_arm.run_angle(200,100)
await drive_base.turn(30) await drive_base.turn(30)
await drive_base.straight(-775) await drive_base.straight(-775)
await drive_base.stop await drive_base.stop
<<<<<<< HEAD
=======
<<<<<<< HEAD
async def Run12(): # Experimental Research Platform Lifter async def Run12(): # Experimental Research Platform Lifter
=======
>>>>>>> 3cd1849 (Update discovery-phase/missions/Showcase_codes.py)
async def Run7(): # Seed Dispersal and Keystone - Parthiv
await drive_base.straight(600)
await drive_base.arc(400,angle=35)
await drive_base.straight(-40)
await right_arm.run_angle(150,-70)
await drive_base.arc(400,angle=-35)
await drive_base.straight(-540)
await drive_base.turn(-90)
await drive_base.straight(200)
await left_arm.run_angle(100,100)
await drive_base.straight(-350)
<<<<<<< HEAD
=======
>>>>>>> a9d3cd9 (Update discovery-phase/missions/Showcase_codes.py)
>>>>>>> 3cd1849 (Update discovery-phase/missions/Showcase_codes.py)
await multilift_up(1200)
await multilift_down(300)
await drive_base.straight(50)
# Function to classify color based on HSV # Function to classify color based on HSV
@@ -579,9 +340,9 @@ def detect_color(h, s, v, reflected):
if reflected > 4: if reflected > 4:
if h < 4 or h > 350: # red if h < 4 or h > 350: # red
return "Red" return "Red"
elif 3 < h < 24 and s > 80: # orange elif 3 < h < 40 and s > 70: # orange
return "Orange" return "Orange"
elif 30 < h < 60 and v > 50: # yellow elif 47 < h < 56: # yellow
return "Yellow" return "Yellow"
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
return "Green" return "Green"
@@ -591,8 +352,7 @@ def detect_color(h, s, v, reflected):
return "Blue" return "Blue"
elif 260 < h < 350: # purple elif 260 < h < 350: # purple
return "Purple" return "Purple"
elif s < 15 and v > 70: # white
return "White"
else: else:
return "Unknown" return "Unknown"
return "Unknown" return "Unknown"
@@ -612,28 +372,26 @@ async def main():
if color == "Green": if color == "Green":
print('Running Mission 1, Detect Green') print('Running Mission 1')
await Run1() await Run1()
elif color == "Red": elif color == "Red":
print('Running Mission 2, Detect Red') print('Running Mission 2')
await Run2() await Run2()
elif color == "Yellow": elif color == "Yellow":
print('Running Mission 3, Detect Yellow') print('Running Mission 3')
await Run3() await Run3()
elif color == "Blue": elif color == "Blue":
print('Running Mission 4, Detect Blue') print('Running Mission 4')
await Run4() await Run4()
elif color == "Orange": elif color == "Orange":
print('Running Mission 5, Detect Orange') print('Running Mission 5')
await Run5() await Run3()
elif color == "Purple": elif color == "Purple":
print('Running Mission 6, Detect Purple') print('Running Mission 11')
await Run6() await Run11()
elif color == "Light_Blue": elif color == "Light_Blue":
print('Running Mission 7, Detect Cyan') print("Running Mission 12")
await Run7() await Run12()
elif color == "White":
print('Detect White')
else: else:
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
#pass #pass

View File

@@ -42,13 +42,8 @@ async def multilift_down(rotation_angle):
async def main(): async def main():
await drive_base.arc(900,None,400) await multilift_up(1200)
await drive_base.arc(-500,None,250) await multilift_down(300)
set_speed(100,400,300,200) await drive_base.straight(50)
await drive_base.straight(200)
await multilift_up(1950)
await multilift_down(500)
set_speed(1000,500,300,200)
await drive_base.arc(-1000,None,-900)
run_task(main()) run_task(main())

View File

@@ -32,39 +32,33 @@ async def main():
await drive_base.straight(110) await drive_base.straight(110)
await drive_base.turn(-45) await drive_base.turn(-45)
await drive_base.straight(280) await drive_base.straight(240)
await right_arm.run_angle(300,120) await right_arm.run_angle(300,120)
await left_arm.run_angle(400,200) await drive_base.straight(290)
await drive_base.straight(-70)
await left_arm.run_angle(600,-110)
await drive_base.straight(370)
await drive_base.turn(50) await drive_base.turn(50)
await multitask( await drive_base.straight(300)
drive_base.straight(250),
left_arm.run_angle(600,-60)
)
await drive_base.turn(40) await drive_base.turn(40)
set_speed(100,200,300,200) set_speed(100,200,300,200)
await drive_base.straight(170) await drive_base.straight(130)
await drive_base.straight(-115)
await drive_base.turn(87)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-135) await drive_base.straight(-135)
await drive_base.turn(80)
await drive_base.straight(85)
await multitask( await multitask(
drive_base.turn(-36), drive_base.turn(-40),
right_arm.run_angle(200,-140) right_arm.run_angle(100,-110)
) )
set_speed(100,200,300,200) set_speed(100,200,300,200)
await drive_base.straight(230) await drive_base.straight(220)
await multitask( await right_arm.run_angle(300,85)
right_arm.run_angle(300,160), await wait(500)
left_arm.run_angle(300,150) await drive_base.straight(142)
) await drive_base.arc(110,None,-166)
await wait(200) await drive_base.straight(140)
await drive_base.straight(30) await drive_base.straight(-150)
await drive_base.turn(-90)
await drive_base.straight(-170)
set_speed(600,500,300,200) set_speed(600,500,300,200)
await drive_base.straight(100) await drive_base.straight(150)
await drive_base.arc(800,None,-900) await drive_base.arc(800,None,-900)
run_task(main()) run_task(main())