diff --git a/utils/tests/colorsensortest.py b/utils/tests/colorsensortest.py new file mode 100644 index 0000000..b01d0d9 --- /dev/null +++ b/utils/tests/colorsensortest.py @@ -0,0 +1,45 @@ +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() + +color_sensor = ColorSensor(Port.F) + +# Color Settings +# https://docs.pybricks.com/en/latest/parameters/color.html +print("Default Detected Colors:", color_sensor.detectable_colors()) + +# Custom color Hue, Saturation, Brightness value for Lego bricks +Color.MAGENTA = Color(300,100,100) +Color.VIOLET = Color(277,68,32) +Color.BLUE = Color(240,100,100) +Color.CYAN = Color(180,100,100) + +LEGO_BRICKS_COLOR = [ + Color.BLUE, + Color.GREEN, + Color.WHITE, + Color.RED, + Color.YELLOW, + Color.MAGENTA, + Color.VIOLET, + Color.NONE +] +#Update Detectable colors +color_sensor.detectable_colors(LEGO_BRICKS_COLOR) +print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}') +print("Updated Detected Colors:", color_sensor.detectable_colors()) +async def main(): + while True: + color_reflected_percent = await color_sensor.reflection() + print(color_reflected_percent) + + if color_reflected_percent > 1: # Make sure we actually have color reflections before checking for color + color_detected = await color_sensor.color() + print(f'Detected color:{color_detected} : {color_detected.h}, {color_detected.s}, {color_detected.v}') +run_task(main()) \ No newline at end of file