diff --git a/DOCUMENTATION.md b/DOCUMENTATION.md index 831e459..6e67002 100644 --- a/DOCUMENTATION.md +++ b/DOCUMENTATION.md @@ -19,6 +19,7 @@ See [readme.md](readme.md) for project overview and status. | `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/) | +| `as5600_node` — AS5600 12-bit magnetic encoder (C++, I2C) | [src/as5600_node/](src/as5600_node/) | --- @@ -42,11 +43,13 @@ pixi run app # Terminal 2 — start desktop GUI 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 build-as5600 # installs system deps (apt) + builds as5600_node pixi run gpio-node # start GPIO node pixi run pca9685-node # start PCA9685 PWM node +pixi run as5600-node # start AS5600 encoder node ``` -`build-gpio` and `build-pca9685` automatically run `setup-dep` which installs system libraries (`libgpiod-dev`, `liblttng-ust-dev`, `i2c-tools`) via `apt`. +`build-gpio`, `build-pca9685`, and `build-as5600` 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 36b7628..3ff2be8 100644 --- a/pixi.toml +++ b/pixi.toml @@ -50,4 +50,6 @@ setup-dep = { cmd = "sudo apt update && sudo apt install -y liblttng-ust- 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"] } 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 +pca9685-node = { cmd = "bash -c 'source install/setup.bash && ros2 run pca9685_node pca9685_node'", depends-on = ["build-pca9685"] } +build-as5600 = { cmd = "colcon build --symlink-install --packages-select as5600_node", depends-on = ["setup-dep", "build-interfaces"] } +as5600-node = { cmd = "bash -c 'source install/setup.bash && ros2 run as5600_node as5600_node'", depends-on = ["build-as5600"] } \ No newline at end of file diff --git a/readme.md b/readme.md index 3e4168f..e4d867b 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**: Task #3 — PCA9685 PWM Controller (I2C) +> **Current Focus**: Task #4 — AS5600 Magnetic Encoder (I2C) Dokumentasi lengkap dapat dilihat di [DOCUMENTATION.md](DOCUMENTATION.md). @@ -36,9 +36,9 @@ 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 +- **Blockly UI Enhancement**: Lets make Human Interface in same view to help me monitoring node that needed. it programaticaly using block is real good, you can take labview interface as refrence. you can separated program as main_program to handle human interface. +- **Feasibility Study to implement Controller**: mobile robot need controller to move flawlesly. +- **Launch files**: ROS2 launch files to start all nodes with one command includ node in raspberry pi - **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 @@ -208,6 +208,23 @@ Lazy-create publisher di `hardware.node._pwm_write_pub`, sama dengan pola `gpio. Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel. +#### G. Penggunaan + +```bash +# Default — /dev/i2c-1, 50 Hz +pixi run pca9685-node + +# Ganti I2C device dan frequency via --ros-args +source install/setup.bash +ros2 run pca9685_node pca9685_node --ros-args -p i2c_device:=/dev/i2c-0 -p frequency:=1000 + +# Cek I2C bus yang tersedia di Pi +ls /dev/i2c-* # list semua bus +i2cdetect -y 1 # scan device di bus 1 (perlu i2c-tools) +``` + +**Catatan**: `pixi run pca9685-node` menggunakan parameter default. Untuk override parameter, jalankan `ros2 run` langsung (setelah `source install/setup.bash`) karena pixi task tidak meneruskan `--ros-args` ke proses inner. + ### 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()` @@ -218,3 +235,107 @@ Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel. - [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 + +## 4 Enhancement: AS5600 — 12-bit Magnetic Rotary Encoder (I2C) : [ ] +AS5600 adalah 12-bit magnetic rotary position sensor via I2C. Kiwi wheel AMR menggunakan 3 encoder (satu per roda) untuk feedback posisi. AS5600 memiliki **fixed I2C address (0x36)** — untuk 3 module, setiap encoder menggunakan **I2C bus terpisah** (e.g., `/dev/i2c-1`, `/dev/i2c-3`, `/dev/i2c-4`). Node ini publisher — membaca angle secara periodik dan publish ke ROS2 topic. + +### Implementasi + +#### A. Package Structure (C++, ament_cmake) +``` +src/as5600_node/ +├── CMakeLists.txt # ament_cmake — NO external lib dependency +├── package.xml # depend: rclcpp, blockly_interfaces +├── include/as5600_node/ +│ └── as5600_node.hpp # As5600Node class + I2C helpers +└── src/ + ├── as5600_node.cpp # I2C init, timer_callback, read_raw_angle() + └── 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/EncoderRead.msg`: +``` +uint8 encoder_id # Encoder index (0, 1, 2, ...) +float32 angle # Angle in degrees (0.0-360.0) +uint16 raw_angle # Raw 12-bit value (0-4095) +``` + +**Topic**: `/encoder/state` (as5600_node → executor) + +**ROS2 Parameters** (configurable via `--ros-args -p`): +| Parameter | Type | Default | Fungsi | +|---|---|---|---| +| `i2c_devices` | string[] | `["/dev/i2c-1"]` | List of I2C device paths, satu per encoder | +| `publish_rate` | double | 10.0 | Publish frequency Hz | + +#### C. Node Behavior — `As5600Node` +1. **Constructor**: open setiap I2C bus, set slave address 0x36 via `ioctl(I2C_SLAVE)`, create publisher dan timer +2. **Timer callback**: iterate semua I2C fds → read 2-byte RAW_ANGLE register → compute `angle = raw * 360.0 / 4096.0` → publish EncoderRead +3. **`read_raw_angle(fd)`**: write register address 0x0C, read 2 bytes → `((buf[0] & 0x0F) << 8) | buf[1]` (12-bit) +4. **Multi-bus support**: satu node mengontrol semua encoder — setiap fd dedicated ke satu bus/encoder +5. **Cleanup**: close semua file descriptors di destructor + +AS5600 register map: +| Register | Address | Fungsi | +|---|---|---| +| RAW_ANGLE | 0x0C-0x0D | 12-bit raw angle (0-4095) | +| STATUS | 0x0B | Magnet detect status | +| AGC | 0x1A | Automatic gain control | + +#### D. Handler — `blockly_executor/handlers/encoder.py` +```python +@handler("encoder_read") +def handle_encoder_read(params, hardware): + encoder_id = int(params["encoder_id"]) + # Dummy: return "0.0". Real: subscribe /encoder/state, return cached angle +``` +Lazy-create subscriber dengan cache `{encoder_id: {angle, raw_angle}}`, sama dengan pola `digital_in` di `gpio.py`. + +#### E. Blockly Block — `encoderRead.js` +``` +┌──────────────────────────────────────┐ +│ Encoder Read id: [0] │ +└──────────────────────────────────────┘ +``` +- **id**: `FieldNumber` (0–2) +- Returns: `Number` (angle 0-360) +- Category: `Robot`, Command: `encoder_read` +- Output block (can be used in expressions, e.g., `set variable to [Encoder Read id: 0]`) + +#### F. pixi.toml Changes +- `build-as5600`: `colcon build --packages-select as5600_node` (depends-on: setup-dep, build-interfaces) +- `as5600-node`: `ros2 run as5600_node as5600_node` + +Tidak perlu conda deps baru — Linux I2C headers sudah tersedia di kernel. + +#### G. Penggunaan + +```bash +# Default — /dev/i2c-1, 10 Hz, 1 encoder +pixi run as5600-node + +# 3 encoder pada bus terpisah, 20 Hz +source install/setup.bash +ros2 run as5600_node as5600_node --ros-args \ + -p i2c_devices:="['/dev/i2c-1', '/dev/i2c-3', '/dev/i2c-4']" \ + -p publish_rate:=20.0 + +# Raspberry Pi: enable extra I2C buses via config.txt +# dtoverlay=i2c-gpio,bus=3,i2c_gpio_sda=17,i2c_gpio_scl=27 +# dtoverlay=i2c-gpio,bus=4,i2c_gpio_sda=22,i2c_gpio_scl=23 +``` + +### Definition Of Done +- [x] `src/as5600_node/` berisi `CMakeLists.txt`, `package.xml`, `include/`, `src/` +- [x] `blockly_interfaces/msg/EncoderRead.msg` terdaftar di `rosidl_generate_interfaces()` +- [ ] `pixi run build-interfaces` berhasil — EncoderRead.msg ter-generate +- [ ] `pixi run build-as5600` berhasil di Raspberry Pi (native build) tanpa error +- [ ] Node berjalan: `pixi run as5600-node` — publish `/encoder/state` +- [ ] Parameter `i2c_devices`, `publish_rate` berfungsi via `--ros-args -p` +- [x] Handler `encoder_read` berfungsi di dummy mode (test passes) +- [x] Blockly block `encoderRead` muncul di toolbox, generate valid JS code +- [ ] End-to-end: Blockly block → executor (real) → cache `/encoder/state` → return angle diff --git a/src/as5600_node/CMakeLists.txt b/src/as5600_node/CMakeLists.txt new file mode 100644 index 0000000..01c7c27 --- /dev/null +++ b/src/as5600_node/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.10) +project(as5600_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(as5600_node + src/main.cpp + src/as5600_node.cpp +) + +target_include_directories(as5600_node PUBLIC + $ + $ +) + +ament_target_dependencies(as5600_node + rclcpp + blockly_interfaces +) + +install(TARGETS as5600_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/as5600_node/include/as5600_node/as5600_node.hpp b/src/as5600_node/include/as5600_node/as5600_node.hpp new file mode 100644 index 0000000..96648b1 --- /dev/null +++ b/src/as5600_node/include/as5600_node/as5600_node.hpp @@ -0,0 +1,35 @@ +#ifndef AS5600_NODE__AS5600_NODE_HPP_ +#define AS5600_NODE__AS5600_NODE_HPP_ + +#include +#include + +#include + +#include "blockly_interfaces/msg/encoder_read.hpp" + +class As5600Node : public rclcpp::Node +{ +public: + As5600Node(); + ~As5600Node() override; + +private: + void open_i2c_buses(const std::vector & device_paths); + uint16_t read_raw_angle(int fd); + void timer_callback(); + + std::vector i2c_fds_; + + // AS5600 fixed I2C address + static constexpr uint8_t AS5600_ADDR = 0x36; + + // AS5600 register addresses + static constexpr uint8_t REG_RAW_ANGLE_H = 0x0C; + static constexpr uint8_t REG_STATUS = 0x0B; + + rclcpp::Publisher::SharedPtr encoder_pub_; + rclcpp::TimerBase::SharedPtr read_timer_; +}; + +#endif // AS5600_NODE__AS5600_NODE_HPP_ diff --git a/src/as5600_node/package.xml b/src/as5600_node/package.xml new file mode 100644 index 0000000..d325499 --- /dev/null +++ b/src/as5600_node/package.xml @@ -0,0 +1,18 @@ + + + + as5600_node + 0.1.0 + ROS2 AS5600 magnetic encoder node — 12-bit rotary position via Linux i2c-dev (C++) + developer + MIT + + ament_cmake + + rclcpp + blockly_interfaces + + + ament_cmake + + diff --git a/src/as5600_node/src/as5600_node.cpp b/src/as5600_node/src/as5600_node.cpp new file mode 100644 index 0000000..21bd4ad --- /dev/null +++ b/src/as5600_node/src/as5600_node.cpp @@ -0,0 +1,106 @@ +#include "as5600_node/as5600_node.hpp" + +#include +#include +#include +#include + +#include +#include + +As5600Node::As5600Node() +: Node("as5600_node") +{ + // Declare ROS2 parameters + this->declare_parameter("i2c_devices", std::vector{"/dev/i2c-1"}); + this->declare_parameter("publish_rate", 10.0); + + auto device_paths = this->get_parameter("i2c_devices").as_string_array(); + double rate = this->get_parameter("publish_rate").as_double(); + + // Open all I2C buses + open_i2c_buses(device_paths); + + // Create publisher for /encoder/state + encoder_pub_ = this->create_publisher( + "/encoder/state", 10); + + // Create timer for periodic reads + auto period = std::chrono::duration(1.0 / rate); + read_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&As5600Node::timer_callback, this)); + + RCLCPP_INFO( + this->get_logger(), + "As5600Node ready — %zu encoder(s), publish_rate=%.1f Hz", + i2c_fds_.size(), rate); +} + +As5600Node::~As5600Node() +{ + for (int fd : i2c_fds_) { + if (fd >= 0) { + close(fd); + } + } + RCLCPP_INFO(this->get_logger(), "I2C devices closed"); +} + +void As5600Node::open_i2c_buses(const std::vector & device_paths) +{ + for (size_t i = 0; i < device_paths.size(); ++i) { + int fd = open(device_paths[i].c_str(), O_RDWR); + if (fd < 0) { + throw std::runtime_error( + "Failed to open I2C device: " + device_paths[i]); + } + + // Set slave address once — each fd is dedicated to one AS5600 + if (ioctl(fd, I2C_SLAVE, AS5600_ADDR) < 0) { + close(fd); + throw std::runtime_error( + "Failed to set I2C slave address 0x36 on " + device_paths[i]); + } + + i2c_fds_.push_back(fd); + RCLCPP_INFO( + this->get_logger(), + "Encoder %zu opened on %s (addr=0x%02X)", + i, device_paths[i].c_str(), AS5600_ADDR); + } +} + +uint16_t As5600Node::read_raw_angle(int fd) +{ + // AS5600 RAW_ANGLE register: 0x0C (high byte) + 0x0D (low byte) + // Write register address, then read 2 bytes + uint8_t reg = REG_RAW_ANGLE_H; + if (write(fd, ®, 1) != 1) { + RCLCPP_ERROR(this->get_logger(), "I2C register select failed"); + return 0; + } + + uint8_t buf[2] = {0, 0}; + if (read(fd, buf, 2) != 2) { + RCLCPP_ERROR(this->get_logger(), "I2C read failed"); + return 0; + } + + // 12-bit value: high nibble of buf[0] + full buf[1] + return ((buf[0] & 0x0F) << 8) | buf[1]; +} + +void As5600Node::timer_callback() +{ + for (size_t i = 0; i < i2c_fds_.size(); ++i) { + uint16_t raw = read_raw_angle(i2c_fds_[i]); + float angle = static_cast(raw) * 360.0f / 4096.0f; + + blockly_interfaces::msg::EncoderRead msg; + msg.encoder_id = static_cast(i); + msg.raw_angle = raw; + msg.angle = angle; + encoder_pub_->publish(msg); + } +} diff --git a/src/as5600_node/src/main.cpp b/src/as5600_node/src/main.cpp new file mode 100644 index 0000000..8a4d6e7 --- /dev/null +++ b/src/as5600_node/src/main.cpp @@ -0,0 +1,11 @@ +#include +#include "as5600_node/as5600_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/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js b/src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js new file mode 100644 index 0000000..05e323d --- /dev/null +++ b/src/blockly_app/blockly_app/ui/blockly/blocks/encoderRead.js @@ -0,0 +1,26 @@ + +BlockRegistry.register({ + name: 'encoderRead', + category: 'Robot', + categoryColor: '#5b80a5', + color: '#9C27B0', + tooltip: 'Read AS5600 encoder angle in degrees (0-360)', + + definition: { + init: function () { + this.appendDummyInput() + .appendField('Encoder Read id:') + .appendField(new Blockly.FieldNumber(0, 0, 2, 1), 'ENCODER_ID'); + this.setOutput(true, 'Number'); + this.setColour('#9C27B0'); + this.setTooltip('Read AS5600 encoder angle in degrees (0-360)'); + } + }, + + generator: function (block) { + var encoderId = block.getFieldValue('ENCODER_ID'); + var code = + 'parseFloat((await executeAction(\'encoder_read\', { encoder_id: \'' + encoderId + '\' })).message)'; + return [code, Blockly.JavaScript.ORDER_AWAIT]; + } +}); 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 5247862..585795d 100644 --- a/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js +++ b/src/blockly_app/blockly_app/ui/blockly/blocks/manifest.js @@ -14,4 +14,5 @@ const BLOCK_FILES = [ 'digitalIn.js', 'delay.js', 'pwmWrite.js', + 'encoderRead.js', ]; diff --git a/src/blockly_executor/blockly_executor/handlers/encoder.py b/src/blockly_executor/blockly_executor/handlers/encoder.py new file mode 100644 index 0000000..62eb902 --- /dev/null +++ b/src/blockly_executor/blockly_executor/handlers/encoder.py @@ -0,0 +1,55 @@ +"""AS5600 encoder handler — read cached angle from /encoder/state topic. + +Real mode communication: + encoder_read ← subscribe EncoderRead from /encoder/state ← as5600_node reads I2C +""" + +import threading + +from . import handler +from .hardware import Hardware + + +def _get_encoder_state_subscriber(hardware: Hardware): + """Lazy-create a subscriber for /encoder/state with per-encoder cache. + + as5600_node publishes EncoderRead per encoder periodically. + Subscriber caches latest angle per encoder_id. + """ + if not hasattr(hardware.node, "_encoder_state_cache"): + from blockly_interfaces.msg import EncoderRead + + hardware.node._encoder_state_cache = {} + hardware.node._encoder_state_lock = threading.Lock() + + def _encoder_state_cb(msg: EncoderRead): + with hardware.node._encoder_state_lock: + hardware.node._encoder_state_cache[msg.encoder_id] = { + "angle": msg.angle, + "raw_angle": msg.raw_angle, + } + + hardware.node._encoder_state_sub = hardware.node.create_subscription( + EncoderRead, "/encoder/state", _encoder_state_cb, 10 + ) + return hardware.node._encoder_state_cache + + +@handler("encoder_read") +def handle_encoder_read( + params: dict[str, str], hardware: Hardware +) -> tuple[bool, str]: + encoder_id = int(params["encoder_id"]) + hardware.log(f"encoder_read(id={encoder_id})") + + if hardware.is_real(): + cache = _get_encoder_state_subscriber(hardware) + with hardware.node._encoder_state_lock: + data = cache.get(encoder_id) + + if data is None: + return (True, "0.0") + return (True, str(data["angle"])) + + # Dummy mode — return 0.0 + return (True, "0.0") diff --git a/src/blockly_executor/test/test_block_encoder.py b/src/blockly_executor/test/test_block_encoder.py new file mode 100644 index 0000000..9963ee6 --- /dev/null +++ b/src/blockly_executor/test/test_block_encoder.py @@ -0,0 +1,30 @@ +"""Integration tests for encoder_read handler (AS5600).""" + + +def test_encoder_read_success(exe_action): + result = exe_action("encoder_read", encoder_id="0") + assert result.result.success is True + # Dummy mode returns "0.0" + angle = float(result.result.message) + assert 0.0 <= angle <= 360.0 + + +def test_encoder_read_id_1(exe_action): + result = exe_action("encoder_read", encoder_id="1") + assert result.result.success is True + + +def test_encoder_read_id_2(exe_action): + result = exe_action("encoder_read", encoder_id="2") + assert result.result.success is True + + +def test_encoder_read_sends_feedback(exe_action): + result = exe_action("encoder_read", encoder_id="0") + assert len(result.feedbacks) > 0 + assert result.feedbacks[0].status == "executing" + + +def test_encoder_read_missing_params_fails(exe_action): + result = exe_action("encoder_read") + assert result.result.success is False diff --git a/src/blockly_interfaces/CMakeLists.txt b/src/blockly_interfaces/CMakeLists.txt index 8b69443..6a7a9c1 100644 --- a/src/blockly_interfaces/CMakeLists.txt +++ b/src/blockly_interfaces/CMakeLists.txt @@ -9,6 +9,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/GpioWrite.msg" "msg/GpioRead.msg" "msg/PwmWrite.msg" + "msg/EncoderRead.msg" ) ament_export_dependencies(rosidl_default_runtime) diff --git a/src/blockly_interfaces/msg/EncoderRead.msg b/src/blockly_interfaces/msg/EncoderRead.msg new file mode 100644 index 0000000..6ee0d92 --- /dev/null +++ b/src/blockly_interfaces/msg/EncoderRead.msg @@ -0,0 +1,3 @@ +uint8 encoder_id # Encoder index (0, 1, 2, ...) +float32 angle # Angle in degrees (0.0-360.0) +uint16 raw_angle # Raw 12-bit value (0-4095)