From ddb2b354333093b88ca5dc082465a7c030115e22 Mon Sep 17 00:00:00 2001 From: Lucas Niewohner Date: Wed, 5 Feb 2025 19:08:26 -0700 Subject: [PATCH 01/15] Import lovely WS2812 library for Jetson from random github man --- .../rov_led_controller/WS2812.py | 65 +++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 src/rov/rov_led_controller/rov_led_controller/WS2812.py diff --git a/src/rov/rov_led_controller/rov_led_controller/WS2812.py b/src/rov/rov_led_controller/rov_led_controller/WS2812.py new file mode 100644 index 0000000..f063b64 --- /dev/null +++ b/src/rov/rov_led_controller/rov_led_controller/WS2812.py @@ -0,0 +1,65 @@ +# lifted from this lovely repo: https://github.com/seitomatsubara/Jetson-nano-WS2812-LED- + +import spidev +import sys + +class SPItoWS(): + def __init__(self, ledc): + self.led_count = ledc + self.X = '' # X is signal of WS281x + for i in range(self.led_count): + self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100" + self.spi = spidev.SpiDev() + self.spi.open(0, 0) + self.spi.max_speed_hz = 2400000 + + def __del__(self): + self.spi.close() + + def _Bytesto3Bytes(self, num, RGB): # num is number of signal, RGB is 8 bits (1 byte) str + for i in range(8): + if RGB[i] == '0': + self.X = self.X[:num * 3 * 8 + i * 3] + '100' + self.X[num * 3 * 8 + i * 3 + 3:] + elif RGB[i] == '1': + self.X = self.X[:num * 3 * 8 + i * 3] + '110' + self.X[num * 3 * 8 + i * 3 + 3:] + + def _BytesToHex(self, Bytes): + return ''.join(["0x%02X " % x for x in Bytes]).strip() + + def LED_show(self): + Y = [] + for i in range(self.led_count * 9): + Y.append(int(self.X[i*8:(i+1)*8],2)) + WS = self._BytesToHex(Y) + self.spi.xfer3(Y, 2400000,0,8) + + def RGBto3Bytes(self, led_num, R, G, B): + if (R > 255 or G > 255 or B > 255): + print("Invalid Value: RGB is over 255\n") + sys.exit(1) + if (led_num > self.led_count - 1): + print("Invalid Value: The number is over the number of LED") + sys.exit(1) + RR = format(R, '08b') + GG = format(G, '08b') + BB = format(B, '08b') + self._Bytesto3Bytes(led_num * 3, GG) + self._Bytesto3Bytes(led_num * 3 + 1, RR) + self._Bytesto3Bytes(led_num * 3 + 2, BB) + + def LED_OFF_ALL(self): + self.X = '' + for i in range(self.led_count): + self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100" + self.LED_show() + +if __name__ == "__main__": + import time + LED_COUNT = 3 + sig = SPItoWS(LED_COUNT) + sig.RGBto3Bytes(0, 255, 0, 0) + sig.RGBto3Bytes(1, 0, 255, 0) + sig.RGBto3Bytes(2, 0, 0, 255) + sig.LED_show() + time.sleep(1) + sig.LED_OFF_ALL() \ No newline at end of file From 11d6ca1a54fbadafcf42c44f4aea05d831f2ad41 Mon Sep 17 00:00:00 2001 From: kadin Date: Wed, 5 Mar 2025 19:58:55 -0700 Subject: [PATCH 02/15] Implemented WS2812 and it builds --- .../rov_led_controller/led_controller.py | 131 +++++++++++------ src/rov/rov_led_controller/setup.py | 5 +- .../src/rov/led_controller/led_controller.py | 136 ------------------ 3 files changed, 88 insertions(+), 184 deletions(-) delete mode 100644 src/rov/rov_led_controller/src/rov/led_controller/led_controller.py diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index a8ca065..01ddc15 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -3,15 +3,13 @@ import Jetson.GPIO as GPIO import rclpy from rclpy.node import Node -from rainbowio import colorwheel -import board -import neopixel_spi import time import random +from WS2812 import SPItoWS + NUM_LIGHTS = 60 -LED_PIN = board.SPI()._pins[0] BLACK = (0,0,0) WHITE = (255,255,255) RED = (255, 0, 0) @@ -20,8 +18,8 @@ CYAN = (0, 255, 255) BLUE = (0, 0, 255) PURPLE = (180, 0, 255) -PIXELS = neopixel_spi.NeoPixel_SPI(board.SPI(), NUM_LIGHTS) -global ledState +PIXELS = SPItoWS(NUM_LIGHTS) + global ledColor class LEDControllerNode(Node): @@ -31,13 +29,13 @@ def __init__(self): self.state_subscriber = self.create_subscription(PinState, "set_rov_gpio", self.on_set_rov_gpio, 10) self.publisher_ = self.create_publisher(String, 'preprogrammed_animations', 10) - self.animations = [ - "color_chase", - "rainbow_cycle", - "pulse", - "solid", - "seizure_disco" - ] + self.animations = { + "color_chase": self.color_chase, + "rainbow_cycle": self.rainbow_cycle, + "pulse": self.pulse, + "solid": self.solid, + "seizure_disco": self.seizure_disco + } self.colors = [ "WHITE", @@ -49,11 +47,17 @@ def __init__(self): "BLUE", "PURPLE" ] + + # Rainbow RGB values + self.rainbowR = 255 + self.rainbowG = 255 + self.rainbowB = 0 # Listen for requests from the UI self.ui_subscriber = self.create_subscription(String, 'ui_requests', self.ui_request_callback, 10) - PIXELS.fill(BLACK) + PIXELS.LED_OFF_ALL() + def ui_request_callback(self, msg): if msg.data == 'get_animations': @@ -71,59 +75,94 @@ def ui_request_callback(self, msg): # Check if the requested animation exists if msg.data in self.animations: # Call the method corresponding to the requested animation - - if ledState and (lastAnimation != msg.data): - exec(f"self.{msg.data}({ledColor}, 0.1)")() - else: - PIXELS.brightness(0) - lastAnimation = msg.data - elif msg.data in self.colors: - lastColor = ledColor - if ledState and (lastColor != msg.data): - exec(f"self.{lastAnimation}({msg.data}, 0.1)")() - - - def on_set_rov_gpio(self, message: PinState): - if message.pin not in LED_PIN: - pass - ledState = message.state + self.animations[msg.data]() + def color_chase(color, wait=0.1): for i in range(NUM_LIGHTS): - PIXELS[i] = color + PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) time.sleep(wait) - PIXELS.show() + PIXELS.LED_show() time.sleep(0.5) - def rainbow_cycle(color, wait=0.1): - for j in range(255): - for i in range(NUM_LIGHTS): - rc_index = (i * 256 // NUM_LIGHTS) + j - PIXELS[i] = colorwheel(rc_index & 255) - PIXELS.show() - time.sleep(wait) + + def rainbow_cycle(self, color, wait=0.1): + print("Hello world") + RGBincrement = float(255 * 3.0) / NUM_LIGHTS + # LED RGB values + tempR = self.rainbowR + tempG = self.rainbowG + tempB = self.rainbowB + + # Assigning LED RGB values + for i in range(NUM_LIGHTS): + if tempR == 255 and tempB == 0 and tempG < 255: + PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) + tempG += RGBincrement + elif tempG == 255 and tempB == 0 and tempR > 0: + PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) + tempR -= RGBincrement + elif tempG == 255 and tempR == 0 and tempB < 255: + PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) + tempB += RGBincrement + elif tempB == 255 and tempR == 0 and tempG > 0: + PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) + tempG -= RGBincrement + elif tempB == 255 and tempG == 0 and tempR < 255: + PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) + tempR += RGBincrement + elif tempR == 255 and tempG == 0 and tempB > 0: + PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) + tempB -= RGBincrement + PIXELS.LED_show() + time.sleep(wait) + + # increment first rainbow RGB values to next value + if self.rainbowR == 255 and self.rainbowB == 0 and self.rainbowG < 255: + self.rainbowG += 1 + elif self.rainbowG == 255 and self.rainbowB == 0 and self.rainbowR > 0: + self.rainbowR -= 1 + elif self.rainbowG == 255 and self.rainbowR == 0 and self.rainbowB < 255: + self.rainbowB += 1 + elif self.rainbowB == 255 and self.rainbowR == 0 and self.rainbowG > 0: + self.rainbowG -= 1 + elif self.rainbowB == 255 and self.rainbowG == 0 and self.rainbowR < 255: + self.rainbowR += 1 + elif self.rainbowR == 255 and self.rainbowG == 0 and self.rainbowB > 0: + self.rainbowB -= 1 + def pulse(color, wait=0.1): - PIXELS.fill(color) for i in range(255): - PIXELS.setBrightness(i) + for j in range(NUM_LIGHTS): + PIXELS.RGBto3Bytes(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) time.sleep(wait) - PIXELS.show() + PIXELS.LED_show() time.sleep(wait) def solid(color, wait=0.1): - PIXELS.fill(color) - PIXELS.show() + for i in range (NUM_LIGHTS): + PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) + PIXELS.LED_show() def seizure_disco(color, wait=0.1): for i in range(NUM_LIGHTS): R = random.randint(0,255) G = random.randint(0,255) B = random.randint(0,255) - PIXELS[i].fill(R, G, B) - PIXELS.show() + PIXELS.RGBto3Bytes(i, R, G, B) + PIXELS.LED_show() time.sleep(wait) + def _set_all_pixels(color): + """ + Sets all pixels to a single + :param color: color to set all pixels to + """ + for i in range (NUM_LIGHTS): + PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) + + def main(args=None): rclpy.init(args=args) diff --git a/src/rov/rov_led_controller/setup.py b/src/rov/rov_led_controller/setup.py index 479e134..ed33070 100644 --- a/src/rov/rov_led_controller/setup.py +++ b/src/rov/rov_led_controller/setup.py @@ -1,6 +1,7 @@ from setuptools import find_packages, setup -package_name = 'src/rov/rov_led_controller' +# __name__ in ['__main__', 'builtins'] and __import__('setuptools').setup() +package_name = 'led_controller' setup( name=package_name, @@ -20,7 +21,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'led_controller = src/rov/rov_led_controller.led_controller:main' + 'led_controller = led_controller.led_controller:main' ], }, ) diff --git a/src/rov/rov_led_controller/src/rov/led_controller/led_controller.py b/src/rov/rov_led_controller/src/rov/led_controller/led_controller.py deleted file mode 100644 index a8ca065..0000000 --- a/src/rov/rov_led_controller/src/rov/led_controller/led_controller.py +++ /dev/null @@ -1,136 +0,0 @@ -from common.csm_common_interfaces.msg import PinState -from std_msgs.msg import String -import Jetson.GPIO as GPIO -import rclpy -from rclpy.node import Node -from rainbowio import colorwheel - -import board -import neopixel_spi -import time -import random - -NUM_LIGHTS = 60 -LED_PIN = board.SPI()._pins[0] -BLACK = (0,0,0) -WHITE = (255,255,255) -RED = (255, 0, 0) -YELLOW = (255, 150, 0) -GREEN = (0, 255, 0) -CYAN = (0, 255, 255) -BLUE = (0, 0, 255) -PURPLE = (180, 0, 255) -PIXELS = neopixel_spi.NeoPixel_SPI(board.SPI(), NUM_LIGHTS) -global ledState -global ledColor - -class LEDControllerNode(Node): - - def __init__(self): - super().__init__(node_name="LEDController") - self.state_subscriber = self.create_subscription(PinState, "set_rov_gpio", self.on_set_rov_gpio, 10) - self.publisher_ = self.create_publisher(String, 'preprogrammed_animations', 10) - - self.animations = [ - "color_chase", - "rainbow_cycle", - "pulse", - "solid", - "seizure_disco" - ] - - self.colors = [ - "WHITE", - "BLACK", - "RED", - "YELLOW", - "GREEN", - "CYAN", - "BLUE", - "PURPLE" - ] - - # Listen for requests from the UI - self.ui_subscriber = self.create_subscription(String, 'ui_requests', self.ui_request_callback, 10) - - PIXELS.fill(BLACK) - - def ui_request_callback(self, msg): - if msg.data == 'get_animations': - # Send the list of preprogrammed animations to the UI - animations_msg = String() - animations_msg.data = "\n".join(animation for animation in self.animations) - self.publisher_.publish(animations_msg) - self.get_logger().info('Sent preprogrammed animations to UI') - elif msg.data == 'get_colors': - colors_msg = String() - colors_msg.data = "\n".join(color for color in self.colors) - self.publisher_.publish(colors_msg) - self.get_logger().info('Sent preprogrammed colors to UI') - else: - # Check if the requested animation exists - if msg.data in self.animations: - # Call the method corresponding to the requested animation - - if ledState and (lastAnimation != msg.data): - exec(f"self.{msg.data}({ledColor}, 0.1)")() - else: - PIXELS.brightness(0) - lastAnimation = msg.data - elif msg.data in self.colors: - lastColor = ledColor - if ledState and (lastColor != msg.data): - exec(f"self.{lastAnimation}({msg.data}, 0.1)")() - - - def on_set_rov_gpio(self, message: PinState): - if message.pin not in LED_PIN: - pass - ledState = message.state - - def color_chase(color, wait=0.1): - for i in range(NUM_LIGHTS): - PIXELS[i] = color - time.sleep(wait) - PIXELS.show() - time.sleep(0.5) - - def rainbow_cycle(color, wait=0.1): - for j in range(255): - for i in range(NUM_LIGHTS): - rc_index = (i * 256 // NUM_LIGHTS) + j - PIXELS[i] = colorwheel(rc_index & 255) - PIXELS.show() - time.sleep(wait) - - def pulse(color, wait=0.1): - PIXELS.fill(color) - for i in range(255): - PIXELS.setBrightness(i) - time.sleep(wait) - PIXELS.show() - time.sleep(wait) - - def solid(color, wait=0.1): - PIXELS.fill(color) - PIXELS.show() - - def seizure_disco(color, wait=0.1): - for i in range(NUM_LIGHTS): - R = random.randint(0,255) - G = random.randint(0,255) - B = random.randint(0,255) - PIXELS[i].fill(R, G, B) - PIXELS.show() - time.sleep(wait) - - -def main(args=None): - rclpy.init(args=args) - led_controller = LEDControllerNode() - rclpy.spin(led_controller) - led_controller.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() From 97c2ae158b7c05870262da25d499978e2f77011a Mon Sep 17 00:00:00 2001 From: kadin Date: Mon, 10 Mar 2025 19:58:50 -0600 Subject: [PATCH 03/15] Hopefully fixed runtime errors, needs actual testing --- .../rov_led_controller/led_controller.py | 75 +++++++++++-------- src/rov/rov_led_controller/setup.py | 5 +- 2 files changed, 45 insertions(+), 35 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 01ddc15..4dc39f3 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -1,32 +1,17 @@ -from common.csm_common_interfaces.msg import PinState from std_msgs.msg import String -import Jetson.GPIO as GPIO import rclpy from rclpy.node import Node - import time import random - from WS2812 import SPItoWS NUM_LIGHTS = 60 -BLACK = (0,0,0) -WHITE = (255,255,255) -RED = (255, 0, 0) -YELLOW = (255, 150, 0) -GREEN = (0, 255, 0) -CYAN = (0, 255, 255) -BLUE = (0, 0, 255) -PURPLE = (180, 0, 255) PIXELS = SPItoWS(NUM_LIGHTS) -global ledColor - class LEDControllerNode(Node): def __init__(self): super().__init__(node_name="LEDController") - self.state_subscriber = self.create_subscription(PinState, "set_rov_gpio", self.on_set_rov_gpio, 10) self.publisher_ = self.create_publisher(String, 'preprogrammed_animations', 10) self.animations = { @@ -37,21 +22,25 @@ def __init__(self): "seizure_disco": self.seizure_disco } - self.colors = [ - "WHITE", - "BLACK", - "RED", - "YELLOW", - "GREEN", - "CYAN", - "BLUE", - "PURPLE" - ] + self.colors = { + "WHITE": (255,255,255), + "BLACK": (0,0,0), + "RED": (255, 0, 0), + "YELLOW": (255, 150, 0), + "GREEN": (0, 255, 0), + "CYAN": (0, 255, 255), + "BLUE": (0, 0, 255), + "PURPLE": (180, 0, 255) + } # Rainbow RGB values self.rainbowR = 255 self.rainbowG = 255 self.rainbowB = 0 + + # color value and current animation + self.ledColor = "WHITE" + self.currentAnimation = "solid" # Listen for requests from the UI self.ui_subscriber = self.create_subscription(String, 'ui_requests', self.ui_request_callback, 10) @@ -75,10 +64,20 @@ def ui_request_callback(self, msg): # Check if the requested animation exists if msg.data in self.animations: # Call the method corresponding to the requested animation - self.animations[msg.data]() + self.currentAnimation = msg.data + self.get_logger().info('New animation received: %s' % msg.data) + self.animations[msg.data](self.colors[self.ledColor]) + elif msg.data in self.colors: + # Change the color if message changes colors + self.ledColor = msg.data + self.get_logger().info('New color received: %s' % msg.data) + self.animations[self.currentAnimation](self.colors[msg.data]) - def color_chase(color, wait=0.1): + def color_chase(self, color, wait=0.1): + """ + Animates a single color across the LED strip + """ for i in range(NUM_LIGHTS): PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) time.sleep(wait) @@ -87,7 +86,9 @@ def color_chase(color, wait=0.1): def rainbow_cycle(self, color, wait=0.1): - print("Hello world") + """ + Cycles a wave of rainbow over the LEDs + """ RGBincrement = float(255 * 3.0) / NUM_LIGHTS # LED RGB values tempR = self.rainbowR @@ -132,7 +133,10 @@ def rainbow_cycle(self, color, wait=0.1): self.rainbowB -= 1 - def pulse(color, wait=0.1): + def pulse(self, color, wait=0.1): + """ + Fades a single color in + """ for i in range(255): for j in range(NUM_LIGHTS): PIXELS.RGBto3Bytes(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) @@ -140,12 +144,18 @@ def pulse(color, wait=0.1): PIXELS.LED_show() time.sleep(wait) - def solid(color, wait=0.1): + def solid(self, color, wait=0.1): + """ + Changes all LEDs to solid color + """ for i in range (NUM_LIGHTS): PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) PIXELS.LED_show() - def seizure_disco(color, wait=0.1): + def seizure_disco(self, color, wait=0.1): + """ + Changes all LEDs to a random color + """ for i in range(NUM_LIGHTS): R = random.randint(0,255) G = random.randint(0,255) @@ -154,10 +164,9 @@ def seizure_disco(color, wait=0.1): PIXELS.LED_show() time.sleep(wait) - def _set_all_pixels(color): + def _set_all_pixels(self, color): """ Sets all pixels to a single - :param color: color to set all pixels to """ for i in range (NUM_LIGHTS): PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) diff --git a/src/rov/rov_led_controller/setup.py b/src/rov/rov_led_controller/setup.py index ed33070..d85c938 100644 --- a/src/rov/rov_led_controller/setup.py +++ b/src/rov/rov_led_controller/setup.py @@ -1,6 +1,5 @@ from setuptools import find_packages, setup -# __name__ in ['__main__', 'builtins'] and __import__('setuptools').setup() package_name = 'led_controller' setup( @@ -11,12 +10,14 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # add WS2812 + ('lib/' + package_name, [package_name+'/WS2812.py']) ], install_requires=['setuptools'], zip_safe=True, maintainer='catfishjw', maintainer_email='james.wiley@live.com', - description='TODO: Package description', + description='It controls LEDs', license='TODO: License declaration', tests_require=['pytest'], entry_points={ From f89ac466def37af5fe73eb50609c17babfacbd8f Mon Sep 17 00:00:00 2001 From: kadin Date: Thu, 13 Mar 2025 20:48:30 -0600 Subject: [PATCH 04/15] Added off function and brightness control --- .../rov_led_controller/WS2812.py | 15 ++++-- .../rov_led_controller/led_controller.py | 47 ++++++++++++++----- 2 files changed, 47 insertions(+), 15 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/WS2812.py b/src/rov/rov_led_controller/rov_led_controller/WS2812.py index f063b64..981e9d6 100644 --- a/src/rov/rov_led_controller/rov_led_controller/WS2812.py +++ b/src/rov/rov_led_controller/rov_led_controller/WS2812.py @@ -9,12 +9,14 @@ def __init__(self, ledc): self.X = '' # X is signal of WS281x for i in range(self.led_count): self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100" + self.led_brightness = 1.0 self.spi = spidev.SpiDev() self.spi.open(0, 0) self.spi.max_speed_hz = 2400000 def __del__(self): self.spi.close() + print("destructor") def _Bytesto3Bytes(self, num, RGB): # num is number of signal, RGB is 8 bits (1 byte) str for i in range(8): @@ -40,19 +42,26 @@ def RGBto3Bytes(self, led_num, R, G, B): if (led_num > self.led_count - 1): print("Invalid Value: The number is over the number of LED") sys.exit(1) - RR = format(R, '08b') - GG = format(G, '08b') - BB = format(B, '08b') + RR = format(round(R * self.led_brightness), '08b') + GG = format(round(G * self.led_brightness), '08b') + BB = format(round(B * self.led_brightness), '08b') self._Bytesto3Bytes(led_num * 3, GG) self._Bytesto3Bytes(led_num * 3 + 1, RR) self._Bytesto3Bytes(led_num * 3 + 2, BB) + # debug + # print("led_num: %d, R: %d G: %d B: %d" % (led_num, round(R * self.led_brightness), round(G * self.led_brightness), round(B * self.led_brightness))) + def LED_OFF_ALL(self): self.X = '' for i in range(self.led_count): self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100" self.LED_show() + def set_brightness(self, brightness): + self.led_brightness = brightness + + if __name__ == "__main__": import time LED_COUNT = 3 diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 4dc39f3..5017803 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -12,14 +12,15 @@ class LEDControllerNode(Node): def __init__(self): super().__init__(node_name="LEDController") - self.publisher_ = self.create_publisher(String, 'preprogrammed_animations', 10) + self.publisher_ = self.create_publisher(String, 'led_controller_output', 10) self.animations = { "color_chase": self.color_chase, "rainbow_cycle": self.rainbow_cycle, "pulse": self.pulse, "solid": self.solid, - "seizure_disco": self.seizure_disco + "seizure_disco": self.seizure_disco, + "off": self.off } self.colors = { @@ -49,29 +50,48 @@ def __init__(self): def ui_request_callback(self, msg): - if msg.data == 'get_animations': + # Split message + split = msg.data.split(",") + msgs = [s.strip() for s in split] + + # Change brightness + if len(msgs) == 2: + try: + brightness = float(msgs[1]) + if brightness > 1.0: + brightness = 1 + elif brightness < 0: + brightness = 0 + PIXELS.set_brightness(brightness) + self.get_logger().info("Changed brightness to: %f" % brightness) + except: + self.get_logger().info("Second argument, brightness, should be a float.") + + # Change animation and colors + if msgs[0] == 'get_animations': # Send the list of preprogrammed animations to the UI animations_msg = String() animations_msg.data = "\n".join(animation for animation in self.animations) self.publisher_.publish(animations_msg) self.get_logger().info('Sent preprogrammed animations to UI') - elif msg.data == 'get_colors': + elif msgs[0] == 'get_colors': colors_msg = String() colors_msg.data = "\n".join(color for color in self.colors) self.publisher_.publish(colors_msg) self.get_logger().info('Sent preprogrammed colors to UI') else: # Check if the requested animation exists - if msg.data in self.animations: + if msgs[0] in self.animations: # Call the method corresponding to the requested animation - self.currentAnimation = msg.data - self.get_logger().info('New animation received: %s' % msg.data) - self.animations[msg.data](self.colors[self.ledColor]) - elif msg.data in self.colors: + self.currentAnimation = msgs[0] + self.get_logger().info('New animation received: %s' % msgs[0]) + self.animations[msgs[0]](self.colors[self.ledColor]) + elif msgs[0] in self.colors: # Change the color if message changes colors - self.ledColor = msg.data - self.get_logger().info('New color received: %s' % msg.data) - self.animations[self.currentAnimation](self.colors[msg.data]) + self.ledColor = msgs[0] + self.get_logger().info('New color received: %s' % msgs[0]) + self.animations[self.currentAnimation](self.colors[msgs[0]]) + def color_chase(self, color, wait=0.1): @@ -164,6 +184,9 @@ def seizure_disco(self, color, wait=0.1): PIXELS.LED_show() time.sleep(wait) + def off(self, color): + PIXELS.LED_OFF_ALL() + def _set_all_pixels(self, color): """ Sets all pixels to a single From 34f5cd4097378a9eaab5977535f7a73c61255c30 Mon Sep 17 00:00:00 2001 From: kadin Date: Wed, 26 Mar 2025 18:49:48 -0600 Subject: [PATCH 05/15] Add customizable spi bus and device to WS2812 library --- src/rov/rov_led_controller/rov_led_controller/WS2812.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/WS2812.py b/src/rov/rov_led_controller/rov_led_controller/WS2812.py index 981e9d6..4a0d366 100644 --- a/src/rov/rov_led_controller/rov_led_controller/WS2812.py +++ b/src/rov/rov_led_controller/rov_led_controller/WS2812.py @@ -4,14 +4,14 @@ import sys class SPItoWS(): - def __init__(self, ledc): + def __init__(self, ledc, bus=1, device=0): self.led_count = ledc self.X = '' # X is signal of WS281x for i in range(self.led_count): self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100" self.led_brightness = 1.0 self.spi = spidev.SpiDev() - self.spi.open(0, 0) + self.spi.open(bus, device) self.spi.max_speed_hz = 2400000 def __del__(self): From 3bab00bcaf90f59aee04f3bc1afa33d46a20894e Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Wed, 2 Apr 2025 12:37:09 -0600 Subject: [PATCH 06/15] Add HSV rainbow cycle, implemented hermanoid's changes --- .../rov_led_controller/led_controller.py | 59 ++++--------------- src/rov/rov_led_controller/setup.cfg | 4 +- src/rov/rov_led_controller/setup.py | 4 +- 3 files changed, 17 insertions(+), 50 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 5017803..36bc7ad 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -3,6 +3,7 @@ from rclpy.node import Node import time import random +import colorsys from WS2812 import SPItoWS NUM_LIGHTS = 60 @@ -57,11 +58,8 @@ def ui_request_callback(self, msg): # Change brightness if len(msgs) == 2: try: - brightness = float(msgs[1]) - if brightness > 1.0: - brightness = 1 - elif brightness < 0: - brightness = 0 + # set and clamp brightness between 0 and 1 + brightness = max(0.0, min(float(msgs[1]), 1.0)) PIXELS.set_brightness(brightness) self.get_logger().info("Changed brightness to: %f" % brightness) except: @@ -109,48 +107,17 @@ def rainbow_cycle(self, color, wait=0.1): """ Cycles a wave of rainbow over the LEDs """ - RGBincrement = float(255 * 3.0) / NUM_LIGHTS - # LED RGB values - tempR = self.rainbowR - tempG = self.rainbowG - tempB = self.rainbowB - - # Assigning LED RGB values + # HSV implementation + hueIncrement = 1.0 / NUM_LIGHTS for i in range(NUM_LIGHTS): - if tempR == 255 and tempB == 0 and tempG < 255: - PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) - tempG += RGBincrement - elif tempG == 255 and tempB == 0 and tempR > 0: - PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) - tempR -= RGBincrement - elif tempG == 255 and tempR == 0 and tempB < 255: - PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) - tempB += RGBincrement - elif tempB == 255 and tempR == 0 and tempG > 0: - PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) - tempG -= RGBincrement - elif tempB == 255 and tempG == 0 and tempR < 255: - PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) - tempR += RGBincrement - elif tempR == 255 and tempG == 0 and tempB > 0: - PIXELS.RGBto3Bytes(i, tempR, tempG, tempB) - tempB -= RGBincrement - PIXELS.LED_show() - time.sleep(wait) - - # increment first rainbow RGB values to next value - if self.rainbowR == 255 and self.rainbowB == 0 and self.rainbowG < 255: - self.rainbowG += 1 - elif self.rainbowG == 255 and self.rainbowB == 0 and self.rainbowR > 0: - self.rainbowR -= 1 - elif self.rainbowG == 255 and self.rainbowR == 0 and self.rainbowB < 255: - self.rainbowB += 1 - elif self.rainbowB == 255 and self.rainbowR == 0 and self.rainbowG > 0: - self.rainbowG -= 1 - elif self.rainbowB == 255 and self.rainbowG == 0 and self.rainbowR < 255: - self.rainbowR += 1 - elif self.rainbowR == 255 and self.rainbowG == 0 and self.rainbowB > 0: - self.rainbowB -= 1 + for j in range(NUM_LIGHTS): + if (j + i) <= NUM_LIGHTS: + rgb = colorsys.hsv_to_rgb(hueIncrement * (j + i + 1), 1, 1) + else: + rgb = colorsys.hsv_to_rgb(hueIncrement * (j + i + 1 - 60), 1, 1) + PIXELS.RGBto3Bytes(j, rgb[0] * 255, rgb[1] * 255, rgb[2] * 255) + PIXELS.LED_show() + time.sleep(wait) def pulse(self, color, wait=0.1): diff --git a/src/rov/rov_led_controller/setup.cfg b/src/rov/rov_led_controller/setup.cfg index 8ac3298..df03f45 100644 --- a/src/rov/rov_led_controller/setup.cfg +++ b/src/rov/rov_led_controller/setup.cfg @@ -1,4 +1,4 @@ [develop] -script_dir=$base/lib/src/rov/rov_led_controller +script_dir=$base/lib/rov_led_controller [install] -install_scripts=$base/lib/src/rov/rov_led_controller +install_scripts=$base/lib/rov_led_controller diff --git a/src/rov/rov_led_controller/setup.py b/src/rov/rov_led_controller/setup.py index d85c938..ae9c8f3 100644 --- a/src/rov/rov_led_controller/setup.py +++ b/src/rov/rov_led_controller/setup.py @@ -1,6 +1,6 @@ from setuptools import find_packages, setup -package_name = 'led_controller' +package_name = 'rov_led_controller' setup( name=package_name, @@ -22,7 +22,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'led_controller = led_controller.led_controller:main' + 'led_controller = rov_led_controller.led_controller:main' ], }, ) From a67d9e4265535faf86e69046dde3144f7452ad10 Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Wed, 2 Apr 2025 19:53:02 -0600 Subject: [PATCH 07/15] Rainbow cycle precalculations, limit the brightness --- .../rov_led_controller/led_controller.py | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 36bc7ad..0cb8c84 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -4,6 +4,7 @@ import time import random import colorsys +import threading from WS2812 import SPItoWS NUM_LIGHTS = 60 @@ -35,10 +36,13 @@ def __init__(self): "PURPLE": (180, 0, 255) } - # Rainbow RGB values - self.rainbowR = 255 - self.rainbowG = 255 - self.rainbowB = 0 + # Calculate rainbow RGB values + self.rainbowValues = [] + hueIncrement = 1.0 / NUM_LIGHTS + for i in range (1, NUM_LIGHTS + 1): + rgb = colorsys.hsv_to_rgb(i * hueIncrement, 1, 1) + scaled_rgb = tuple(int(value * 255) for value in rgb) + self.rainbowValues.append(scaled_rgb) # color value and current animation self.ledColor = "WHITE" @@ -60,7 +64,7 @@ def ui_request_callback(self, msg): try: # set and clamp brightness between 0 and 1 brightness = max(0.0, min(float(msgs[1]), 1.0)) - PIXELS.set_brightness(brightness) + PIXELS.set_brightness(brightness * 0.5) self.get_logger().info("Changed brightness to: %f" % brightness) except: self.get_logger().info("Second argument, brightness, should be a float.") @@ -79,7 +83,7 @@ def ui_request_callback(self, msg): self.get_logger().info('Sent preprogrammed colors to UI') else: # Check if the requested animation exists - if msgs[0] in self.animations: + if msgs[0] in self.animations: # Call the method corresponding to the requested animation self.currentAnimation = msgs[0] self.get_logger().info('New animation received: %s' % msgs[0]) @@ -103,19 +107,13 @@ def color_chase(self, color, wait=0.1): time.sleep(0.5) - def rainbow_cycle(self, color, wait=0.1): + def rainbow_cycle(self, color, wait = 2 / NUM_LIGHTS): """ Cycles a wave of rainbow over the LEDs """ - # HSV implementation - hueIncrement = 1.0 / NUM_LIGHTS for i in range(NUM_LIGHTS): - for j in range(NUM_LIGHTS): - if (j + i) <= NUM_LIGHTS: - rgb = colorsys.hsv_to_rgb(hueIncrement * (j + i + 1), 1, 1) - else: - rgb = colorsys.hsv_to_rgb(hueIncrement * (j + i + 1 - 60), 1, 1) - PIXELS.RGBto3Bytes(j, rgb[0] * 255, rgb[1] * 255, rgb[2] * 255) + for j in range (NUM_LIGHTS): + PIXELS.RGBto3Bytes(j, self.rainbowValues[i % NUM_LIGHTS][0], self.rainbowValues[i % NUM_LIGHTS][1], self.rainbowValues[i % NUM_LIGHTS][2]) PIXELS.LED_show() time.sleep(wait) @@ -131,6 +129,7 @@ def pulse(self, color, wait=0.1): PIXELS.LED_show() time.sleep(wait) + def solid(self, color, wait=0.1): """ Changes all LEDs to solid color @@ -139,6 +138,7 @@ def solid(self, color, wait=0.1): PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) PIXELS.LED_show() + def seizure_disco(self, color, wait=0.1): """ Changes all LEDs to a random color @@ -161,6 +161,7 @@ def _set_all_pixels(self, color): for i in range (NUM_LIGHTS): PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) + def main(args=None): From 9c537edca6bc73eae7646710df353cac743908ce Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Thu, 10 Apr 2025 12:01:43 -0600 Subject: [PATCH 08/15] Rainbow and other animations loop, smoother rainbow, boot up animation --- .../rov_led_controller/led_controller.py | 89 ++++++++++++++----- 1 file changed, 67 insertions(+), 22 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 0cb8c84..468193b 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -22,7 +22,8 @@ def __init__(self): "pulse": self.pulse, "solid": self.solid, "seizure_disco": self.seizure_disco, - "off": self.off + "off": self.off, + "boot": self.boot } self.colors = { @@ -38,8 +39,8 @@ def __init__(self): # Calculate rainbow RGB values self.rainbowValues = [] - hueIncrement = 1.0 / NUM_LIGHTS - for i in range (1, NUM_LIGHTS + 1): + hueIncrement = 1.0 / 256 + for i in range (1, 257): rgb = colorsys.hsv_to_rgb(i * hueIncrement, 1, 1) scaled_rgb = tuple(int(value * 255) for value in rgb) self.rainbowValues.append(scaled_rgb) @@ -51,6 +52,9 @@ def __init__(self): # Listen for requests from the UI self.ui_subscriber = self.create_subscription(String, 'ui_requests', self.ui_request_callback, 10) + # Stop flag + self.stopAnim = False + PIXELS.LED_OFF_ALL() @@ -84,11 +88,22 @@ def ui_request_callback(self, msg): else: # Check if the requested animation exists if msgs[0] in self.animations: + # Stop current animation + self.stopAnim = True + while threading.active_count() > 1: + time.sleep(0.1) + self.stopAnim = False # Call the method corresponding to the requested animation self.currentAnimation = msgs[0] self.get_logger().info('New animation received: %s' % msgs[0]) - self.animations[msgs[0]](self.colors[self.ledColor]) + tempThread = threading.Thread(target=self.animations[msgs[0]], args=(self.colors[self.ledColor],)) + tempThread.start() elif msgs[0] in self.colors: + # Stop current animation + self.stopAnim = True + while threading.active_count() > 1: + time.sleep(0.1) + self.stopAnim = False # Change the color if message changes colors self.ledColor = msgs[0] self.get_logger().info('New color received: %s' % msgs[0]) @@ -96,38 +111,52 @@ def ui_request_callback(self, msg): - def color_chase(self, color, wait=0.1): + def color_chase(self, color, wait=1/NUM_LIGHTS): """ Animates a single color across the LED strip """ - for i in range(NUM_LIGHTS): - PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) - time.sleep(wait) - PIXELS.LED_show() - time.sleep(0.5) + PIXELS.LED_OFF_ALL() + while self.stopAnim == False: + for i in range(NUM_LIGHTS): + PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) + time.sleep(wait) + PIXELS.LED_show() + if self.stopAnim == True: + break + for i in range(NUM_LIGHTS): + PIXELS.RGBto3Bytes(i, 0, 0, 0) + time.sleep(wait) + PIXELS.LED_show() + if self.stopAnim == True: + break def rainbow_cycle(self, color, wait = 2 / NUM_LIGHTS): """ Cycles a wave of rainbow over the LEDs """ - for i in range(NUM_LIGHTS): - for j in range (NUM_LIGHTS): - PIXELS.RGBto3Bytes(j, self.rainbowValues[i % NUM_LIGHTS][0], self.rainbowValues[i % NUM_LIGHTS][1], self.rainbowValues[i % NUM_LIGHTS][2]) - PIXELS.LED_show() - time.sleep(wait) + while self.stopAnim == False: + for i in range(256): + for j in range (NUM_LIGHTS): + PIXELS.RGBto3Bytes(j, self.rainbowValues[(j + i) % 256][0], self.rainbowValues[(j + i) % 256][1], self.rainbowValues[(j + i) % 256][2]) + PIXELS.LED_show() + time.sleep(wait) + if self.stopAnim == True: + break def pulse(self, color, wait=0.1): """ - Fades a single color in + Fades a single color in and out """ - for i in range(255): - for j in range(NUM_LIGHTS): - PIXELS.RGBto3Bytes(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) - time.sleep(wait) - PIXELS.LED_show() - time.sleep(wait) + while self.stopAnim == False: + for i in range(255): + for j in range(NUM_LIGHTS): + PIXELS.RGBto3Bytes(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) + time.sleep(wait) + PIXELS.LED_show() + if self.stopAnim == True: + break def solid(self, color, wait=0.1): @@ -154,6 +183,22 @@ def seizure_disco(self, color, wait=0.1): def off(self, color): PIXELS.LED_OFF_ALL() + def boot(self, color): + """ + Booting animation + """ + for i in range(NUM_LIGHTS): + PIXELS.RGBto3Bytes(i, 0, 255, 0) + PIXELS.LED_show() + time.sleep(1/NUM_LIGHTS) + time.sleep(0.3) + PIXELS.LED_OFF_ALL() + time.sleep(0.3) + self._set_all_pixels(self.colors["GREEN"]) + time.sleep(1) + PIXELS.LED_OFF_ALL() + + def _set_all_pixels(self, color): """ Sets all pixels to a single From 2fe7ec4172f965404fbba157c63b60ec4852ce61 Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Tue, 22 Apr 2025 23:59:02 -0600 Subject: [PATCH 09/15] New library for new RGB strip --- .../rov_led_controller/SK6812RGBW.py | 79 +++++++++++++++++++ .../rov_led_controller/WS2812.py | 2 +- .../rov_led_controller/led_controller.py | 50 ++++++------ src/rov/rov_led_controller/setup.py | 2 - 4 files changed, 107 insertions(+), 26 deletions(-) create mode 100644 src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py diff --git a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py new file mode 100644 index 0000000..497e2a9 --- /dev/null +++ b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py @@ -0,0 +1,79 @@ +import spidev +import sys + +class SPItoSK(): + def __init__(self, numLeds, bus = 1, device = 0): + self.ledCount = numLeds + self.ledBrightness = 1.0 + self.spi = spidev.SpiDev() + self.spi.open(bus, device) + self.spi.max_speed_hz = 2400000 + # string of bits to send to spi + self.binMsg = 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 + for i in range(numLeds - 1): + # 96 bits per led, 3 bits per real bit + self.binMsg = (self.binMsg << 96) | 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 + + def __del__(self): + self.spi.close() + print("destructor") + + def set_LED_color(self, ledNum, R, G, B, W): + """ + Adjust self.binMsg to the color desired + :param ledNum: the led to change starting with index 0 + :param R, G, B, W: RGBW values from 0-255 + """ + if (R > 255) or (G > 255) or (B > 255) or (W > 255) or (R < 0) or (G < 0) or (B < 0) or (W < 0): + print("RGBW values must be in the rage 0-255") + sys.exit(-1) + msgStartPos = (ledNum * 96) + 2 + self._formatBinMsg(msgStartPos, int(round(R * self.ledBrightness))) + self._formatBinMsg(msgStartPos + 24, int(round(G * self.ledBrightness))) + self._formatBinMsg(msgStartPos + (24 * 2), int(round(B * self.ledBrightness))) + self._formatBinMsg(msgStartPos + (24 * 3), int(round(W * self.ledBrightness))) + + def LED_show(self): + """ + Signals the LEDs + """ + bytesToSend = [] + while self.binMsg: + bytesToSend.insert(0, self.binMsg & ((1 << 8) - 1)) # Extract lowest 8 bits + self.binMsg >>= 8 # Shift right by 8 bits + self.spi.xfer(bytesToSend, delay_usec = 0, bits_per_word = 8) + + # debug + bytesString = [] + for i in bytesToSend: + bytesString.append(bin(i)) + print(bytesString) + + def set_brightness(self, brightness): + """ + Set the global brightness of LEDs + :param brightness: Brightness of LEDs 0-1, clamps if values are outside the range + """ + brightness = max(0.0, min(brightness, 1.0)) + self.ledBrightness = brightness + + + def _formatBinMsg(self, startPos, colorNum): + number = format(colorNum, '08b') + for i in range(8): + msgIndexPos = startPos + (3 * i) + colorBit = number[i] + if (colorBit == "0") and bin(self.binMsg)[msgIndexPos:msgIndexPos + 3] == "110": + self._flipBit(msgIndexPos + 1) + elif (colorBit == "1") and bin(self.binMsg)[msgIndexPos:msgIndexPos + 3] == "100": + self._flipBit(msgIndexPos) + + def _flipBit(self, position): + mask = 1 << len(bin(self.binMsg)[2:]) - position + self.binMsg = self.binMsg ^ mask + + +if __name__ == "__main__": + ledStrip = SPItoSK(1) + ledStrip.set_LED_color(0, 255, 255, 255, 255) + ledStrip.LED_show() \ No newline at end of file diff --git a/src/rov/rov_led_controller/rov_led_controller/WS2812.py b/src/rov/rov_led_controller/rov_led_controller/WS2812.py index 4a0d366..762ef6d 100644 --- a/src/rov/rov_led_controller/rov_led_controller/WS2812.py +++ b/src/rov/rov_led_controller/rov_led_controller/WS2812.py @@ -35,7 +35,7 @@ def LED_show(self): WS = self._BytesToHex(Y) self.spi.xfer3(Y, 2400000,0,8) - def RGBto3Bytes(self, led_num, R, G, B): + def set_LED_color(self, led_num, R, G, B): if (R > 255 or G > 255 or B > 255): print("Invalid Value: RGB is over 255\n") sys.exit(1) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 468193b..e501903 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -5,10 +5,13 @@ import random import colorsys import threading -from WS2812 import SPItoWS +from rov_led_controller.WS2812 import SPItoWS +from rov_led_controller.SK6812RGBW import SPItoSK + NUM_LIGHTS = 60 -PIXELS = SPItoWS(NUM_LIGHTS) +RGB_PIXELS = SPItoWS(NUM_LIGHTS) +RGBW_PIXELS = SPItoSK(NUM_LIGHTS) class LEDControllerNode(Node): @@ -55,7 +58,8 @@ def __init__(self): # Stop flag self.stopAnim = False - PIXELS.LED_OFF_ALL() + RGB_PIXELS.LED_OFF_ALL() + RGBW_PIXELS.set_LED_color(40, 255, 255, 255, 255) def ui_request_callback(self, msg): @@ -68,7 +72,7 @@ def ui_request_callback(self, msg): try: # set and clamp brightness between 0 and 1 brightness = max(0.0, min(float(msgs[1]), 1.0)) - PIXELS.set_brightness(brightness * 0.5) + RGB_PIXELS.set_brightness(brightness * 0.5) self.get_logger().info("Changed brightness to: %f" % brightness) except: self.get_logger().info("Second argument, brightness, should be a float.") @@ -115,18 +119,18 @@ def color_chase(self, color, wait=1/NUM_LIGHTS): """ Animates a single color across the LED strip """ - PIXELS.LED_OFF_ALL() + RGB_PIXELS.LED_OFF_ALL() while self.stopAnim == False: for i in range(NUM_LIGHTS): - PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) + RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) time.sleep(wait) - PIXELS.LED_show() + RGB_PIXELS.LED_show() if self.stopAnim == True: break for i in range(NUM_LIGHTS): - PIXELS.RGBto3Bytes(i, 0, 0, 0) + RGB_PIXELS.set_LED_color(i, 0, 0, 0) time.sleep(wait) - PIXELS.LED_show() + RGB_PIXELS.LED_show() if self.stopAnim == True: break @@ -138,8 +142,8 @@ def rainbow_cycle(self, color, wait = 2 / NUM_LIGHTS): while self.stopAnim == False: for i in range(256): for j in range (NUM_LIGHTS): - PIXELS.RGBto3Bytes(j, self.rainbowValues[(j + i) % 256][0], self.rainbowValues[(j + i) % 256][1], self.rainbowValues[(j + i) % 256][2]) - PIXELS.LED_show() + RGB_PIXELS.set_LED_color(j, self.rainbowValues[(j + i) % 256][0], self.rainbowValues[(j + i) % 256][1], self.rainbowValues[(j + i) % 256][2]) + RGB_PIXELS.LED_show() time.sleep(wait) if self.stopAnim == True: break @@ -152,9 +156,9 @@ def pulse(self, color, wait=0.1): while self.stopAnim == False: for i in range(255): for j in range(NUM_LIGHTS): - PIXELS.RGBto3Bytes(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) + RGB_PIXELS.set_LED_color(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) time.sleep(wait) - PIXELS.LED_show() + RGB_PIXELS.LED_show() if self.stopAnim == True: break @@ -164,8 +168,8 @@ def solid(self, color, wait=0.1): Changes all LEDs to solid color """ for i in range (NUM_LIGHTS): - PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) - PIXELS.LED_show() + RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) + RGB_PIXELS.LED_show() def seizure_disco(self, color, wait=0.1): @@ -176,27 +180,27 @@ def seizure_disco(self, color, wait=0.1): R = random.randint(0,255) G = random.randint(0,255) B = random.randint(0,255) - PIXELS.RGBto3Bytes(i, R, G, B) - PIXELS.LED_show() + RGB_PIXELS.set_LED_color(i, R, G, B) + RGB_PIXELS.LED_show() time.sleep(wait) def off(self, color): - PIXELS.LED_OFF_ALL() + RGB_PIXELS.LED_OFF_ALL() def boot(self, color): """ Booting animation """ for i in range(NUM_LIGHTS): - PIXELS.RGBto3Bytes(i, 0, 255, 0) - PIXELS.LED_show() + RGB_PIXELS.set_LED_color(i, 0, 255, 0) + RGB_PIXELS.LED_show() time.sleep(1/NUM_LIGHTS) time.sleep(0.3) - PIXELS.LED_OFF_ALL() + RGB_PIXELS.LED_OFF_ALL() time.sleep(0.3) self._set_all_pixels(self.colors["GREEN"]) time.sleep(1) - PIXELS.LED_OFF_ALL() + RGB_PIXELS.LED_OFF_ALL() def _set_all_pixels(self, color): @@ -204,7 +208,7 @@ def _set_all_pixels(self, color): Sets all pixels to a single """ for i in range (NUM_LIGHTS): - PIXELS.RGBto3Bytes(i, color[0], color[1], color[2]) + RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) diff --git a/src/rov/rov_led_controller/setup.py b/src/rov/rov_led_controller/setup.py index ae9c8f3..0d652b1 100644 --- a/src/rov/rov_led_controller/setup.py +++ b/src/rov/rov_led_controller/setup.py @@ -10,8 +10,6 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - # add WS2812 - ('lib/' + package_name, [package_name+'/WS2812.py']) ], install_requires=['setuptools'], zip_safe=True, From 664c5d6fa31854ccecacad461fe67cb946278818 Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Wed, 23 Apr 2025 18:25:01 -0600 Subject: [PATCH 10/15] Used new library in led_controller node --- .../rov_led_controller/SK6812RGBW.py | 15 ++++- .../rov_led_controller/led_controller.py | 63 ++++++++++++++----- 2 files changed, 59 insertions(+), 19 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py index 497e2a9..ac60637 100644 --- a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py +++ b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py @@ -1,5 +1,6 @@ import spidev import sys +import time class SPItoSK(): def __init__(self, numLeds, bus = 1, device = 0): @@ -42,7 +43,8 @@ def LED_show(self): bytesToSend.insert(0, self.binMsg & ((1 << 8) - 1)) # Extract lowest 8 bits self.binMsg >>= 8 # Shift right by 8 bits self.spi.xfer(bytesToSend, delay_usec = 0, bits_per_word = 8) - + time.sleep(80e-6) + # debug bytesString = [] for i in bytesToSend: @@ -56,7 +58,16 @@ def set_brightness(self, brightness): """ brightness = max(0.0, min(brightness, 1.0)) self.ledBrightness = brightness - + + def LED_OFF_ALL(self): + """ + Turns off LEDs + """ + self.binMsg = 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 + for i in range(self.ledCount - 1): + # 96 bits per led, 3 bits per real bit + self.binMsg = (self.binMsg << 96) | 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 + self.LED_show() def _formatBinMsg(self, startPos, colorNum): number = format(colorNum, '08b') diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index e501903..f9b244c 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -9,9 +9,9 @@ from rov_led_controller.SK6812RGBW import SPItoSK -NUM_LIGHTS = 60 -RGB_PIXELS = SPItoWS(NUM_LIGHTS) -RGBW_PIXELS = SPItoSK(NUM_LIGHTS) +NUM_LEDS = 60 +RGB_PIXELS = SPItoWS(NUM_LEDS) +RGBW_PIXELS = SPItoSK(NUM_LEDS) class LEDControllerNode(Node): @@ -115,35 +115,44 @@ def ui_request_callback(self, msg): - def color_chase(self, color, wait=1/NUM_LIGHTS): + def color_chase(self, color, wait=1/NUM_LEDS): """ Animates a single color across the LED strip """ RGB_PIXELS.LED_OFF_ALL() while self.stopAnim == False: - for i in range(NUM_LIGHTS): + for i in range(NUM_LEDS): RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) + self._set_RGBW_LED(i, color[0], color[1], color[2]) time.sleep(wait) RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() if self.stopAnim == True: break - for i in range(NUM_LIGHTS): + for i in range(NUM_LEDS): RGB_PIXELS.set_LED_color(i, 0, 0, 0) + self._set_RGBW_LED(i, 0, 0, 0) time.sleep(wait) RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() if self.stopAnim == True: break - def rainbow_cycle(self, color, wait = 2 / NUM_LIGHTS): + def rainbow_cycle(self, color, wait = 2 / NUM_LEDS): """ Cycles a wave of rainbow over the LEDs """ while self.stopAnim == False: for i in range(256): - for j in range (NUM_LIGHTS): - RGB_PIXELS.set_LED_color(j, self.rainbowValues[(j + i) % 256][0], self.rainbowValues[(j + i) % 256][1], self.rainbowValues[(j + i) % 256][2]) + for j in range (NUM_LEDS): + R = self.rainbowValues[(j + i) % 256][0] + G = self.rainbowValues[(j + i) % 256][1] + B = self.rainbowValues[(j + i) % 256][2] + RGB_PIXELS.set_LED_color(j,R ,G ,B) + self._set_RGBW_LED(j, R, G, B) RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() time.sleep(wait) if self.stopAnim == True: break @@ -155,10 +164,15 @@ def pulse(self, color, wait=0.1): """ while self.stopAnim == False: for i in range(255): - for j in range(NUM_LIGHTS): - RGB_PIXELS.set_LED_color(j, color[0] * (i / 255.0), color[1] * (i / 255.0), color[2] * (i / 255.0)) + for j in range(NUM_LEDS): + R = color[0] * (i / 255.0) + G = color[1] * (i / 255.0) + B = color[2] * (i / 255.0) + RGB_PIXELS.set_LED_color(j ,R ,G ,B) + self._set_RGBW_LED(j, R, G,B) time.sleep(wait) RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() if self.stopAnim == True: break @@ -167,21 +181,23 @@ def solid(self, color, wait=0.1): """ Changes all LEDs to solid color """ - for i in range (NUM_LIGHTS): - RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) + self._set_all_pixels(color[0], color[1], color[2]) RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() def seizure_disco(self, color, wait=0.1): """ Changes all LEDs to a random color """ - for i in range(NUM_LIGHTS): + for i in range(NUM_LEDS): R = random.randint(0,255) G = random.randint(0,255) B = random.randint(0,255) RGB_PIXELS.set_LED_color(i, R, G, B) + self._set_RGBW_LED(i, R, G, B) RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() time.sleep(wait) def off(self, color): @@ -191,10 +207,10 @@ def boot(self, color): """ Booting animation """ - for i in range(NUM_LIGHTS): + for i in range(NUM_LEDS): RGB_PIXELS.set_LED_color(i, 0, 255, 0) RGB_PIXELS.LED_show() - time.sleep(1/NUM_LIGHTS) + time.sleep(1/NUM_LEDS) time.sleep(0.3) RGB_PIXELS.LED_OFF_ALL() time.sleep(0.3) @@ -207,9 +223,22 @@ def _set_all_pixels(self, color): """ Sets all pixels to a single """ - for i in range (NUM_LIGHTS): + for i in range (NUM_LEDS): RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) + self._set_RGBW_LED(i, color[0], color[1], color[2]) + + def _set_RGBW_LED(self, ledNum, R, G, B): + """ + Convert RGB value to RGBW value and set RGBW LEDs to new color + """ + newW = min(R, G, B) + RGBScale = 255/newW + newR = R * RGBScale + newG = G * RGBScale + newB = B * RGBScale + RGBW_PIXELS.set_LED_color(ledNum, newR, newG, newB, newW) + From ebe1cb4e678bbc25a35e1bd5d61235a4ea9b336f Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Thu, 24 Apr 2025 21:55:28 -0600 Subject: [PATCH 11/15] Testing needed, Fixed RGB to RGBW scaling, threading, pulse anim, solid anim, off anim --- .../rov_led_controller/SK6812RGBW.py | 11 +-- .../rov_led_controller/led_controller.py | 74 +++++++++++++++---- 2 files changed, 62 insertions(+), 23 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py index ac60637..65e7168 100644 --- a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py +++ b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py @@ -26,7 +26,7 @@ def set_LED_color(self, ledNum, R, G, B, W): :param R, G, B, W: RGBW values from 0-255 """ if (R > 255) or (G > 255) or (B > 255) or (W > 255) or (R < 0) or (G < 0) or (B < 0) or (W < 0): - print("RGBW values must be in the rage 0-255") + print("RGBW values must be in the range 0-255") sys.exit(-1) msgStartPos = (ledNum * 96) + 2 self._formatBinMsg(msgStartPos, int(round(R * self.ledBrightness))) @@ -34,6 +34,9 @@ def set_LED_color(self, ledNum, R, G, B, W): self._formatBinMsg(msgStartPos + (24 * 2), int(round(B * self.ledBrightness))) self._formatBinMsg(msgStartPos + (24 * 3), int(round(W * self.ledBrightness))) + # debug + print("led_num: %d, R: %d G: %d B: %d W: %d" % (ledNum, round(R * self.ledBrightness), round(G * self.ledBrightness), round(B * self.ledBrightness), round(W * self.ledBrightness))) + def LED_show(self): """ Signals the LEDs @@ -44,12 +47,6 @@ def LED_show(self): self.binMsg >>= 8 # Shift right by 8 bits self.spi.xfer(bytesToSend, delay_usec = 0, bits_per_word = 8) time.sleep(80e-6) - - # debug - bytesString = [] - for i in bytesToSend: - bytesString.append(bin(i)) - print(bytesString) def set_brightness(self, brightness): """ diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index f9b244c..14b4924 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -57,9 +57,10 @@ def __init__(self): # Stop flag self.stopAnim = False + self.stopLock = threading.Lock() RGB_PIXELS.LED_OFF_ALL() - RGBW_PIXELS.set_LED_color(40, 255, 255, 255, 255) + RGBW_PIXELS.LED_OFF_ALL() def ui_request_callback(self, msg): @@ -73,6 +74,7 @@ def ui_request_callback(self, msg): # set and clamp brightness between 0 and 1 brightness = max(0.0, min(float(msgs[1]), 1.0)) RGB_PIXELS.set_brightness(brightness * 0.5) + RGBW_PIXELS.set_brightness(brightness * 0.5) self.get_logger().info("Changed brightness to: %f" % brightness) except: self.get_logger().info("Second argument, brightness, should be a float.") @@ -93,7 +95,8 @@ def ui_request_callback(self, msg): # Check if the requested animation exists if msgs[0] in self.animations: # Stop current animation - self.stopAnim = True + with self.stopLock: + self.stopAnim = True while threading.active_count() > 1: time.sleep(0.1) self.stopAnim = False @@ -104,14 +107,16 @@ def ui_request_callback(self, msg): tempThread.start() elif msgs[0] in self.colors: # Stop current animation - self.stopAnim = True + with self.stopLock: + self.stopAnim = True while threading.active_count() > 1: time.sleep(0.1) self.stopAnim = False # Change the color if message changes colors self.ledColor = msgs[0] self.get_logger().info('New color received: %s' % msgs[0]) - self.animations[self.currentAnimation](self.colors[msgs[0]]) + tempThread = threading.Thread(target=self.animations[self.currentAnimation], args=(self.colors[msgs[0]],)) + tempThread.start() @@ -158,18 +163,26 @@ def rainbow_cycle(self, color, wait = 2 / NUM_LEDS): break - def pulse(self, color, wait=0.1): + def pulse(self, color, wait=0.005): """ Fades a single color in and out """ while self.stopAnim == False: for i in range(255): - for j in range(NUM_LEDS): - R = color[0] * (i / 255.0) - G = color[1] * (i / 255.0) - B = color[2] * (i / 255.0) - RGB_PIXELS.set_LED_color(j ,R ,G ,B) - self._set_RGBW_LED(j, R, G,B) + R = color[0] * (i / 255.0) + G = color[1] * (i / 255.0) + B = color[2] * (i / 255.0) + self._set_all_pixels((R, G, B)) + time.sleep(wait) + RGB_PIXELS.LED_show() + RGBW_PIXELS.LED_show() + if self.stopAnim == True: + break + for i in range(255): + R = color[0] * ((255 - i) / 255.0) + G = color[1] * ((255 - i) / 255.0) + B = color[2] * ((255 - i) / 255.0) + self._set_all_pixels((R, G, B)) time.sleep(wait) RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() @@ -181,7 +194,7 @@ def solid(self, color, wait=0.1): """ Changes all LEDs to solid color """ - self._set_all_pixels(color[0], color[1], color[2]) + self._set_all_pixels((color[0], color[1], color[2])) RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() @@ -202,6 +215,7 @@ def seizure_disco(self, color, wait=0.1): def off(self, color): RGB_PIXELS.LED_OFF_ALL() + RGBW_PIXELS.LED_OFF_ALL() def boot(self, color): """ @@ -232,11 +246,39 @@ def _set_RGBW_LED(self, ledNum, R, G, B): """ Convert RGB value to RGBW value and set RGBW LEDs to new color """ + # percentage of RGB in old colorspace + RGBTotal = R + G + B + if RGBTotal == 0: + RGBW_PIXELS.set_LED_color(ledNum, 0, 0, 0, 0) + return + percentR = R/RGBTotal + percentG = G/RGBTotal + percentB = B/RGBTotal + # set white value as lowest RGB value newW = min(R, G, B) - RGBScale = 255/newW - newR = R * RGBScale - newG = G * RGBScale - newB = B * RGBScale + # To preserve max brightness, set new highest RGB value to corresponding RGBW value + # Then scale the value of the 2 other RGB values to preserve original color percentage + newR = 0 + newG = 0 + newB = 0 + if max(R, G, B) == R: + newR = R + RTotal = newR + newW + RGBWTotal = RTotal / percentR + newG = (RGBWTotal * percentG) - newW + newB = (RGBWTotal * percentB) - newW + elif max(R, G, B) == G: + newG = G + GTotal = newG + newW + RGBWTotal = GTotal / percentG + newR = (RGBWTotal * percentR) - newW + newB = (RGBWTotal * percentB) - newW + elif max(R, G, B) == B: + newB = B + BTotal = newB + newW + RGBWTotal = BTotal / percentB + newR = (RGBWTotal * percentR) - newW + newG = (RGBWTotal * percentG) - newW RGBW_PIXELS.set_LED_color(ledNum, newR, newG, newB, newW) From baa6d83521851af75bebe4541837a4261256b670 Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Sat, 26 Apr 2025 13:35:16 -0600 Subject: [PATCH 12/15] Fixed timing --- .../rov_led_controller/SK6812RGBW.py | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py index 65e7168..6bc150f 100644 --- a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py +++ b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py @@ -8,7 +8,7 @@ def __init__(self, numLeds, bus = 1, device = 0): self.ledBrightness = 1.0 self.spi = spidev.SpiDev() self.spi.open(bus, device) - self.spi.max_speed_hz = 2400000 + self.spi.max_speed_hz = 2500000 # string of bits to send to spi self.binMsg = 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 for i in range(numLeds - 1): @@ -35,17 +35,27 @@ def set_LED_color(self, ledNum, R, G, B, W): self._formatBinMsg(msgStartPos + (24 * 3), int(round(W * self.ledBrightness))) # debug - print("led_num: %d, R: %d G: %d B: %d W: %d" % (ledNum, round(R * self.ledBrightness), round(G * self.ledBrightness), round(B * self.ledBrightness), round(W * self.ledBrightness))) + # print("led_num: %d, R: %d G: %d B: %d W: %d" % (ledNum, round(R * self.ledBrightness), round(G * self.ledBrightness), round(B * self.ledBrightness), round(W * self.ledBrightness))) def LED_show(self): """ Signals the LEDs """ + print(bin(self.binMsg)) bytesToSend = [] - while self.binMsg: - bytesToSend.insert(0, self.binMsg & ((1 << 8) - 1)) # Extract lowest 8 bits - self.binMsg >>= 8 # Shift right by 8 bits - self.spi.xfer(bytesToSend, delay_usec = 0, bits_per_word = 8) + binMsgCpy = self.binMsg + while binMsgCpy: + bytesToSend.insert(0, binMsgCpy & ((1 << 8) - 1)) # Extract lowest 8 bits + binMsgCpy >>= 8 # Shift right by 8 bits + + # self.spi.xfer3([0b10010010, 0b01001001, 0b00100100, 0b10010010, 0b01001001, 0b00100100, 0b10010010, 0b01001001, 0b00100100, 0b10010010, 0b01001001, 0b00100100], 2400000, 0, 8) + self.spi.xfer3(bytesToSend, 2500000, 0, 8) + + # debug + offString = 100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 + print() + for i in bytesToSend: + print(bin(i)) time.sleep(80e-6) def set_brightness(self, brightness): @@ -72,7 +82,7 @@ def _formatBinMsg(self, startPos, colorNum): msgIndexPos = startPos + (3 * i) colorBit = number[i] if (colorBit == "0") and bin(self.binMsg)[msgIndexPos:msgIndexPos + 3] == "110": - self._flipBit(msgIndexPos + 1) + self._flipBit(msgIndexPos) elif (colorBit == "1") and bin(self.binMsg)[msgIndexPos:msgIndexPos + 3] == "100": self._flipBit(msgIndexPos) From 0cd033b2244d5ffb333bacca6404325d9c49ac75 Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Tue, 29 Apr 2025 18:09:22 -0600 Subject: [PATCH 13/15] Fixed color of new LEDs --- .../rov_led_controller/SK6812RGBW.py | 21 +++++++++-------- .../rov_led_controller/led_controller.py | 23 ++----------------- 2 files changed, 14 insertions(+), 30 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py index 6bc150f..6c67905 100644 --- a/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py +++ b/src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py @@ -29,9 +29,13 @@ def set_LED_color(self, ledNum, R, G, B, W): print("RGBW values must be in the range 0-255") sys.exit(-1) msgStartPos = (ledNum * 96) + 2 - self._formatBinMsg(msgStartPos, int(round(R * self.ledBrightness))) - self._formatBinMsg(msgStartPos + 24, int(round(G * self.ledBrightness))) + # Set G in binary message + self._formatBinMsg(msgStartPos, int(round(G * self.ledBrightness))) + # Set R in binary message + self._formatBinMsg(msgStartPos + 24, int(round(R * self.ledBrightness))) + # Set B in binary message self._formatBinMsg(msgStartPos + (24 * 2), int(round(B * self.ledBrightness))) + # Set W in binary message self._formatBinMsg(msgStartPos + (24 * 3), int(round(W * self.ledBrightness))) # debug @@ -41,22 +45,21 @@ def LED_show(self): """ Signals the LEDs """ - print(bin(self.binMsg)) bytesToSend = [] binMsgCpy = self.binMsg while binMsgCpy: bytesToSend.insert(0, binMsgCpy & ((1 << 8) - 1)) # Extract lowest 8 bits binMsgCpy >>= 8 # Shift right by 8 bits - # self.spi.xfer3([0b10010010, 0b01001001, 0b00100100, 0b10010010, 0b01001001, 0b00100100, 0b10010010, 0b01001001, 0b00100100, 0b10010010, 0b01001001, 0b00100100], 2400000, 0, 8) self.spi.xfer3(bytesToSend, 2500000, 0, 8) # debug - offString = 100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 - print() - for i in bytesToSend: - print(bin(i)) - time.sleep(80e-6) + # print(bin(self.binMsg)) + # offString = 100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100 + # print() + # for i in bytesToSend: + # print(bin(i)) + # time.sleep(80e-6) def set_brightness(self, brightness): """ diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 14b4924..0245ac0 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -10,7 +10,6 @@ NUM_LEDS = 60 -RGB_PIXELS = SPItoWS(NUM_LEDS) RGBW_PIXELS = SPItoSK(NUM_LEDS) class LEDControllerNode(Node): @@ -59,7 +58,6 @@ def __init__(self): self.stopAnim = False self.stopLock = threading.Lock() - RGB_PIXELS.LED_OFF_ALL() RGBW_PIXELS.LED_OFF_ALL() @@ -73,7 +71,6 @@ def ui_request_callback(self, msg): try: # set and clamp brightness between 0 and 1 brightness = max(0.0, min(float(msgs[1]), 1.0)) - RGB_PIXELS.set_brightness(brightness * 0.5) RGBW_PIXELS.set_brightness(brightness * 0.5) self.get_logger().info("Changed brightness to: %f" % brightness) except: @@ -124,21 +121,16 @@ def color_chase(self, color, wait=1/NUM_LEDS): """ Animates a single color across the LED strip """ - RGB_PIXELS.LED_OFF_ALL() while self.stopAnim == False: for i in range(NUM_LEDS): - RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) self._set_RGBW_LED(i, color[0], color[1], color[2]) time.sleep(wait) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() if self.stopAnim == True: break for i in range(NUM_LEDS): - RGB_PIXELS.set_LED_color(i, 0, 0, 0) self._set_RGBW_LED(i, 0, 0, 0) time.sleep(wait) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() if self.stopAnim == True: break @@ -154,9 +146,7 @@ def rainbow_cycle(self, color, wait = 2 / NUM_LEDS): R = self.rainbowValues[(j + i) % 256][0] G = self.rainbowValues[(j + i) % 256][1] B = self.rainbowValues[(j + i) % 256][2] - RGB_PIXELS.set_LED_color(j,R ,G ,B) self._set_RGBW_LED(j, R, G, B) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() time.sleep(wait) if self.stopAnim == True: @@ -174,7 +164,6 @@ def pulse(self, color, wait=0.005): B = color[2] * (i / 255.0) self._set_all_pixels((R, G, B)) time.sleep(wait) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() if self.stopAnim == True: break @@ -184,7 +173,6 @@ def pulse(self, color, wait=0.005): B = color[2] * ((255 - i) / 255.0) self._set_all_pixels((R, G, B)) time.sleep(wait) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() if self.stopAnim == True: break @@ -195,7 +183,6 @@ def solid(self, color, wait=0.1): Changes all LEDs to solid color """ self._set_all_pixels((color[0], color[1], color[2])) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() @@ -207,14 +194,11 @@ def seizure_disco(self, color, wait=0.1): R = random.randint(0,255) G = random.randint(0,255) B = random.randint(0,255) - RGB_PIXELS.set_LED_color(i, R, G, B) self._set_RGBW_LED(i, R, G, B) - RGB_PIXELS.LED_show() RGBW_PIXELS.LED_show() time.sleep(wait) def off(self, color): - RGB_PIXELS.LED_OFF_ALL() RGBW_PIXELS.LED_OFF_ALL() def boot(self, color): @@ -222,15 +206,13 @@ def boot(self, color): Booting animation """ for i in range(NUM_LEDS): - RGB_PIXELS.set_LED_color(i, 0, 255, 0) - RGB_PIXELS.LED_show() + self._set_RGBW_LED(i, 0, 255, 0) + RGBW_PIXELS.LED_show() time.sleep(1/NUM_LEDS) time.sleep(0.3) - RGB_PIXELS.LED_OFF_ALL() time.sleep(0.3) self._set_all_pixels(self.colors["GREEN"]) time.sleep(1) - RGB_PIXELS.LED_OFF_ALL() def _set_all_pixels(self, color): @@ -238,7 +220,6 @@ def _set_all_pixels(self, color): Sets all pixels to a single """ for i in range (NUM_LEDS): - RGB_PIXELS.set_LED_color(i, color[0], color[1], color[2]) self._set_RGBW_LED(i, color[0], color[1], color[2]) From 707f6f6f6471ebceb16590c1e5a9cffcfd34bc2b Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Thu, 1 May 2025 00:01:10 -0600 Subject: [PATCH 14/15] Added Ring and Strip support --- .../rov_led_controller/led_controller.py | 256 ++++++++++++------ 1 file changed, 175 insertions(+), 81 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 0245ac0..593af74 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -5,18 +5,18 @@ import random import colorsys import threading -from rov_led_controller.WS2812 import SPItoWS from rov_led_controller.SK6812RGBW import SPItoSK -NUM_LEDS = 60 -RGBW_PIXELS = SPItoSK(NUM_LEDS) +# Ring leds go first, then strip leds +NUM_RING_LEDS = 12 +NUM_STRIP_LEDS = 48 +RGBW_PIXELS = SPItoSK(NUM_RING_LEDS + NUM_STRIP_LEDS) class LEDControllerNode(Node): def __init__(self): super().__init__(node_name="LEDController") - self.publisher_ = self.create_publisher(String, 'led_controller_output', 10) self.animations = { "color_chase": self.color_chase, @@ -48,124 +48,189 @@ def __init__(self): self.rainbowValues.append(scaled_rgb) # color value and current animation - self.ledColor = "WHITE" - self.currentAnimation = "solid" + self.ringColor = "WHITE" + self.ringCurrAnim = "off" + self.stripColor = "WHITE" + self.stripCurrAnim = "off" # Listen for requests from the UI self.ui_subscriber = self.create_subscription(String, 'ui_requests', self.ui_request_callback, 10) - # Stop flag - self.stopAnim = False - self.stopLock = threading.Lock() + # SK6812 Mutex + self.LEDMutex = threading.Lock() - RGBW_PIXELS.LED_OFF_ALL() + # Thread management + # Can't believe I need to wrap StopAnim in a list to pass by reference + self.ringStopAnim = [False] + self.stripStopAnim = [False] + self.ringStopMutex = threading.Lock() + self.stripStopMutex = threading.Lock() + # Set leds to off when starting + self.ringAnimThread = threading.Thread(target=self.animations[self.ringCurrAnim], args=(self.colors[self.ringColor], self._set_ring_LED)) + self.stripAnimThread = threading.Thread(target=self.animations[self.stripCurrAnim], args=(self.colors[self.stripColor], self._set_strip_LED)) + self.ringAnimThread.start() + self.stripAnimThread.start() def ui_request_callback(self, msg): # Split message split = msg.data.split(",") msgs = [s.strip() for s in split] - + # Change brightness - if len(msgs) == 2: + if len(msgs) == 3: try: # set and clamp brightness between 0 and 1 - brightness = max(0.0, min(float(msgs[1]), 1.0)) - RGBW_PIXELS.set_brightness(brightness * 0.5) + brightness = max(0.0, min(float(msgs[2]), 1.0)) + RGBW_PIXELS.set_brightness(brightness) self.get_logger().info("Changed brightness to: %f" % brightness) except: - self.get_logger().info("Second argument, brightness, should be a float.") + self.get_logger().info("Third argument, brightness, should be a number from 0-1.") + else: + RGBW_PIXELS.set_brightness(0.25) # Change animation and colors if msgs[0] == 'get_animations': # Send the list of preprogrammed animations to the UI animations_msg = String() animations_msg.data = "\n".join(animation for animation in self.animations) - self.publisher_.publish(animations_msg) - self.get_logger().info('Sent preprogrammed animations to UI') + self.get_logger().info(animations_msg) elif msgs[0] == 'get_colors': colors_msg = String() colors_msg.data = "\n".join(color for color in self.colors) - self.publisher_.publish(colors_msg) - self.get_logger().info('Sent preprogrammed colors to UI') - else: + self.get_logger().info(colors_msg) + elif len(msgs) >= 2: + # Check for which leds the user wants to modify + setStripFunc = self._set_ring_LED + if msgs[0] == "strip": + setStripFunc = self._set_strip_LED + elif msgs[0] != "ring": + self.get_logger().info("First argument must be \"ring\" or \"strip\", defaulting to ring") + msgs[0] = "ring" # Check if the requested animation exists - if msgs[0] in self.animations: + if msgs[1] in self.animations: # Stop current animation - with self.stopLock: - self.stopAnim = True - while threading.active_count() > 1: - time.sleep(0.1) - self.stopAnim = False + if msgs[0] == "ring": + with self.ringStopMutex: + self.ringStopAnim[0] = True + self.ringAnimThread.join() + self.ringStopAnim[0] = False + elif msgs[0] == "strip": + with self.stripStopMutex: + self.stripStopAnim[0] = True + self.stripAnimThread.join() + self.stripStopAnim[0] = False # Call the method corresponding to the requested animation - self.currentAnimation = msgs[0] - self.get_logger().info('New animation received: %s' % msgs[0]) - tempThread = threading.Thread(target=self.animations[msgs[0]], args=(self.colors[self.ledColor],)) - tempThread.start() - elif msgs[0] in self.colors: + if msgs[0] == "ring": + self.ringCurrAnim = msgs[1] + self.get_logger().info('New ring animation received: %s' % msgs[1]) + self.ringAnimThread = threading.Thread(target=self.animations[msgs[1]], args=(self.colors[self.ringColor], setStripFunc)) + self.ringAnimThread.start() + elif msgs[0] == "strip": + self.stripCurrAnim = msgs[1] + self.get_logger().info('New strip animation received: %s' % msgs[1]) + self.stripAnimThread = threading.Thread(target=self.animations[msgs[1]], args=(self.colors[self.stripColor], setStripFunc)) + self.stripAnimThread.start() + elif msgs[1] in self.colors: # Stop current animation - with self.stopLock: - self.stopAnim = True - while threading.active_count() > 1: - time.sleep(0.1) - self.stopAnim = False + if msgs[0] == "ring": + with self.ringStopMutex: + self.ringStopAnim[0] = True + self.ringAnimThread.join() + self.ringStopAnim[0] = False + elif msgs[0] == "strip": + with self.stripStopMutex: + self.stripStopAnim[0] = True + self.stripAnimThread.join() + self.stripStopAnim[0] = False # Change the color if message changes colors - self.ledColor = msgs[0] - self.get_logger().info('New color received: %s' % msgs[0]) - tempThread = threading.Thread(target=self.animations[self.currentAnimation], args=(self.colors[msgs[0]],)) - tempThread.start() + if msgs[0] == "ring": + self.ringColor = msgs[1] + self.get_logger().info('New ring color received: %s' % msgs[1]) + self.ringAnimThread = threading.Thread(target=self.animations[self.ringCurrAnim], args=(self.colors[self.ringColor], setStripFunc)) + self.ringAnimThread.start() + elif msgs[0] == "strip": + self.stripColor = msgs[1] + self.get_logger().info('New strip color received: %s' % msgs[1]) + self.stripAnimThread = threading.Thread(target=self.animations[self.stripCurrAnim], args=(self.colors[self.stripColor], setStripFunc)) + self.stripAnimThread.start() + else: + self.get_logger().info("Second argument must be an animation or color. Send \"get_animations\" or \"get_colors\" to view options.") + else: + self.get_logger().info("Request most be formatted: \"ring\"/\"strip\", animation/color, [brightness]. Send \"get_animations\" or \"get_colors\" to view options.") - def color_chase(self, color, wait=1/NUM_LEDS): + def color_chase(self, color, setStripFunc): """ Animates a single color across the LED strip """ - while self.stopAnim == False: - for i in range(NUM_LEDS): - self._set_RGBW_LED(i, color[0], color[1], color[2]) + numLEDs = 0 + if setStripFunc == self._set_ring_LED: + numLEDs = NUM_RING_LEDS + stopAnim = self.ringStopAnim + else: + numLEDs = NUM_STRIP_LEDS + stopAnim = self.stripStopAnim + wait = 1/numLEDs + while stopAnim == [False]: + for i in range(numLEDs): + setStripFunc(i, color[0], color[1], color[2]) time.sleep(wait) - RGBW_PIXELS.LED_show() - if self.stopAnim == True: + self._LED_show() + if stopAnim == [True]: break - for i in range(NUM_LEDS): - self._set_RGBW_LED(i, 0, 0, 0) + for i in range(numLEDs): + setStripFunc(i, 0, 0, 0) time.sleep(wait) - RGBW_PIXELS.LED_show() - if self.stopAnim == True: + self._LED_show() + if stopAnim == [True]: break - def rainbow_cycle(self, color, wait = 2 / NUM_LEDS): + def rainbow_cycle(self, color, setStripFunc): """ Cycles a wave of rainbow over the LEDs """ - while self.stopAnim == False: + numLEDs = 0 + if setStripFunc == self._set_ring_LED: + numLEDs = NUM_RING_LEDS + stopAnim = self.ringStopAnim + else: + numLEDs = NUM_STRIP_LEDS + stopAnim = self.stripStopAnim + wait = 0.05 + while stopAnim == [False]: for i in range(256): - for j in range (NUM_LEDS): + for j in range (numLEDs): R = self.rainbowValues[(j + i) % 256][0] G = self.rainbowValues[(j + i) % 256][1] B = self.rainbowValues[(j + i) % 256][2] - self._set_RGBW_LED(j, R, G, B) - RGBW_PIXELS.LED_show() + setStripFunc(j, R, G, B) + self._LED_show() time.sleep(wait) - if self.stopAnim == True: + if stopAnim == [True]: break - def pulse(self, color, wait=0.005): + def pulse(self, color, setStripFunc): """ Fades a single color in and out """ - while self.stopAnim == False: + if setStripFunc == self._set_ring_LED: + stopAnim = self.ringStopAnim + else: + stopAnim = self.stripStopAnim + wait = 0.005 + while stopAnim == [False]: for i in range(255): R = color[0] * (i / 255.0) G = color[1] * (i / 255.0) B = color[2] * (i / 255.0) self._set_all_pixels((R, G, B)) time.sleep(wait) - RGBW_PIXELS.LED_show() - if self.stopAnim == True: + self._LED_show() + if stopAnim == [True]: break for i in range(255): R = color[0] * ((255 - i) / 255.0) @@ -173,55 +238,83 @@ def pulse(self, color, wait=0.005): B = color[2] * ((255 - i) / 255.0) self._set_all_pixels((R, G, B)) time.sleep(wait) - RGBW_PIXELS.LED_show() - if self.stopAnim == True: + self._LED_show() + if stopAnim == [True]: break - def solid(self, color, wait=0.1): + def solid(self, color, setStripFunc): """ Changes all LEDs to solid color """ - self._set_all_pixels((color[0], color[1], color[2])) - RGBW_PIXELS.LED_show() + self._set_all_pixels((color[0], color[1], color[2]), setStripFunc) + self._LED_show() - def seizure_disco(self, color, wait=0.1): + def seizure_disco(self, color, setStripFunc): """ Changes all LEDs to a random color """ - for i in range(NUM_LEDS): + if setStripFunc == self._set_ring_LED: + numLEDs = NUM_RING_LEDS + else: + numLEDs = NUM_STRIP_LEDS + for i in range(numLEDs): R = random.randint(0,255) G = random.randint(0,255) B = random.randint(0,255) - self._set_RGBW_LED(i, R, G, B) - RGBW_PIXELS.LED_show() - time.sleep(wait) + setStripFunc(i, R, G, B) + self._LED_show() - def off(self, color): - RGBW_PIXELS.LED_OFF_ALL() + def off(self, color, setStripFunc): + if setStripFunc == self._set_ring_LED: + numLEDs = NUM_RING_LEDS + else: + numLEDs = NUM_STRIP_LEDS + for i in range(numLEDs): + setStripFunc(i, 0, 0, 0) + self._LED_show() - def boot(self, color): + def boot(self, color, setStripFunc): """ Booting animation """ - for i in range(NUM_LEDS): - self._set_RGBW_LED(i, 0, 255, 0) - RGBW_PIXELS.LED_show() - time.sleep(1/NUM_LEDS) + if setStripFunc == self._set_ring_LED: + numLEDs = NUM_RING_LEDS + else: + numLEDs = NUM_STRIP_LEDS + for i in range(numLEDs): + setStripFunc(i, 0, 255, 0) + self._LED_show() + time.sleep(1/numLEDs) time.sleep(0.3) time.sleep(0.3) self._set_all_pixels(self.colors["GREEN"]) time.sleep(1) - def _set_all_pixels(self, color): + def _set_all_pixels(self, color, setStripFunc): """ Sets all pixels to a single """ - for i in range (NUM_LEDS): - self._set_RGBW_LED(i, color[0], color[1], color[2]) + if setStripFunc == self._set_ring_LED: + for i in range (NUM_RING_LEDS): + setStripFunc(i, color[0], color[1], color[2]) + elif setStripFunc == self._set_strip_LED: + for i in range (NUM_STRIP_LEDS): + setStripFunc(i, color[0], color[1], color[2]) + def _set_ring_LED(self, ledNum, R, G, B): + if ledNum <= NUM_RING_LEDS - 1 and ledNum >= 0: + self._set_RGBW_LED(ledNum, R, G, B) + + def _set_strip_LED(self, ledNum, R, G, B): + if ledNum <= NUM_STRIP_LEDS - 1 and ledNum >= 0: + self._set_RGBW_LED(NUM_RING_LEDS + ledNum, R, G, B) + + def _LED_show(self): + with self.LEDMutex: + RGBW_PIXELS.LED_show() def _set_RGBW_LED(self, ledNum, R, G, B): """ @@ -260,7 +353,8 @@ def _set_RGBW_LED(self, ledNum, R, G, B): RGBWTotal = BTotal / percentB newR = (RGBWTotal * percentR) - newW newG = (RGBWTotal * percentG) - newW - RGBW_PIXELS.set_LED_color(ledNum, newR, newG, newB, newW) + with self.LEDMutex: + RGBW_PIXELS.set_LED_color(ledNum, newR, newG, newB, newW) From 4a5a302bc356eafae5bda5901a61b1dc700fa088 Mon Sep 17 00:00:00 2001 From: Kadin Le Date: Fri, 2 May 2025 00:00:28 -0600 Subject: [PATCH 15/15] Corrected number of LEDs in ring --- .../rov_led_controller/led_controller.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/rov/rov_led_controller/rov_led_controller/led_controller.py b/src/rov/rov_led_controller/rov_led_controller/led_controller.py index 593af74..4533f57 100644 --- a/src/rov/rov_led_controller/rov_led_controller/led_controller.py +++ b/src/rov/rov_led_controller/rov_led_controller/led_controller.py @@ -9,7 +9,7 @@ # Ring leds go first, then strip leds -NUM_RING_LEDS = 12 +NUM_RING_LEDS = 24 NUM_STRIP_LEDS = 48 RGBW_PIXELS = SPItoSK(NUM_RING_LEDS + NUM_STRIP_LEDS) @@ -63,7 +63,6 @@ def __init__(self): # Can't believe I need to wrap StopAnim in a list to pass by reference self.ringStopAnim = [False] self.stripStopAnim = [False] - self.ringStopMutex = threading.Lock() self.stripStopMutex = threading.Lock() # Set leds to off when starting @@ -111,8 +110,7 @@ def ui_request_callback(self, msg): if msgs[1] in self.animations: # Stop current animation if msgs[0] == "ring": - with self.ringStopMutex: - self.ringStopAnim[0] = True + self.ringStopAnim[0] = True self.ringAnimThread.join() self.ringStopAnim[0] = False elif msgs[0] == "strip": @@ -134,8 +132,7 @@ def ui_request_callback(self, msg): elif msgs[1] in self.colors: # Stop current animation if msgs[0] == "ring": - with self.ringStopMutex: - self.ringStopAnim[0] = True + self.ringStopAnim[0] = True self.ringAnimThread.join() self.ringStopAnim[0] = False elif msgs[0] == "strip":