Compare commits
53 Commits
1a94799e5a
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 34c153105b | |||
| 51b7b36cf7 | |||
| ee274df574 | |||
| 73f96e6498 | |||
| 33344aafd0 | |||
| f1ca63bad2 | |||
| 514bb91f7b | |||
| 466657022a | |||
| f4cfa3e8f4 | |||
| 0c3b2e1053 | |||
| cc1643f9e8 | |||
| 47a5a8445b | |||
| e53e367751 | |||
| 13a24e17ad | |||
| bda38fab1c | |||
| 275c73382c | |||
| 72ffdf2172 | |||
| ff561e51a0 | |||
| 0cd478fde1 | |||
| 2888d96d30 | |||
| d444fb97c5 | |||
| a9d3cd971e | |||
| e608f7ea1f | |||
| f3041b0483 | |||
| 83363ed0c4 | |||
| c15d8f0fd6 | |||
| 788745f4f4 | |||
| a60078d89d | |||
| 0e1e580fb0 | |||
| 98b987fe71 | |||
| 19912d759e | |||
| e9b42189b0 | |||
| b1783a2fce | |||
| 57844e8b1a | |||
| 74b363f97e | |||
| e332b0c324 | |||
| 64a296a84e | |||
| de8a3455b0 | |||
| 3fcbb79439 | |||
| 8c7bec48ff | |||
| ad69b7560d | |||
| 9125c0b4aa | |||
| 54b58c64c3 | |||
| 4c9b7607f6 | |||
| d8bf319fdf | |||
| 99826dad19 | |||
| ddbc8a6390 | |||
| 4748e17030 | |||
| 1ffcfceae7 | |||
| b31f815876 | |||
| 3fee5ba45c | |||
| 37aa0e9037 | |||
| 08fa18fc72 |
114
discovery-phase/missions/Elder tree - Axel
Normal file
114
discovery-phase/missions/Elder tree - Axel
Normal file
@@ -0,0 +1,114 @@
|
|||||||
|
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() #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 main():
|
||||||
|
await drive_base.straight(610)
|
||||||
|
await drive_base.turn(25)
|
||||||
|
await drive_base.straight(45)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
await drive_base.turn(-30)
|
||||||
|
await drive_base.straight(50)
|
||||||
|
await drive_base.straight(-500)
|
||||||
|
await drive_base.turn(5)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
|
||||||
|
# Runs the main program from start to finish.
|
||||||
|
run_task(main())
|
||||||
@@ -1,11 +1,8 @@
|
|||||||
#SETTING UP
|
from pybricks.hubs import PrimeHub
|
||||||
import umath
|
|
||||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
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.robotics import DriveBase
|
||||||
from pybricks.hubs import PrimeHub
|
from pybricks.tools import wait, StopWatch
|
||||||
|
|
||||||
# Initialize hub and devices
|
# Initialize hub and devices
|
||||||
hub = PrimeHub()
|
hub = PrimeHub()
|
||||||
@@ -17,26 +14,22 @@ lazer_ranger = UltrasonicSensor(Port.E)
|
|||||||
color_sensor = ColorSensor(Port.F)
|
color_sensor = ColorSensor(Port.F)
|
||||||
|
|
||||||
# DriveBase configuration
|
# DriveBase configuration
|
||||||
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
|
WHEEL_DIAMETER = 62.4 # mm (adjust for your wheels)
|
||||||
AXLE_TRACK = 180 # mm (distance between wheels)
|
AXLE_TRACK = 150 # mm (distance between wheels)
|
||||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
drive_base.settings(600, 450, 300, 200)
|
drive_base.settings(600, 500, 300, 200)
|
||||||
drive_base.use_gyro(True)
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
#Run
|
def set_default_speed():
|
||||||
drive_base.straight(550)#Works (600(Previous))
|
drive_base.settings(600, 500, 300, 200)
|
||||||
drive_base.straight(-200)
|
|
||||||
drive_base.turn(45)
|
|
||||||
drive_base.straight(575)
|
|
||||||
drive_base.turn(-90)#-90 for Coach Cisco table
|
|
||||||
left_arm.run_angle(200,270)
|
|
||||||
drive_base.straight(300)
|
|
||||||
left_arm.run_angle(200,-265)
|
|
||||||
right_arm.run_angle(200, -200)
|
|
||||||
drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
|
|
||||||
drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
|
|
||||||
right_arm.run_angle(1500, 180)
|
|
||||||
drive_base.turn(30)
|
|
||||||
drive_base.straight(-1000)
|
|
||||||
|
|
||||||
|
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)
|
||||||
@@ -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 = 68.8 # mm — adjust to your wheel (Done)
|
WHEEL_DIAMETER = 62.4 # mm — adjust to your wheel (Done)
|
||||||
AXLE_TRACK = 180 # mm — used only if you add turning later (Done)
|
AXLE_TRACK = 150 # 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,8 +202,16 @@ 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
|
||||||
|
<<<<<<< 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(
|
||||||
@@ -263,7 +271,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(110)
|
await drive_base.straight(98)
|
||||||
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)
|
||||||
@@ -272,6 +280,7 @@ 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
|
<<<<<<< HEAD
|
||||||
await drive_base.straight(-145)
|
await drive_base.straight(-145)
|
||||||
@@ -286,17 +295,29 @@ async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
|
|||||||
set_speed(600, 500, 300, 200)
|
set_speed(600, 500, 300, 200)
|
||||||
await drive_base.straight(-145)
|
await drive_base.straight(-145)
|
||||||
>>>>>>> 0e1e580 (Update discovery-phase/missions/Showcase_codes.py)
|
>>>>>>> 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
|
<<<<<<< HEAD
|
||||||
await right_arm.run_angle(300,105)
|
await right_arm.run_angle(300,105)
|
||||||
=======
|
=======
|
||||||
await right_arm.run_angle(300,160)
|
await right_arm.run_angle(300,160)
|
||||||
>>>>>>> 0e1e580 (Update discovery-phase/missions/Showcase_codes.py)
|
>>>>>>> 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)
|
||||||
@@ -308,22 +329,237 @@ 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.straight(415)
|
||||||
|
await drive_base.straight(-150)
|
||||||
|
>>>>>>> dev
|
||||||
await drive_base.turn(45)
|
await drive_base.turn(45)
|
||||||
await drive_base.straight(575)
|
await drive_base.straight(600)
|
||||||
await drive_base.turn(-90)#-90 for Coach Cisco table
|
await drive_base.turn(-90)
|
||||||
await left_arm.run_angle(200,270)
|
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 drive_base.straight(300)
|
||||||
await left_arm.run_angle(200,-265)
|
await left_arm.run_angle(200,-300)
|
||||||
await right_arm.run_angle(200, -200)
|
await drive_base.straight(-95)
|
||||||
await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
|
await drive_base.turn(22)
|
||||||
await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
|
await right_arm.run_angle(5000,100)
|
||||||
await right_arm.run_angle(1500, 180)
|
await drive_base.straight(90)
|
||||||
await drive_base.turn(30)
|
set_speed(700,500,300,200)
|
||||||
await drive_base.straight(-1000)
|
await drive_base.arc(-700,angle=-150)
|
||||||
|
|
||||||
|
<<<<<<< HEAD
|
||||||
|
<<<<<<< HEAD
|
||||||
async def Run11(): # Interchangable Mission - Parthiv
|
async def Run11(): # Interchangable Mission - Parthiv
|
||||||
|
=======
|
||||||
|
=======
|
||||||
|
>>>>>>> dev
|
||||||
|
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
|
||||||
|
<<<<<<< HEAD
|
||||||
|
=======
|
||||||
|
>>>>>>> dev
|
||||||
|
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)
|
||||||
|
<<<<<<< HEAD
|
||||||
|
=======
|
||||||
|
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)
|
||||||
|
=======
|
||||||
|
>>>>>>> dev
|
||||||
|
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
|
||||||
|
<<<<<<< HEAD
|
||||||
|
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
|
||||||
|
=======
|
||||||
|
=======
|
||||||
|
>>>>>>> dev
|
||||||
|
>>>>>>> 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
|
||||||
|
<<<<<<< HEAD
|
||||||
|
=======
|
||||||
|
>>>>>>> dev
|
||||||
|
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)
|
||||||
|
<<<<<<< HEAD
|
||||||
|
=======
|
||||||
|
await drive_base.straight(230)
|
||||||
|
await right_arm.run_angle(300,105)
|
||||||
|
await wait(200)
|
||||||
|
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
|
||||||
|
=======
|
||||||
|
>>>>>>> dev
|
||||||
|
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
|
||||||
|
<<<<<<< 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)
|
||||||
|
=======
|
||||||
|
async def Run6(): # Interchangable Mission - Parthiv
|
||||||
|
=======
|
||||||
|
async def Run11(): # Interchangable Mission - Parthiv
|
||||||
|
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
|
||||||
|
>>>>>>> dev
|
||||||
|
|
||||||
await left_arm.run_angle(200,-80)
|
await left_arm.run_angle(200,-80)
|
||||||
await drive_base.arc(-350,angle=90)
|
await drive_base.arc(-350,angle=90)
|
||||||
@@ -342,11 +578,31 @@ async def Run11(): # Interchangable Mission - Parthiv
|
|||||||
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
|
||||||
@@ -354,9 +610,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 < 40 and s > 70: # orange
|
elif 3 < h < 24 and s > 80: # orange
|
||||||
return "Orange"
|
return "Orange"
|
||||||
elif 47 < h < 56: # yellow
|
elif 30 < h < 60 and v > 50: # 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"
|
||||||
@@ -366,7 +622,8 @@ 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"
|
||||||
@@ -386,26 +643,28 @@ async def main():
|
|||||||
|
|
||||||
|
|
||||||
if color == "Green":
|
if color == "Green":
|
||||||
print('Running Mission 1')
|
print('Running Mission 1, Detect Green')
|
||||||
await Run1()
|
await Run1()
|
||||||
elif color == "Red":
|
elif color == "Red":
|
||||||
print('Running Mission 2')
|
print('Running Mission 2, Detect Red')
|
||||||
await Run2()
|
await Run2()
|
||||||
elif color == "Yellow":
|
elif color == "Yellow":
|
||||||
print('Running Mission 3')
|
print('Running Mission 3, Detect Yellow')
|
||||||
await Run3()
|
await Run3()
|
||||||
elif color == "Blue":
|
elif color == "Blue":
|
||||||
print('Running Mission 4')
|
print('Running Mission 4, Detect Blue')
|
||||||
await Run4()
|
await Run4()
|
||||||
elif color == "Orange":
|
elif color == "Orange":
|
||||||
print('Running Mission 5')
|
print('Running Mission 5, Detect Orange')
|
||||||
await Run3()
|
await Run5()
|
||||||
elif color == "Purple":
|
elif color == "Purple":
|
||||||
print('Running Mission 11')
|
print('Running Mission 6, Detect Purple')
|
||||||
await Run11()
|
await Run6()
|
||||||
elif color == "Light_Blue":
|
elif color == "Light_Blue":
|
||||||
print("Running Mission 12")
|
print('Running Mission 7, Detect Cyan')
|
||||||
await Run12()
|
await Run7()
|
||||||
|
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
|
||||||
|
|||||||
52
discovery-phase/missions/mission_15_14_ant
Normal file
52
discovery-phase/missions/mission_15_14_ant
Normal file
@@ -0,0 +1,52 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 14,15
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Seed of Renewal and Keystone Species - Parthiv
|
||||||
|
# BLUE COLOR CODE
|
||||||
|
drive_base.stop()
|
||||||
|
drive_base.reset(0,0)
|
||||||
|
|
||||||
|
helper.set_speed(400,200,200,100)
|
||||||
|
await left_arm.run_angle(1000,-25)
|
||||||
|
await drive_base.arc(-315,angle=90, then=Stop.HOLD)
|
||||||
|
await drive_base.straight(150,then=Stop.HOLD)
|
||||||
|
await drive_base.turn(90,then=Stop.HOLD)
|
||||||
|
await drive_base.straight(235,then=Stop.NONE)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.turn(-85,then=Stop.HOLD)
|
||||||
|
helper.set_default_speed
|
||||||
|
await drive_base.straight(90,then=Stop.COAST)
|
||||||
|
await drive_base.straight(-95)
|
||||||
|
await drive_base.turn(-85)
|
||||||
|
await drive_base.straight(-250)
|
||||||
|
await drive_base.turn(60)
|
||||||
|
helper.set_speed(50,200,250,200)
|
||||||
|
await drive_base.straight(-175)
|
||||||
|
helper.set_default_speed
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.turn(-125)
|
||||||
|
await left_arm.run_angle(200,21)
|
||||||
|
await drive_base.turn(-21)
|
||||||
|
await drive_base.straight(25)
|
||||||
|
await left_arm.run_angle(1000,-25)
|
||||||
|
helper.set_speed(500,200,200,100)
|
||||||
|
await drive_base.straight(250)
|
||||||
|
await drive_base.turn(-42)
|
||||||
|
await drive_base.straight(150)
|
||||||
|
await right_arm.run_angle(300,-40)
|
||||||
|
await drive_base.straight(-150,then=Stop.NONE)
|
||||||
|
await drive_base.turn(93,then=Stop.HOLD)
|
||||||
|
await drive_base.straight(550,then=Stop.NONE)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
@@ -1,53 +0,0 @@
|
|||||||
#Solution for biosentric center
|
|
||||||
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 = 68.8 # mm (adjust for your wheels)
|
|
||||||
AXLE_TRACK = 180 # mm (distance between wheels)
|
|
||||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
|
||||||
drive_base.settings(700, 500, 300, 200)
|
|
||||||
drive_base.use_gyro(True)
|
|
||||||
|
|
||||||
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
|
|
||||||
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
|
|
||||||
|
|
||||||
async def run_both_arms():
|
|
||||||
|
|
||||||
await multitask(
|
|
||||||
left_arm.run_angle(200,80),
|
|
||||||
right_arm.run_angle(200,-100)
|
|
||||||
)
|
|
||||||
|
|
||||||
async def main ():
|
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
run_task(main())
|
|
||||||
@@ -42,8 +42,13 @@ async def multilift_down(rotation_angle):
|
|||||||
|
|
||||||
async def main():
|
async def main():
|
||||||
|
|
||||||
await multilift_up(1200)
|
await drive_base.arc(900,None,400)
|
||||||
await multilift_down(300)
|
await drive_base.arc(-500,None,250)
|
||||||
await drive_base.straight(50)
|
set_speed(100,400,300,200)
|
||||||
|
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())
|
||||||
@@ -32,33 +32,39 @@ 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(240)
|
await drive_base.straight(280)
|
||||||
await right_arm.run_angle(300,120)
|
await right_arm.run_angle(300,120)
|
||||||
await drive_base.straight(290)
|
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)
|
await drive_base.turn(50)
|
||||||
await drive_base.straight(300)
|
await multitask(
|
||||||
|
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(130)
|
await drive_base.straight(170)
|
||||||
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(-40),
|
drive_base.turn(-36),
|
||||||
right_arm.run_angle(100,-110)
|
right_arm.run_angle(200,-140)
|
||||||
)
|
)
|
||||||
set_speed(100,200,300,200)
|
set_speed(100,200,300,200)
|
||||||
await drive_base.straight(220)
|
await drive_base.straight(230)
|
||||||
await right_arm.run_angle(300,85)
|
await multitask(
|
||||||
await wait(500)
|
right_arm.run_angle(300,160),
|
||||||
await drive_base.straight(142)
|
left_arm.run_angle(300,150)
|
||||||
await drive_base.arc(110,None,-166)
|
)
|
||||||
await drive_base.straight(140)
|
await wait(200)
|
||||||
await drive_base.straight(-150)
|
await drive_base.straight(30)
|
||||||
|
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(150)
|
await drive_base.straight(100)
|
||||||
await drive_base.arc(800,None,-900)
|
await drive_base.arc(800,None,-900)
|
||||||
|
|
||||||
run_task(main())
|
run_task(main())
|
||||||
49
shout-fll-scrimmage/PynamicsBatteryDiagnostics.py
Normal file
49
shout-fll-scrimmage/PynamicsBatteryDiagnostics.py
Normal file
@@ -0,0 +1,49 @@
|
|||||||
|
from pybricks.tools import wait
|
||||||
|
import umath
|
||||||
|
class BatteryDiagnostics:
|
||||||
|
def __init__(self, hub):
|
||||||
|
self.voltage = 0
|
||||||
|
self.current = 0
|
||||||
|
self.hub = hub
|
||||||
|
def printVoltage(self):
|
||||||
|
self.voltage = self.hub.battery.voltage()
|
||||||
|
if self.voltage > 7800:
|
||||||
|
print(f"Battery voltage is sufficient: {self.voltage}")
|
||||||
|
elif self.voltage < 7800 :
|
||||||
|
print(f"Charging needed: {self.voltage}")
|
||||||
|
def printCurrent(self):
|
||||||
|
self.current = self.hub.battery.current()
|
||||||
|
print("Battery current:", self.current)
|
||||||
|
def printAll(self):
|
||||||
|
timeelapsed = 0
|
||||||
|
voltageList = []
|
||||||
|
currentList = []
|
||||||
|
while True:
|
||||||
|
print("\n\n\n\n\n")
|
||||||
|
self.printVoltage()
|
||||||
|
voltageList.append(self.voltage)
|
||||||
|
self.printCurrent()
|
||||||
|
currentList.append(self.current)
|
||||||
|
wait(50)
|
||||||
|
timeelapsed += 50
|
||||||
|
|
||||||
|
if(timeelapsed >= 3000):
|
||||||
|
break
|
||||||
|
print("--------------FINAL RESULTS OF BATTERY DIAGNOSTICS---------------")
|
||||||
|
print("Voltage deviation:", self.stdev(voltageList))
|
||||||
|
print("Current deviation:", self.stdev(currentList))
|
||||||
|
def stdev(self, vals):
|
||||||
|
DATA = vals
|
||||||
|
if len(DATA) < 2:
|
||||||
|
return 0
|
||||||
|
# Calculate the mean
|
||||||
|
MEAN = sum(DATA) / len(DATA)
|
||||||
|
|
||||||
|
# Calculate the variance (sum of squared differences from the mean, divided by n-1 for sample standard deviation)
|
||||||
|
VARIANCE = sum([(x - MEAN) ** 2 for x in DATA]) / float(len(DATA) - 1)
|
||||||
|
|
||||||
|
# Calculate the standard deviation (square root of the variance)
|
||||||
|
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
|
||||||
|
|
||||||
|
|
||||||
|
return (STD_DEV_MANUAL)
|
||||||
48
shout-fll-scrimmage/PynamicsColorSensorDiagnostics.py
Normal file
48
shout-fll-scrimmage/PynamicsColorSensorDiagnostics.py
Normal file
@@ -0,0 +1,48 @@
|
|||||||
|
from pybricks.parameters import Color, Port, Stop
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
|
||||||
|
class ColorSensorDiagnostics:
|
||||||
|
def __init__(self, hub, colorsensorclass):
|
||||||
|
self.colorsensor = None
|
||||||
|
self.colorsensorclass = colorsensorclass
|
||||||
|
self.PORT_MAP = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
def initializeColorSensor(self):
|
||||||
|
VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
|
||||||
|
while True:
|
||||||
|
colorinput = input(
|
||||||
|
"This will test your color sensor.\n"
|
||||||
|
"Enter the port for the color sensor you would like to test (A, B, C, D, E, or F): "
|
||||||
|
).strip().upper()
|
||||||
|
if colorinput not in VALID_PORTS:
|
||||||
|
print("Invalid port. Please enter A-F.")
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
if self.colorsensor is None:
|
||||||
|
self.colorsensor = self.colorsensorclass(self.PORT_MAP[colorinput])
|
||||||
|
print(f"Color Sensor initialized on port {colorinput}.")
|
||||||
|
else:
|
||||||
|
print(f"Reusing existing color sensor on port {colorinput}.")
|
||||||
|
break
|
||||||
|
|
||||||
|
except OSError as e:
|
||||||
|
if e.errno == 16: # EBUSY
|
||||||
|
print(f"Port {colorinput} is already in use. Reusing existing color sensor.")
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
print(f"Error initializing color sensor on port {colorinput}: {e}")
|
||||||
|
print("Make sure a color sensor is actually connected to this port.")
|
||||||
|
self.colorsensor = None
|
||||||
|
self.colorsensor.detectable_colors([Color.RED, Color.YELLOW, Color.GREEN, Color.BLUE, Color.WHITE, Color.NONE])
|
||||||
|
def printAll(self):
|
||||||
|
self.initializeColorSensor()
|
||||||
|
stopwatch = StopWatch()
|
||||||
|
while stopwatch.time() < 5000:
|
||||||
|
print("HSV output:", self.colorsensor.hsv())
|
||||||
|
print("Detected color:", self.colorsensor.color())
|
||||||
165
shout-fll-scrimmage/PynamicsDriveBaseDiagnostics.py
Normal file
165
shout-fll-scrimmage/PynamicsDriveBaseDiagnostics.py
Normal file
@@ -0,0 +1,165 @@
|
|||||||
|
from pybricks.parameters import Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
|
||||||
|
from usys import stdin
|
||||||
|
from uselect import poll
|
||||||
|
|
||||||
|
class DriveBaseDiagnostics:
|
||||||
|
def __init__(self, hub, motorclass, dbclass):
|
||||||
|
self.hub = hub
|
||||||
|
self.motorclass = motorclass
|
||||||
|
self.dbclass = dbclass
|
||||||
|
self.drive_base = None
|
||||||
|
self.PORT_MAP = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
def initializeDriveBase(self):
|
||||||
|
|
||||||
|
print("DriveBase setup:")
|
||||||
|
print("1 = Load from savefile (paste JSON)")
|
||||||
|
print("2 = Use defaults")
|
||||||
|
print("3 = Enter values manually")
|
||||||
|
|
||||||
|
choice = input("Choose an option: ")
|
||||||
|
|
||||||
|
# Default values
|
||||||
|
WHEEL_DIAMETER = 68.8
|
||||||
|
AXLE_TRACK = 180
|
||||||
|
DRIVE_SETTINGS = (600, 2000, 300, 2000)
|
||||||
|
|
||||||
|
# Motor ports (None until set)
|
||||||
|
leftmotorport = Port.A
|
||||||
|
rightmotorport = Port.B
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# OPTION 1: LOAD SAVEFILE
|
||||||
|
# -----------------------------
|
||||||
|
if choice == "1":
|
||||||
|
print("Paste JSON:")
|
||||||
|
raw = input("> ")
|
||||||
|
|
||||||
|
# --- wheel ---
|
||||||
|
if "\"wheel\"" in raw:
|
||||||
|
part = raw.split("\"wheel\"")[1]
|
||||||
|
part = part.split(":")[1]
|
||||||
|
part = part.split(",")[0]
|
||||||
|
WHEEL_DIAMETER = float(part)
|
||||||
|
|
||||||
|
# --- axle ---
|
||||||
|
if "\"axle\"" in raw:
|
||||||
|
part = raw.split("\"axle\"")[1]
|
||||||
|
part = part.split(":")[1]
|
||||||
|
part = part.split(",")[0]
|
||||||
|
AXLE_TRACK = float(part)
|
||||||
|
|
||||||
|
# --- settings ---
|
||||||
|
if "\"settings\"" in raw:
|
||||||
|
part = raw.split("\"settings\"")[1]
|
||||||
|
part = part.split("[")[1]
|
||||||
|
part = part.split("]")[0]
|
||||||
|
nums = part.split(",")
|
||||||
|
DRIVE_SETTINGS = (int(nums[0]), int(nums[1]), int(nums[2]), int(nums[3]))
|
||||||
|
|
||||||
|
# --- left motor port ---
|
||||||
|
if "\"left_port\"" in raw:
|
||||||
|
part = raw.split("\"left_port\"")[1]
|
||||||
|
part = part.split("\"")[1] # first quoted value
|
||||||
|
leftmotorport = part
|
||||||
|
|
||||||
|
# --- right motor port ---
|
||||||
|
if "\"right_port\"" in raw:
|
||||||
|
part = raw.split("\"right_port\"")[1]
|
||||||
|
part = part.split("\"")[1]
|
||||||
|
rightmotorport = part
|
||||||
|
|
||||||
|
|
||||||
|
print("Loaded config.")
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# OPTION 3: MANUAL ENTRY
|
||||||
|
# -----------------------------
|
||||||
|
elif choice == "3":
|
||||||
|
WHEEL_DIAMETER = float(input("Wheel diameter: "))
|
||||||
|
AXLE_TRACK = float(input("Axle track: "))
|
||||||
|
|
||||||
|
print("Enter drive settings:")
|
||||||
|
s1 = int(input("Straight speed: "))
|
||||||
|
s2 = int(input("Straight accel: "))
|
||||||
|
s3 = int(input("Turn rate: "))
|
||||||
|
s4 = int(input("Turn accel: "))
|
||||||
|
DRIVE_SETTINGS = (s1, s2, s3, s4)
|
||||||
|
|
||||||
|
# Ask for motor ports HERE (manual only)
|
||||||
|
leftmotorport = input("Left motor port: ")
|
||||||
|
rightmotorport = input("Right motor port: ")
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# OPTION 2: DEFAULTS
|
||||||
|
# -----------------------------
|
||||||
|
else:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# CREATE MOTORS
|
||||||
|
# -----------------------------
|
||||||
|
left_motor = self.motorclass(leftmotorport, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = self.motorclass(rightmotorport, Direction.CLOCKWISE)
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# CREATE DRIVEBASE
|
||||||
|
# -----------------------------
|
||||||
|
self.drive_base = self.dbclass(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
self.drive_base.settings(*DRIVE_SETTINGS)
|
||||||
|
self.drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
print("DriveBase initialized.")
|
||||||
|
return DRIVE_SETTINGS
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def printAll(self):
|
||||||
|
self.driveRobot()
|
||||||
|
def driveRobot(self):
|
||||||
|
drivesettings = self.initializeDriveBase()
|
||||||
|
print(drivesettings)
|
||||||
|
keyboard = poll()
|
||||||
|
keyboard.register(stdin)
|
||||||
|
|
||||||
|
last_key_time = StopWatch()
|
||||||
|
last_key_time.reset()
|
||||||
|
|
||||||
|
while True:
|
||||||
|
key = None
|
||||||
|
|
||||||
|
# Check for keypress immediately
|
||||||
|
if keyboard.poll(0):
|
||||||
|
key = stdin.read(1).upper()
|
||||||
|
last_key_time.reset()
|
||||||
|
|
||||||
|
# Process key
|
||||||
|
if key:
|
||||||
|
if key == "W":
|
||||||
|
self.drive_base.drive(drivesettings[0], 0)
|
||||||
|
elif key == "A":
|
||||||
|
self.drive_base.drive(drivesettings[0], -180)
|
||||||
|
elif key == "S":
|
||||||
|
self.drive_base.drive(-drivesettings[0], 0)
|
||||||
|
elif key == "D":
|
||||||
|
self.drive_base.drive(drivesettings[0], 180)
|
||||||
|
elif key == "X":
|
||||||
|
break
|
||||||
|
|
||||||
|
# Auto-stop if no key for 120 ms
|
||||||
|
if last_key_time.time() > 120:
|
||||||
|
self.drive_base.stop()
|
||||||
|
|
||||||
|
# Tiny manual delay to avoid 100% CPU
|
||||||
|
# (much faster than wait())
|
||||||
|
for _ in range(200):
|
||||||
|
pass
|
||||||
74
shout-fll-scrimmage/PynamicsFullDiagnostics.py
Normal file
74
shout-fll-scrimmage/PynamicsFullDiagnostics.py
Normal file
@@ -0,0 +1,74 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
HUB = PrimeHub()
|
||||||
|
from BatteryDiagnostics import BatteryDiagnostics
|
||||||
|
from MotorDiagnostics import MotorDiagnostics
|
||||||
|
from ColorSensorDiagnostics import ColorSensorDiagnostics
|
||||||
|
from DriveBaseDiagnostics import DriveBaseDiagnostics
|
||||||
|
from HubDiagnostics import HubDiagnostics
|
||||||
|
battery = BatteryDiagnostics(HUB)
|
||||||
|
motor = MotorDiagnostics(HUB, Motor)
|
||||||
|
colorsensor = ColorSensorDiagnostics(HUB, ColorSensor)
|
||||||
|
hubdiags = HubDiagnostics(HUB)
|
||||||
|
drivebase = DriveBaseDiagnostics(HUB, Motor, DriveBase)
|
||||||
|
CLEARCONFIRM = input("Clear the console before proceeding? Y/N (default: yes): ")
|
||||||
|
if(CLEARCONFIRM == "Y" or CLEARCONFIRM == "y" or CLEARCONFIRM == "yes" or CLEARCONFIRM == ""):
|
||||||
|
print("Clearing console... \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n")
|
||||||
|
print("""
|
||||||
|
███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████
|
||||||
|
▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███
|
||||||
|
▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒
|
||||||
|
▒██████████ ▒▒█████ ▒███▒▒███▒███ ▒███████████ ▒███▒▒███ ▒███ ▒███ ▒███ ▒▒█████████
|
||||||
|
▒███▒▒▒▒▒▒ ▒▒███ ▒███ ▒▒██████ ▒███▒▒▒▒▒███ ▒███ ▒▒▒ ▒███ ▒███ ▒███ ▒▒▒▒▒▒▒▒███
|
||||||
|
▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███
|
||||||
|
█████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████
|
||||||
|
▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒
|
||||||
|
|
||||||
|
The free and open source diagnostics tool for the LEGO® Education SPIKE™ Prime robots, designed for FIRST Lego League.
|
||||||
|
Developed by Team 65266, Lego Dynamics.
|
||||||
|
Please set your window size to 90% on small screens for best results with the ASCII art.
|
||||||
|
"""
|
||||||
|
)
|
||||||
|
while True:
|
||||||
|
|
||||||
|
print("\nWhich diagnostic do you want to perform?")
|
||||||
|
print("Enter 'b' for battery diagnostics")
|
||||||
|
print("Enter 'm' for motor diagnostics")
|
||||||
|
print("Enter 'cs' for color sensor diagnostics")
|
||||||
|
print("Enter 'h' for hub diagnostics")
|
||||||
|
print("Enter 'db' for drive base diagnostics")
|
||||||
|
print("Enter 'q' to quit")
|
||||||
|
|
||||||
|
choice = input("Your choice: ").strip().lower()
|
||||||
|
|
||||||
|
if choice == "b":
|
||||||
|
print("-----------------------BATTERY DIAGNOSTICS-----------------------")
|
||||||
|
print("This test will check the battery voltage and current. It will measure these over a period of 3 seconds and provide average and deviation values. Your voltage should be above 7800 mV for optimal performance.")
|
||||||
|
input("Press Enter to begin the battery diagnostics.")
|
||||||
|
battery.printAll()
|
||||||
|
print("Battery diagnostics completed.")
|
||||||
|
|
||||||
|
elif choice == "m":
|
||||||
|
print("------------------------MOTOR DIAGNOSTICS------------------------")
|
||||||
|
motor.fullTest()
|
||||||
|
print("Motor diagnostics completed.")
|
||||||
|
elif choice == "h":
|
||||||
|
print("-------------------------HUB DIAGNOSTICS-------------------------")
|
||||||
|
hubdiags.printAll()
|
||||||
|
print("Hub diagnostics completed.")
|
||||||
|
elif choice == "q":
|
||||||
|
print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
|
||||||
|
break
|
||||||
|
elif choice == "db":
|
||||||
|
print("----------------------DRIVEBASE DIAGNOSTICS----------------------")
|
||||||
|
drivebase.printAll()
|
||||||
|
elif choice == "cs":
|
||||||
|
print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
|
||||||
|
colorsensor.printAll()
|
||||||
|
print("Color sensor diagnostics completed.")
|
||||||
|
|
||||||
|
else:
|
||||||
|
print("Invalid choice. Please enter 'b', 'm', or 'q'.")
|
||||||
35
shout-fll-scrimmage/PynamicsHubDiagnostics.py
Normal file
35
shout-fll-scrimmage/PynamicsHubDiagnostics.py
Normal file
@@ -0,0 +1,35 @@
|
|||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.parameters import Port
|
||||||
|
from pybricks import version
|
||||||
|
import OtherFunctions as debug
|
||||||
|
import usys
|
||||||
|
|
||||||
|
|
||||||
|
class HubDiagnostics:
|
||||||
|
def __init__(self, hub):
|
||||||
|
self.hub = hub
|
||||||
|
self.port_map = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
def printAbout(self):
|
||||||
|
print("Pybricks version information:", version)
|
||||||
|
print("MicroPython information:", usys.implementation)
|
||||||
|
print("MicroPython version:", usys.version)
|
||||||
|
def testLightSources(self, verbose):
|
||||||
|
v = verbose
|
||||||
|
self.hub.display.off()
|
||||||
|
for x in range(5):
|
||||||
|
for y in range(5):
|
||||||
|
debug.log(f"Turning on pixel at position {x}, {y}...", v)
|
||||||
|
self.hub.display.pixel(x, y, brightness=100)
|
||||||
|
wait(100)
|
||||||
|
debug.log(f"Turning off pixel at position {x}, {y}...", v)
|
||||||
|
self.hub.display.pixel(x, y, brightness=0)
|
||||||
|
def printAll(self):
|
||||||
|
self.printAbout()
|
||||||
|
self.testLightSources(False)
|
||||||
136
shout-fll-scrimmage/PynamicsMotorDiagnostics.py
Normal file
136
shout-fll-scrimmage/PynamicsMotorDiagnostics.py
Normal file
@@ -0,0 +1,136 @@
|
|||||||
|
from pybricks.parameters import Direction, Port, Stop
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
import umath
|
||||||
|
class MotorDiagnostics:
|
||||||
|
def __init__(self, hub, motorclass):
|
||||||
|
self.testmotor = None
|
||||||
|
self.motorclass = motorclass
|
||||||
|
self.port_map = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
|
||||||
|
def stdev(self, vals):
|
||||||
|
DATA = vals
|
||||||
|
if len(DATA) < 2:
|
||||||
|
return 0
|
||||||
|
# Calculate the mean
|
||||||
|
MEAN = sum(DATA) / len(DATA)
|
||||||
|
|
||||||
|
# Calculate the variance (sum of squared differences from the mean, divided by n-1 for sample standard deviation)
|
||||||
|
VARIANCE = sum([(x - MEAN) ** 2 for x in DATA]) / float(len(DATA) - 1)
|
||||||
|
|
||||||
|
# Calculate the standard deviation (square root of the variance)
|
||||||
|
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
|
||||||
|
|
||||||
|
|
||||||
|
return (STD_DEV_MANUAL)
|
||||||
|
def health_score(self, desired, avg_speed, stdev_speed, avg_load):
|
||||||
|
# Speed accuracy: penalize % error
|
||||||
|
ACCURACY = max(0, 100 - abs(avg_speed - desired) / desired * 100)
|
||||||
|
|
||||||
|
# Stability: penalize deviation relative to desired
|
||||||
|
STABILITY = max(0, 100 - (stdev_speed / desired) * 100)
|
||||||
|
|
||||||
|
# Normalize load: map 10 to 20 as baseline (around 0%), 200 as max (around 100%)
|
||||||
|
BASELINE = 15 # midpoint of idle range
|
||||||
|
MAX_OBSERVED = 200 # heavy load/stall
|
||||||
|
NORMALIZED_LOAD = max(0, avg_load - BASELINE)
|
||||||
|
LOAD_PCT = min(100, (NORMALIZED_LOAD / (MAX_OBSERVED - BASELINE)) * 100)
|
||||||
|
|
||||||
|
LOAD_SCORE = max(0, 100 - LOAD_PCT)
|
||||||
|
|
||||||
|
# Final score: average of the three
|
||||||
|
return (ACCURACY + STABILITY + LOAD_SCORE) / 3
|
||||||
|
|
||||||
|
def initializeMotor(self):
|
||||||
|
VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
|
||||||
|
while True:
|
||||||
|
motorinput = input(
|
||||||
|
"This test will run your motor at 3 speeds: 180, 540, and 1000 degrees per second.\n"
|
||||||
|
"Please make sure your motor is not under any load (for example, your hand) during the test.\n"
|
||||||
|
"If you want to test the wheel's load, note that this will affect the load measurements.\n"
|
||||||
|
"Enter the port for the motor you would like to test (A, B, C, D, E, or F): "
|
||||||
|
).strip().upper()
|
||||||
|
if motorinput not in VALID_PORTS:
|
||||||
|
print("That is not a valid port. Please enter A-F.")
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
# Only create a new Motor if we don't already have one
|
||||||
|
if self.testmotor is None:
|
||||||
|
self.testmotor = self.motorclass(self.port_map[motorinput])
|
||||||
|
print(f"Motor initialized on port {motorinput}.")
|
||||||
|
else:
|
||||||
|
print(f"Reusing existing motor on port {motorinput}.")
|
||||||
|
break
|
||||||
|
|
||||||
|
except OSError as e:
|
||||||
|
if e.errno == 16: # EBUSY
|
||||||
|
print(f"Port {motorinput} is already in use. Reusing existing motor.")
|
||||||
|
# Do not overwrite self.testmotor here — keep the existing reference
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
print(f"Error initializing motor on port {motorinput}: {e}")
|
||||||
|
print("Make sure a motor is actually connected to this port.")
|
||||||
|
self.testmotor = None
|
||||||
|
|
||||||
|
def testSpeed(self, speed):
|
||||||
|
self.testmotor.reset_angle(0)
|
||||||
|
|
||||||
|
motorspeeds = []
|
||||||
|
motorloads = []
|
||||||
|
TARGET_ANGLE = speed * 3
|
||||||
|
print("\n", speed, "DEGREES PER SECOND TEST")
|
||||||
|
self.testmotor.run_angle(speed, TARGET_ANGLE, Stop.HOLD, False)
|
||||||
|
stopwatchmotor = StopWatch()
|
||||||
|
while stopwatchmotor.time() < 3000:
|
||||||
|
wait(20)
|
||||||
|
motorspeeds.append(self.testmotor.speed())
|
||||||
|
motorloads.append(self.testmotor.load())
|
||||||
|
|
||||||
|
MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
|
||||||
|
|
||||||
|
|
||||||
|
print("Desired motor speed: ", str(speed))
|
||||||
|
if motorspeeds:
|
||||||
|
avg = sum(motorspeeds) / len(motorspeeds)
|
||||||
|
print("Average motor speed:", avg)
|
||||||
|
print("Motor speed deviation:", str(self.stdev(motorspeeds)))
|
||||||
|
else:
|
||||||
|
print("No speed samples collected.")
|
||||||
|
avg = 0
|
||||||
|
if motorloads:
|
||||||
|
avgload = sum(motorloads) / len(motorloads)
|
||||||
|
|
||||||
|
print("Average motor load:", avgload)
|
||||||
|
print("Motor load deviation:", str(self.stdev(motorloads)))
|
||||||
|
else:
|
||||||
|
print("No load samples collected.")
|
||||||
|
avgload = 0
|
||||||
|
SCORE = self.health_score(speed, avg, self.stdev(motorspeeds), avgload)
|
||||||
|
print("Health score for this test:", str(SCORE) + "%")
|
||||||
|
return SCORE
|
||||||
|
def fullTest(self):
|
||||||
|
self.initializeMotor()
|
||||||
|
print("Load measurements are in mNm. Speed measurements are in degrees per second.")
|
||||||
|
MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
|
||||||
|
print("Maximum motor speed:", MAX_SPEED)
|
||||||
|
test180 = self.testSpeed(180)
|
||||||
|
test540 = self.testSpeed(540)
|
||||||
|
test1000 = self.testSpeed(1000)
|
||||||
|
print("\n FINAL MOTOR STATISTICS")
|
||||||
|
final = (test180 + test540 + test1000) / 3
|
||||||
|
print("Final motor health score:", str(final) + "%")
|
||||||
|
if final < 65:
|
||||||
|
print("Your motor is in need of attention. Make sure to clean it regularly and charge the Prime Hub.")
|
||||||
|
elif final < 85:
|
||||||
|
print("Your motor is in OK condition. Make sure to clean it regularly and charge the Prime Hub.")
|
||||||
|
elif final < 95:
|
||||||
|
print("Your motor is in great condition!")
|
||||||
|
else:
|
||||||
|
print("Your motor is in AMAZING condition!!!")
|
||||||
|
self.testmotor.stop()
|
||||||
176
shout-fll-scrimmage/bioglow_helpers.py
Normal file
176
shout-fll-scrimmage/bioglow_helpers.py
Normal file
@@ -0,0 +1,176 @@
|
|||||||
|
"""
|
||||||
|
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 bioglow_settings import * # hub, motors, sensors, drive_base, wait, umath, ...
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
|
# 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)
|
||||||
|
)
|
||||||
|
|
||||||
27
shout-fll-scrimmage/bioglow_missions_3_4_5.py
Normal file
27
shout-fll-scrimmage/bioglow_missions_3_4_5.py
Normal file
@@ -0,0 +1,27 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Missions 3,4,5
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Rishabh
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
27
shout-fll-scrimmage/bioglow_robot_settings.py
Normal file
27
shout-fll-scrimmage/bioglow_robot_settings.py
Normal file
@@ -0,0 +1,27 @@
|
|||||||
|
#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.
|
||||||
|
|
||||||
|
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
|
||||||
|
import umath
|
||||||
|
|
||||||
|
# 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]]) # FIXED: both gear trains in ONE list
|
||||||
|
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)
|
||||||
|
|
||||||
|
# The ONE drive base. Every other file imports it from here — never make another.
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
drive_base.settings(600, 500, 300, 200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
86
shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py
Normal file
86
shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py
Normal file
@@ -0,0 +1,86 @@
|
|||||||
|
import bioglow_helpers as helper
|
||||||
|
import bioglow_settings as robot
|
||||||
|
import bioglow_mission_3_4_5 as Run_1
|
||||||
|
|
||||||
|
# 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 < 24 and s > 80: # orange
|
||||||
|
return "Orange"
|
||||||
|
elif 30 < h < 60 and v > 50: # 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"
|
||||||
|
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}")
|
||||||
|
|
||||||
|
|
||||||
|
if color == "Green":
|
||||||
|
print('Running Mission 1, Detect Green')
|
||||||
|
await Run_1.solve_missions()
|
||||||
|
elif color == "Red":
|
||||||
|
print('Running Mission 2, Detect Red')
|
||||||
|
pass # TODO: Run2 not written yet (it crashed with NameError)
|
||||||
|
elif color == "Yellow":
|
||||||
|
print('Running Mission 3, Detect Yellow')
|
||||||
|
pass # TODO: Run3 not written yet (it crashed with NameError)
|
||||||
|
elif color == "Blue":
|
||||||
|
print('Running Mission 4, Detect Blue')
|
||||||
|
pass # TODO: Run4 not written yet (it crashed with NameError)
|
||||||
|
elif color == "Orange":
|
||||||
|
print('Running Mission 5, Detect Orange')
|
||||||
|
pass # TODO: Run5 not written yet (it crashed with NameError)
|
||||||
|
elif color == "Purple":
|
||||||
|
print('Running Mission 6, Detect Purple')
|
||||||
|
pass # TODO: Run6 not written yet (it crashed with NameError)
|
||||||
|
elif color == "Light_Blue":
|
||||||
|
print('Running Mission 7, Detect Cyan')
|
||||||
|
pass # TODO: Run7 not written yet (it crashed with NameError)
|
||||||
|
elif color == "White":
|
||||||
|
print('Detect White')
|
||||||
|
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())
|
||||||
@@ -1,6 +0,0 @@
|
|||||||
'''
|
|
||||||
|
|
||||||
This is where you write the main code to start all mission codes
|
|
||||||
Combine all call here
|
|
||||||
|
|
||||||
'''
|
|
||||||
@@ -1 +0,0 @@
|
|||||||
#Write Code here for your missions
|
|
||||||
Reference in New Issue
Block a user