FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed
Template
10
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 7 Wiki Activity

Upload files to "utils/tests/FullDiagnostics"

Browse Source
This commit is contained in:
Vickram
2025-12-06 17:22:50 +00:00
parent 61d3dd3c06
commit 821924e875
2 changed files with 68 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
utils/tests/FullDiagnostics/BatteryDiagnostics.py Normal file
View File

@@ -0,0 +1,33 @@
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) # 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(600, 500, 300, 200)
drive_base.use_gyro(True)
WALL_DISTANCE = 200 # mm
if hub.battery.voltage() > 7800:
print(f"Battery voltage is sufficient: {hub.battery.voltage()}")
elif hub.battery.voltage < 7800 :
print(f"Charging needed: {hub.battery.voltage()}")
print(hub.battery.current())
print(hub.imu.heading())
print(hub.system.info())

35
utils/tests/FullDiagnostics/FullDiagnostics.py Normal file
View File

@@ -0,0 +1,35 @@
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
battery = BatteryDiagnostics()
clearConfirmation = input("Do you want to clear the console before proceeding? Y/N (default: yes): ")
if(clearConfirmation == "Y" or clearConfirmation == "y" or clearConfirmation == "yes" or clearConfirmation == ""):
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")
while True:
print("\nWhat diagnostic do you want to perform?")
print("Enter 'b' for Battery diagnostics")
print("Enter 'm' for Motor diagnostics")
print("Enter 'q' to Quit")
choice = input("Your choice: ").strip().lower()
if choice == "b":
print("-----------------------BATTERY DIAGNOSTICS-----------------------")
battery.printAll()
elif choice == "m":
print("------------------------MOTOR DIAGNOSTICS------------------------")
# motor.printAll() # call your motor diagnostics here
print("Motor diagnostics would run here.")
elif choice == "q":
print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
break
else:
print("Invalid choice. Please enter 'b', 'm', or 'q'.")