diff --git a/DOCUMENTATION.md b/DOCUMENTATION.md index d262793..831e459 100644 --- a/DOCUMENTATION.md +++ b/DOCUMENTATION.md @@ -18,6 +18,7 @@ See [readme.md](readme.md) for project overview and status. | `blockly_executor` — file reference, handlers & testing guide | [src/blockly_executor/README.md](src/blockly_executor/README.md) | | `blockly_interfaces` — ROS2 action & message interfaces | [src/blockly_interfaces/README.md](src/blockly_interfaces/README.md) | | `gpio_node` — Raspberry Pi GPIO node (C++, libgpiod) | [src/gpio_node/](src/gpio_node/) | +| `pca9685_node` — PCA9685 16-channel PWM controller (C++, I2C) | [src/pca9685_node/](src/pca9685_node/) | --- @@ -40,10 +41,12 @@ pixi run app # Terminal 2 — start desktop GUI ```bash pixi install # install ROS2 + deps via conda pixi run build-gpio # installs system deps (apt) + builds gpio_node +pixi run build-pca9685 # installs system deps (apt) + builds pca9685_node pixi run gpio-node # start GPIO node +pixi run pca9685-node # start PCA9685 PWM node ``` -`build-gpio` automatically runs `setup-dep` which installs system libraries (`libgpiod-dev`, `liblttng-ust-dev`) via `apt`. +`build-gpio` and `build-pca9685` automatically run `setup-dep` which installs system libraries (`libgpiod-dev`, `liblttng-ust-dev`, `i2c-tools`) via `apt`. See [docs/installation.md](docs/installation.md) for full setup and prerequisites. diff --git a/pixi.toml b/pixi.toml index 438cea5..36b7628 100644 --- a/pixi.toml +++ b/pixi.toml @@ -46,6 +46,8 @@ test = { cmd = "bash -c 'source install/setup.bash && PYTHONPATH=$PY setup-ui = "npm install blockly && mkdir -p src/blockly_app/blockly_app/ui/vendor && cp node_modules/blockly/blockly.min.js src/blockly_app/blockly_app/ui/vendor/ && cp node_modules/blockly/javascript_compressed.js src/blockly_app/blockly_app/ui/vendor/ && cp node_modules/blockly/blocks_compressed.js src/blockly_app/blockly_app/ui/vendor/ && cp node_modules/blockly/msg/en.js src/blockly_app/blockly_app/ui/vendor/" [target.linux-aarch64.tasks] -setup-dep = { cmd = "sudo apt update && sudo apt install -y liblttng-ust-dev lttng-tools libgpiod-dev gpiod"} +setup-dep = { cmd = "sudo apt update && sudo apt install -y liblttng-ust-dev lttng-tools libgpiod-dev gpiod i2c-tools"} build-gpio = { cmd = "colcon build --symlink-install --packages-select gpio_node", depends-on = ["setup-dep", "build-interfaces"] } -gpio-node = { cmd = "bash -c 'source install/setup.bash && ros2 run gpio_node gpio_node'", depends-on = ["build-gpio"] } \ No newline at end of file +gpio-node = { cmd = "bash -c 'source install/setup.bash && ros2 run gpio_node gpio_node'", depends-on = ["build-gpio"] } +build-pca9685 = { cmd = "colcon build --symlink-install --packages-select pca9685_node", depends-on = ["setup-dep", "build-interfaces"] } +pca9685-node = { cmd = "bash -c 'source install/setup.bash && ros2 run pca9685_node pca9685_node'", depends-on = ["build-pca9685"] } \ No newline at end of file diff --git a/readme.md b/readme.md index b00843e..3e4168f 100644 --- a/readme.md +++ b/readme.md @@ -3,7 +3,7 @@ > **Project**: Blockly ROS2 Robot Controller (Kiwi Wheel AMR) > **ROS2 Distro**: Jazzy > **Last Updated**: 2026-03-16 -> **Current Focus**: +> **Current Focus**: Task #3 — PCA9685 PWM Controller (I2C) Dokumentasi lengkap dapat dilihat di [DOCUMENTATION.md](DOCUMENTATION.md). @@ -36,6 +36,8 @@ jelaskan apa yang dimaksut untuk menyelesaikan task # Potential Enhancements this list is short by priority +- **AS5600 feature**: AS5600 magnetic based encoder with I2C interface. lets make publisher node in C/C++ to be able pusblih 3 module encoder. use parameter ins ros2 command to chose linux dev interface. +- **Feasibility Study to implement Adaptive Controller**: mobile robot need controller to move flawlesly. - **Launch files**: `blockly_bringup` package with ROS2 launch files to start all nodes with one command - **Simulation**: Integrate with Gazebo/Isaac Sim for testing Kiwi Wheel kinematics before deploying to hardware - **Block categories**: Future blocks grouped into Robot, Sensors, Navigation categories @@ -123,3 +125,96 @@ pixi run gpio-node - [x] Node berjalan: `pixi run gpio-node` — subscribe `/gpio/write`, publish `/gpio/state` - [x] Parameter `output_pins`, `input_pins`, `input_publish_rate`, `gpio_chip` berfungsi via `--ros-args -p` - [x] Executor (`blockly_executor`) tetap berfungsi tanpa perubahan — interface ROS2 identik + +## 3 Enhancement: PCA9685 — 16-Channel PWM Controller (I2C) : [ ] +PCA9685 adalah 16-channel, 12-bit PWM controller via I2C. Motor DC kiwi wheel menggunakan 6 channel (3 motor × 2: PWM + direction), sehingga 10 channel tersedia untuk extensi (servo, LED, dll). Node ini general-purpose — mengontrol channel mana saja via Blockly block dengan parameter address, channel, dan PWM value. + +### Implementasi + +#### A. Package Structure (C++, ament_cmake) +``` +src/pca9685_node/ +├── CMakeLists.txt # ament_cmake — NO external lib dependency +├── package.xml # depend: rclcpp, blockly_interfaces +├── include/pca9685_node/ +│ └── pca9685_node.hpp # Pca9685Node class + I2C helpers +└── src/ + ├── pca9685_node.cpp # I2C init, write_callback, set_pwm() + └── main.cpp # rclcpp::spin(node) +``` + +Hardware interface menggunakan Linux I2C (`/dev/i2c-X`) via `ioctl()` — tidak perlu external library, cukup `linux/i2c-dev.h` (kernel header). + +#### B. ROS2 Interface + +**New message** — `blockly_interfaces/msg/PwmWrite.msg`: +``` +uint8 address # I2C address (default 0x40, configurable via solder bridges: 0x40–0x7F) +uint8 channel # PWM channel (0–15) +uint16 value # Duty cycle (0–4095, 12-bit resolution) +``` + +**Topic**: `/pwm/write` (executor → pca9685_node) + +**ROS2 Parameters** (configurable via `--ros-args -p`): +| Parameter | Type | Default | Fungsi | +|---|---|---|---| +| `i2c_device` | string | `/dev/i2c-1` | Linux I2C device path | +| `frequency` | int | 50 | PWM frequency Hz (semua channel) | + +PCA9685 write-only — tidak perlu `PwmRead.msg`. + +#### C. Node Behavior — `Pca9685Node` +1. **Constructor**: open `i2c_device`, configure prescaler berdasarkan `frequency` param +2. **Subscribe** `/pwm/write` (`PwmWrite`) — set duty cycle via I2C register write +3. **`set_pwm(address, channel, value)`**: select I2C slave address via `ioctl(I2C_SLAVE)`, write 4 bytes ke channel registers +4. **Multi-address support**: satu node bisa mengontrol multiple PCA9685 boards (address dikirim per-message, `ioctl(I2C_SLAVE)` di-set setiap write) +5. **Cleanup**: close file descriptor di destructor + +PCA9685 register map: +| Register | Address | Fungsi | +|---|---|---| +| MODE1 | 0x00 | Sleep/restart, auto-increment | +| LED0_ON_L | 0x06 | Channel 0 ON timing (4 registers per channel) | +| PRE_SCALE | 0xFE | PWM frequency: `prescale = round(25MHz / (4096 × freq)) - 1` | + +#### D. Handler — `blockly_executor/handlers/pwm.py` +```python +@handler("pwm_write") +def handle_pwm_write(params, hardware): + address = int(params["address"], 16) # hex string → int + channel = int(params["channel"]) + value = int(params["value"]) + # Dummy: log only. Real: publish PwmWrite to /pwm/write +``` +Lazy-create publisher di `hardware.node._pwm_write_pub`, sama dengan pola `gpio.py`. + +#### E. Blockly Block — `pwmWrite.js` +``` +┌──────────────────────────────────────────────┐ +│ PCA9685 addr: [0x40] │ +│ channel: [0 ▾] pwm: [■ value] │ +└──────────────────────────────────────────────┘ +``` +- **addr**: `FieldDropdown` — common addresses (0x40–0x47) +- **channel**: `FieldNumber` (0–15) +- **pwm**: `ValueInput` (0–4095) — accepts expression blocks, uses `String(expr)` pattern +- Category: `Robot`, Command: `pwm_write` + +#### F. pixi.toml Changes +- `setup-dep`: tambah `i2c-tools` (optional, untuk debugging `i2cdetect`) +- `build-pca9685`: `colcon build --packages-select pca9685_node` (depends-on: setup-dep, build-interfaces) +- `pca9685-node`: `ros2 run pca9685_node pca9685_node` + +Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel. + +### Definition Of Done +- [x] `src/pca9685_node/` berisi `CMakeLists.txt`, `package.xml`, `include/`, `src/` +- [x] `blockly_interfaces/msg/PwmWrite.msg` terdaftar di `rosidl_generate_interfaces()` +- [x] `pixi run build-interfaces` berhasil — PwmWrite.msg ter-generate +- [ ] `pixi run build-pca9685` berhasil di Raspberry Pi (native build) tanpa error +- [ ] Node berjalan: `pixi run pca9685-node` — subscribe `/pwm/write` +- [ ] Parameter `i2c_device`, `frequency` berfungsi via `--ros-args -p` +- [x] Handler `pwm_write` berfungsi di dummy mode (test passes) +- [x] Blockly block `pwmWrite` muncul di toolbox, generate valid JS code +- [ ] End-to-end: Blockly block → executor (real) → `/pwm/write` → pca9685_node → I2C write diff --git a/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js b/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js index 84d28b5..5247862 100644 --- a/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js +++ b/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js @@ -13,4 +13,5 @@ const BLOCK_FILES = [ 'digitalOut.js', 'digitalIn.js', 'delay.js', + 'pwmWrite.js', ]; diff --git a/src/blockly_app/blockly_app/ui/blockly/blocks/pwmWrite.js b/src/blockly_app/blockly_app/ui/blockly/blocks/pwmWrite.js new file mode 100644 index 0000000..22cdb93 --- /dev/null +++ b/src/blockly_app/blockly_app/ui/blockly/blocks/pwmWrite.js @@ -0,0 +1,39 @@ + +BlockRegistry.register({ + name: 'pwmWrite', + category: 'Robot', + categoryColor: '#5b80a5', + color: '#FF9800', + tooltip: 'Set PCA9685 PWM channel duty cycle (0-4095)', + + definition: { + init: function () { + this.appendValueInput('PWM_VALUE') + .appendField('PCA9685') + .appendField(' addr:') + .appendField(new Blockly.FieldDropdown([ + ['0x40', '64'], ['0x41', '65'], ['0x42', '66'], ['0x43', '67'], + ['0x44', '68'], ['0x45', '69'], ['0x46', '70'], ['0x47', '71'] + ]), 'ADDRESS') + .appendField(' ch:') + .appendField(new Blockly.FieldNumber(0, 0, 15, 1), 'CHANNEL') + .setCheck('Number') + .appendField(' pwm:'); + + this.setPreviousStatement(true, null); + this.setNextStatement(true, null); + this.setColour('#FF9800'); + this.setTooltip('Set PCA9685 PWM channel duty cycle (0-4095)'); + } + }, + + generator: function (block) { + var ADDRESS = block.getFieldValue('ADDRESS'); + var CHANNEL = block.getFieldValue('CHANNEL'); + var VALUE = Blockly.JavaScript.valueToCode(block, 'PWM_VALUE', Blockly.JavaScript.ORDER_ATOMIC) || '0'; + return ( + 'await highlightBlock(\'' + block.id + '\');\n' + + 'await executeAction(\'pwm_write\', { address: \'' + ADDRESS + '\', channel: \'' + CHANNEL + '\', value: String(' + VALUE + ') });\n' + ); + } +}); diff --git a/src/blockly_executor/blockly_executor/handlers/pwm.py b/src/blockly_executor/blockly_executor/handlers/pwm.py new file mode 100644 index 0000000..a95f6ff --- /dev/null +++ b/src/blockly_executor/blockly_executor/handlers/pwm.py @@ -0,0 +1,49 @@ +"""PWM command handler — PCA9685 16-channel PWM control via I2C. + +Real mode communication: + pwm_write → publish PwmWrite to /pwm/write → pca9685_node writes I2C register +""" + +from . import handler +from .hardware import Hardware + + +def _get_pwm_write_publisher(hardware: Hardware): + """Lazy-create a publisher for /pwm/write on the executor node.""" + if not hasattr(hardware.node, "_pwm_write_pub"): + from blockly_interfaces.msg import PwmWrite + + hardware.node._pwm_write_pub = hardware.node.create_publisher( + PwmWrite, "/pwm/write", 10 + ) + return hardware.node._pwm_write_pub + + +@handler("pwm_write") +def handle_pwm_write(params: dict[str, str], hardware: Hardware) -> tuple[bool, str]: + address = int(params["address"]) + channel = int(params["channel"]) + value = int(params["value"]) + + if channel < 0 or channel > 15: + return (False, f"Invalid channel {channel} (must be 0-15)") + + if value < 0 or value > 4095: + return (False, f"Invalid PWM value {value} (must be 0-4095)") + + hardware.log(f"pwm_write(addr=0x{address:02X}, ch={channel}, val={value})") + + if hardware.is_real(): + from blockly_interfaces.msg import PwmWrite + + pub = _get_pwm_write_publisher(hardware) + msg = PwmWrite() + msg.address = address + msg.channel = channel + msg.value = value + pub.publish(msg) + hardware.node.get_logger().info( + f"PWM write: addr=0x{address:02X} ch={channel} value={value}" + ) + + return (True, f"PWM ch{channel} set to {value} (addr=0x{address:02X})") diff --git a/src/blockly_executor/test/test_block_pca9685.py b/src/blockly_executor/test/test_block_pca9685.py new file mode 100644 index 0000000..5d42535 --- /dev/null +++ b/src/blockly_executor/test/test_block_pca9685.py @@ -0,0 +1,39 @@ +"""Integration tests for pwm_write handler (PCA9685).""" + + +def test_pwm_write_success(exe_action): + result = exe_action("pwm_write", address="64", channel="0", value="2048") + assert result.result.success is True + assert "ch0" in result.result.message + assert "2048" in result.result.message + + +def test_pwm_write_max_value(exe_action): + result = exe_action("pwm_write", address="64", channel="15", value="4095") + assert result.result.success is True + + +def test_pwm_write_zero_value(exe_action): + result = exe_action("pwm_write", address="64", channel="0", value="0") + assert result.result.success is True + + +def test_pwm_write_sends_feedback(exe_action): + result = exe_action("pwm_write", address="64", channel="0", value="1000") + assert len(result.feedbacks) > 0 + assert result.feedbacks[0].status == "executing" + + +def test_pwm_write_missing_params_fails(exe_action): + result = exe_action("pwm_write") + assert result.result.success is False + + +def test_pwm_write_invalid_channel_fails(exe_action): + result = exe_action("pwm_write", address="64", channel="16", value="100") + assert result.result.success is False + + +def test_pwm_write_invalid_value_fails(exe_action): + result = exe_action("pwm_write", address="64", channel="0", value="5000") + assert result.result.success is False diff --git a/src/blockly_interfaces/CMakeLists.txt b/src/blockly_interfaces/CMakeLists.txt index d607544..8b69443 100644 --- a/src/blockly_interfaces/CMakeLists.txt +++ b/src/blockly_interfaces/CMakeLists.txt @@ -8,6 +8,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/BlocklyAction.action" "msg/GpioWrite.msg" "msg/GpioRead.msg" + "msg/PwmWrite.msg" ) ament_export_dependencies(rosidl_default_runtime) diff --git a/src/blockly_interfaces/msg/PwmWrite.msg b/src/blockly_interfaces/msg/PwmWrite.msg new file mode 100644 index 0000000..e03e36b --- /dev/null +++ b/src/blockly_interfaces/msg/PwmWrite.msg @@ -0,0 +1,3 @@ +uint8 address # I2C device address (e.g. 0x40) +uint8 channel # PWM channel (0-15) +uint16 value # Duty cycle (0-4095, 12-bit) diff --git a/src/pca9685_node/CMakeLists.txt b/src/pca9685_node/CMakeLists.txt new file mode 100644 index 0000000..23f073c --- /dev/null +++ b/src/pca9685_node/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.10) +project(pca9685_node) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# ROS2 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(blockly_interfaces REQUIRED) + +add_executable(pca9685_node + src/main.cpp + src/pca9685_node.cpp +) + +target_include_directories(pca9685_node PUBLIC + $ + $ +) + +ament_target_dependencies(pca9685_node + rclcpp + blockly_interfaces +) + +install(TARGETS pca9685_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/pca9685_node/include/pca9685_node/pca9685_node.hpp b/src/pca9685_node/include/pca9685_node/pca9685_node.hpp new file mode 100644 index 0000000..3731580 --- /dev/null +++ b/src/pca9685_node/include/pca9685_node/pca9685_node.hpp @@ -0,0 +1,39 @@ +#ifndef PCA9685_NODE__PCA9685_NODE_HPP_ +#define PCA9685_NODE__PCA9685_NODE_HPP_ + +#include + +#include + +#include "blockly_interfaces/msg/pwm_write.hpp" + +class Pca9685Node : public rclcpp::Node +{ +public: + Pca9685Node(); + ~Pca9685Node() override; + +private: + void open_i2c(const std::string & device_path); + void configure_frequency(int frequency); + void write_callback(const blockly_interfaces::msg::PwmWrite::SharedPtr msg); + void set_pwm(uint8_t address, uint8_t channel, uint16_t value); + void write_register(uint8_t address, uint8_t reg, uint8_t data); + uint8_t read_register(uint8_t address, uint8_t reg); + + int i2c_fd_ = -1; + + // PCA9685 register addresses + static constexpr uint8_t REG_MODE1 = 0x00; + static constexpr uint8_t REG_PRESCALE = 0xFE; + static constexpr uint8_t REG_LED0_ON_L = 0x06; + + // MODE1 bits + static constexpr uint8_t MODE1_SLEEP = 0x10; + static constexpr uint8_t MODE1_AI = 0x20; // auto-increment + static constexpr uint8_t MODE1_RESTART = 0x80; + + rclcpp::Subscription::SharedPtr write_sub_; +}; + +#endif // PCA9685_NODE__PCA9685_NODE_HPP_ diff --git a/src/pca9685_node/package.xml b/src/pca9685_node/package.xml new file mode 100644 index 0000000..c5aca45 --- /dev/null +++ b/src/pca9685_node/package.xml @@ -0,0 +1,18 @@ + + + + pca9685_node + 0.1.0 + ROS2 PCA9685 PWM controller node — 16-channel I2C PWM via Linux i2c-dev (C++) + developer + MIT + + ament_cmake + + rclcpp + blockly_interfaces + + + ament_cmake + + diff --git a/src/pca9685_node/src/main.cpp b/src/pca9685_node/src/main.cpp new file mode 100644 index 0000000..4e4bd73 --- /dev/null +++ b/src/pca9685_node/src/main.cpp @@ -0,0 +1,11 @@ +#include +#include "pca9685_node/pca9685_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/pca9685_node/src/pca9685_node.cpp b/src/pca9685_node/src/pca9685_node.cpp new file mode 100644 index 0000000..d54fc21 --- /dev/null +++ b/src/pca9685_node/src/pca9685_node.cpp @@ -0,0 +1,185 @@ +#include "pca9685_node/pca9685_node.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +Pca9685Node::Pca9685Node() +: Node("pca9685_node") +{ + // Declare ROS2 parameters + this->declare_parameter("i2c_device", std::string("/dev/i2c-1")); + this->declare_parameter("frequency", 50); + + std::string device_path = this->get_parameter("i2c_device").as_string(); + int frequency = static_cast(this->get_parameter("frequency").as_int()); + + // Open I2C device + open_i2c(device_path); + + // Configure PWM frequency on default address (0x40) + configure_frequency(frequency); + + // Subscribe to /pwm/write from executor + write_sub_ = this->create_subscription( + "/pwm/write", 10, + std::bind(&Pca9685Node::write_callback, this, std::placeholders::_1)); + + RCLCPP_INFO( + this->get_logger(), + "Pca9685Node ready — device=%s, frequency=%d Hz", + device_path.c_str(), frequency); +} + +Pca9685Node::~Pca9685Node() +{ + if (i2c_fd_ >= 0) { + close(i2c_fd_); + i2c_fd_ = -1; + } + RCLCPP_INFO(this->get_logger(), "I2C device closed"); +} + +void Pca9685Node::open_i2c(const std::string & device_path) +{ + i2c_fd_ = open(device_path.c_str(), O_RDWR); + if (i2c_fd_ < 0) { + throw std::runtime_error("Failed to open I2C device: " + device_path); + } +} + +void Pca9685Node::configure_frequency(int frequency) +{ + // PCA9685 prescale formula: prescale = round(25MHz / (4096 * freq)) - 1 + int prescale = static_cast(std::round(25000000.0 / (4096.0 * frequency))) - 1; + if (prescale < 3) prescale = 3; // min prescale (max ~1526 Hz) + if (prescale > 255) prescale = 255; // max prescale (min ~24 Hz) + + uint8_t address = 0x40; // default PCA9685 address + + // Read current MODE1 + uint8_t mode1 = read_register(address, REG_MODE1); + + // Sleep mode (required before changing prescale) + write_register(address, REG_MODE1, (mode1 & 0x7F) | MODE1_SLEEP); + + // Set prescale + write_register(address, REG_PRESCALE, static_cast(prescale)); + + // Wake up + enable auto-increment + write_register(address, REG_MODE1, (mode1 & 0x7F) | MODE1_AI); + + // Wait for oscillator (500µs per datasheet) + std::this_thread::sleep_for(std::chrono::microseconds(500)); + + // Restart + write_register(address, REG_MODE1, mode1 | MODE1_RESTART | MODE1_AI); + + RCLCPP_INFO( + this->get_logger(), + "PCA9685 (0x%02X) frequency configured: %d Hz (prescale=%d)", + address, frequency, prescale); +} + +void Pca9685Node::write_callback( + const blockly_interfaces::msg::PwmWrite::SharedPtr msg) +{ + uint8_t address = msg->address; + uint8_t channel = msg->channel; + uint16_t value = msg->value; + + if (channel > 15) { + RCLCPP_WARN(this->get_logger(), "Invalid channel %d (must be 0-15)", channel); + return; + } + + if (value > 4095) { + value = 4095; + } + + set_pwm(address, channel, value); + + RCLCPP_INFO( + this->get_logger(), + "PWM write: addr=0x%02X ch=%d value=%d", + address, channel, value); +} + +void Pca9685Node::set_pwm(uint8_t address, uint8_t channel, uint16_t value) +{ + // Each channel has 4 registers: ON_L, ON_H, OFF_L, OFF_H + // For simple duty cycle: ON=0, OFF=value + uint8_t reg_base = REG_LED0_ON_L + 4 * channel; + + // Select I2C slave address + if (ioctl(i2c_fd_, I2C_SLAVE, address) < 0) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to select I2C address 0x%02X", address); + return; + } + + // Write 4 bytes: ON_L, ON_H, OFF_L, OFF_H (auto-increment enabled) + uint8_t buf[5]; + buf[0] = reg_base; // starting register + buf[1] = 0; // ON_L = 0 + buf[2] = 0; // ON_H = 0 + buf[3] = value & 0xFF; // OFF_L = low byte + buf[4] = (value >> 8) & 0x0F; // OFF_H = high nibble + + if (write(i2c_fd_, buf, 5) != 5) { + RCLCPP_ERROR( + this->get_logger(), + "I2C write failed for addr=0x%02X ch=%d", address, channel); + } +} + +void Pca9685Node::write_register(uint8_t address, uint8_t reg, uint8_t data) +{ + if (ioctl(i2c_fd_, I2C_SLAVE, address) < 0) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to select I2C address 0x%02X", address); + return; + } + + uint8_t buf[2] = {reg, data}; + if (write(i2c_fd_, buf, 2) != 2) { + RCLCPP_ERROR( + this->get_logger(), + "I2C register write failed: addr=0x%02X reg=0x%02X", address, reg); + } +} + +uint8_t Pca9685Node::read_register(uint8_t address, uint8_t reg) +{ + if (ioctl(i2c_fd_, I2C_SLAVE, address) < 0) { + RCLCPP_ERROR( + this->get_logger(), + "Failed to select I2C address 0x%02X", address); + return 0; + } + + if (write(i2c_fd_, ®, 1) != 1) { + RCLCPP_ERROR( + this->get_logger(), + "I2C register select failed: addr=0x%02X reg=0x%02X", address, reg); + return 0; + } + + uint8_t data = 0; + if (read(i2c_fd_, &data, 1) != 1) { + RCLCPP_ERROR( + this->get_logger(), + "I2C register read failed: addr=0x%02X reg=0x%02X", address, reg); + return 0; + } + + return data; +}