feat: add PCA9685 PWM controller node with I2C support; implement message handling and integration tests
This commit is contained in:
parent
059f7431e5
commit
c23e1471b2
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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"] }
|
||||
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"] }
|
||||
97
readme.md
97
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
|
||||
|
|
|
|||
|
|
@ -13,4 +13,5 @@ const BLOCK_FILES = [
|
|||
'digitalOut.js',
|
||||
'digitalIn.js',
|
||||
'delay.js',
|
||||
'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'
|
||||
);
|
||||
}
|
||||
});
|
||||
|
|
@ -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})")
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
|
||||
ament_target_dependencies(pca9685_node
|
||||
rclcpp
|
||||
blockly_interfaces
|
||||
)
|
||||
|
||||
install(TARGETS pca9685_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#ifndef PCA9685_NODE__PCA9685_NODE_HPP_
|
||||
#define PCA9685_NODE__PCA9685_NODE_HPP_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#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<blockly_interfaces::msg::PwmWrite>::SharedPtr write_sub_;
|
||||
};
|
||||
|
||||
#endif // PCA9685_NODE__PCA9685_NODE_HPP_
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>pca9685_node</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS2 PCA9685 PWM controller node — 16-channel I2C PWM via Linux i2c-dev (C++)</description>
|
||||
<maintainer email="dev@example.com">developer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>blockly_interfaces</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "pca9685_node/pca9685_node.hpp"
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<Pca9685Node>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,185 @@
|
|||
#include "pca9685_node/pca9685_node.hpp"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
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<int>(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<blockly_interfaces::msg::PwmWrite>(
|
||||
"/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<int>(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<uint8_t>(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;
|
||||
}
|
||||
Loading…
Reference in New Issue