From b40402ce6219d9cbcc75fd8ee633544e6a03dd98 Mon Sep 17 00:00:00 2001
From: Achllle <achille.verheye@gmail.com>
Date: Mon, 26 May 2025 20:05:31 -0700
Subject: [PATCH] Add support for PWM LEDs

---
 ROS/osr_control/osr_control/led_control.py | 68 ++++++++++++++++++++++
 ROS/osr_control/setup.py                   |  3 +-
 2 files changed, 70 insertions(+), 1 deletion(-)
 create mode 100644 ROS/osr_control/osr_control/led_control.py

diff --git a/ROS/osr_control/osr_control/led_control.py b/ROS/osr_control/osr_control/led_control.py
new file mode 100644
index 00000000..7892a64e
--- /dev/null
+++ b/ROS/osr_control/osr_control/led_control.py
@@ -0,0 +1,68 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from std_msgs.msg import UInt8MultiArray
+from adafruit_servokit import ServoKit
+
+
+class LEDControlNode(Node):
+    """
+    Node to control the intensity of two LEDs connected to channels 4 and 5 of the PCA9685.
+    The intensity is controlled by publishing two percentages (0-100) to the topic '/led_intensity'.
+    """
+
+    def __init__(self):
+        super().__init__('led_control_node')
+        self.log = self.get_logger()
+        self.log.info("Initializing LED Control Node")
+
+        # Initialize I2C and PCA9685
+        self.kit = ServoKit(channels=16)
+
+        # LED channels
+        self.led_channels = [4, 5]
+
+        # Create subscription to control LED intensity
+        qos_profile = QoSProfile(depth=10)
+        self.subscription = self.create_subscription(
+            UInt8MultiArray,
+            '/led_intensity',
+            self.led_intensity_callback,
+            qos_profile
+        )
+
+    def led_intensity_callback(self, msg: UInt8MultiArray):
+        """
+        Callback to set the intensity of the LEDs based on the received message.
+        The message should contain two float values (0-100) representing the intensity percentages.
+        """
+        if len(msg.data) != 2:
+            self.log.error("Received message does not contain exactly two values. Ignoring.")
+            return
+
+        for i, intensity in enumerate(msg.data):
+            if not intensity <= 100:
+                self.log.error(f"Invalid intensity value {intensity}. Must be between 0 and 100.")
+                return
+
+            # Convert percentage to an angle
+            angle = int((intensity / 100.0) * 180)
+            self.kit.servo[self.led_channels[i]].angle = angle
+            self.log.debug(f"Set LED on channel {self.led_channels[i]} to {intensity}% intensity.")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LEDControlNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/ROS/osr_control/setup.py b/ROS/osr_control/setup.py
index 0676f196..3d1bca04 100644
--- a/ROS/osr_control/setup.py
+++ b/ROS/osr_control/setup.py
@@ -28,7 +28,8 @@
             'roboclaw_wrapper = osr_control.roboclaw_wrapper:main',
             'servo_control = osr_control.servo_control:main',
             'ina260 = osr_control.ina_260_pub:main',
-            'joy_extras = osr_control.joy_extras:main'
+            'joy_extras = osr_control.joy_extras:main',
+            'led_control = osr_control.led_control:main',
         ],
     },
 )