Upload files to "/"
This commit is contained in:
49
BatteryDiagnostics.py
Normal file
49
BatteryDiagnostics.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
ColorSensorDiagnostics.py
Normal file
48
ColorSensorDiagnostics.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
DriveBaseDiagnostics.py
Normal file
165
DriveBaseDiagnostics.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
FullDiagnostics.py
Normal file
74
FullDiagnostics.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
HubDiagnostics.py
Normal file
35
HubDiagnostics.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)
|
||||||
Reference in New Issue
Block a user