diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index 567a169..8553ef2 100644 --- a/boards/nucleo_h563zi.overlay +++ b/boards/nucleo_h563zi.overlay @@ -1,20 +1,27 @@ / { aliases { - led0 = &user_led; + led0 = &red_led_1; + led1 = &yellow_led_1; + led2 = &green_led_1; + + servo0 = &pwm_servo0; }; - leds { - compatible = "gpio-leds"; - user_led: user_led_0 { - gpios = <&gpiob 0 GPIO_ACTIVE_HIGH>; - label = "USER_LED_0"; - }; - }; + pwmleds: pwmleds { + compatible = "pwm-leds"; + status = "okay"; + + pwm_servo0: pwm_servo0 { + pwms = <&pwm4 3 PWM_MSEC(20) PWM_POLARITY_NORMAL>; + label = "servo 0"; + }; + }; + }; &uart4 { status = "okay"; - current-speed = <921600>; + current-speed = <460800>; pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>; pinctrl-names = "default"; }; @@ -23,15 +30,13 @@ status = "okay"; }; -&timers1 { +&timers4 { status = "okay"; + st,prescaler = <119>; - st,prescaler = <0>; /* Required */ - - pwm0: pwm { - compatible = "st,stm32-pwm"; + pwm4: pwm { status = "okay"; - pinctrl-0 = <&tim1_ch1_pa8>; + pinctrl-0 = <&tim4_ch3_pb8>; pinctrl-names = "default"; }; -}; \ No newline at end of file +}; diff --git a/src/devices.cpp b/src/devices.cpp index 9e07776..f6233f1 100644 --- a/src/devices.cpp +++ b/src/devices.cpp @@ -4,7 +4,22 @@ bool verify_are_devices_available(void) { bool devices_ready = true; if (!device_is_ready(led0.port)) { - printk("LED not ready\n"); + printk("LED0 not ready\n"); + devices_ready = false; + } + + if (!device_is_ready(led1.port)) { + printk("LED1 not ready\n"); + devices_ready = false; + } + + if (!device_is_ready(led2.port)) { + printk("LED2 not ready\n"); + devices_ready = false; + } + + if (!pwm_is_ready_dt(&servo0)) { + printk("Servo0 not ready\n"); devices_ready = false; } diff --git a/src/devices.hpp b/src/devices.hpp index 4c10b95..355c54e 100644 --- a/src/devices.hpp +++ b/src/devices.hpp @@ -6,12 +6,17 @@ #include #include #include +#include #else #include #include -#include +#include #endif static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios); +static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios); +static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios); + +static const struct pwm_dt_spec servo0 = PWM_DT_SPEC_GET(DT_ALIAS(servo0)); bool verify_are_devices_available(void); \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 589571b..4f0c32b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,12 +13,15 @@ #include "microros_node.hpp" #include "led0_subscriber.hpp" +#include "led1_subscriber.hpp" +#include "led2_subscriber.hpp" +#include "servo0_subscriber.hpp" int main(void) { if (!verify_are_devices_available()) { - printk("##### Not all devices are available, aborting launch #####"); + printk("##### Not all devices are available, aborting launch #####\n"); for (;;){} } @@ -34,10 +37,16 @@ int main(void) MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0"); while (!node0.connect()) { - printk("Waiting for connection ..."); + printk("Waiting for connection ...\n"); }; + printk("Connection to Agent node established!\n"); + printk("Starting executor ...\n"); + led0_register(&node0); + led1_register(&node0); + led2_register(&node0); + servo0_register(&node0); node0.execute(); diff --git a/src/microros/microros_node.cpp b/src/microros/microros_node.cpp index 2f0f422..9db7821 100644 --- a/src/microros/microros_node.cpp +++ b/src/microros/microros_node.cpp @@ -41,7 +41,7 @@ bool MicrorosExecutor::connect() { if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK) return false; - if (rclc_executor_init(&this->executor, &this->support.context, 1, &this->allocator) != RCL_RET_OK) + if (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK) return false; return true; @@ -67,6 +67,7 @@ rcl_publisher_t* MicrorosExecutor::add_publisher(const rosidl_message_type_suppo // Append to list sys_slist_append(&this->publishers_head, &pub_node->node); + printk("registered publisher %s\n", topic_name); return &pub_node->publisher; } @@ -88,5 +89,6 @@ rcl_subscription_t* MicrorosExecutor::add_subscription(const rosidl_message_type return nullptr; } + printk("registered subscriber %s\n", topic_name); return &sub_node->subscription; } diff --git a/src/microros/subscribers/CMakeLists.txt b/src/microros/subscribers/CMakeLists.txt index 9280b19..a00dcf3 100644 --- a/src/microros/subscribers/CMakeLists.txt +++ b/src/microros/subscribers/CMakeLists.txt @@ -1,3 +1,7 @@ cmake_minimum_required(VERSION 3.20.0) -add_subdirectory(led0) \ No newline at end of file +add_subdirectory(led0) +add_subdirectory(led1) +add_subdirectory(led2) + +add_subdirectory(servo0) diff --git a/src/microros/subscribers/led0/led0_subscriber.cpp b/src/microros/subscribers/led0/led0_subscriber.cpp index c3808cb..528cd1b 100644 --- a/src/microros/subscribers/led0/led0_subscriber.cpp +++ b/src/microros/subscribers/led0/led0_subscriber.cpp @@ -6,11 +6,12 @@ #include "devices.hpp" -std_msgs__msg__Bool msg; +std_msgs__msg__Bool led0_msg; void led0_callback(const void *msgin){ const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin; + printk("Set led0 to %s\n", msg->data ? "on" : "off"); gpio_pin_configure_dt(&led0, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE); } @@ -19,7 +20,7 @@ void led0_register(MicrorosExecutor* node) { node->add_subscription( ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "/nucleo/led0", - &msg, + &led0_msg, led0_callback ); } diff --git a/src/microros/subscribers/led1/CMakeLists.txt b/src/microros/subscribers/led1/CMakeLists.txt new file mode 100644 index 0000000..b613866 --- /dev/null +++ b/src/microros/subscribers/led1/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.20.0) + +target_include_directories(app PRIVATE .) + +target_sources(app PRIVATE + led1_subscriber.cpp +) \ No newline at end of file diff --git a/src/microros/subscribers/led1/led1_subscriber.cpp b/src/microros/subscribers/led1/led1_subscriber.cpp new file mode 100644 index 0000000..cee27ae --- /dev/null +++ b/src/microros/subscribers/led1/led1_subscriber.cpp @@ -0,0 +1,26 @@ +#include "led1_subscriber.hpp" + +#include +#include + +#include "devices.hpp" + + +std_msgs__msg__Bool led1_msg; + +void led1_callback(const void *msgin){ + const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin; + + printk("Set led1 to %s\n", msg->data ? "on" : "off"); + gpio_pin_configure_dt(&led1, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE); +} + + +void led1_register(MicrorosExecutor* node) { + node->add_subscription( + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), + "/nucleo/led1", + &led1_msg, + led1_callback + ); +} diff --git a/src/microros/subscribers/led1/led1_subscriber.hpp b/src/microros/subscribers/led1/led1_subscriber.hpp new file mode 100644 index 0000000..dd583d7 --- /dev/null +++ b/src/microros/subscribers/led1/led1_subscriber.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include "microros_node.hpp" + +void led1_register(MicrorosExecutor* node); diff --git a/src/microros/subscribers/led2/CMakeLists.txt b/src/microros/subscribers/led2/CMakeLists.txt new file mode 100644 index 0000000..69eaef0 --- /dev/null +++ b/src/microros/subscribers/led2/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.20.0) + +target_include_directories(app PRIVATE .) + +target_sources(app PRIVATE + led2_subscriber.cpp +) \ No newline at end of file diff --git a/src/microros/subscribers/led2/led2_subscriber.cpp b/src/microros/subscribers/led2/led2_subscriber.cpp new file mode 100644 index 0000000..c1c7aa1 --- /dev/null +++ b/src/microros/subscribers/led2/led2_subscriber.cpp @@ -0,0 +1,26 @@ +#include "led2_subscriber.hpp" + +#include +#include + +#include "devices.hpp" + + +std_msgs__msg__Bool led2_msg; + +void led2_callback(const void *msgin){ + const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin; + + printk("Set led2 to %s\n", msg->data ? "on" : "off"); + gpio_pin_configure_dt(&led2, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE); +} + + +void led2_register(MicrorosExecutor* node) { + node->add_subscription( + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), + "/nucleo/led2", + &led2_msg, + led2_callback + ); +} diff --git a/src/microros/subscribers/led2/led2_subscriber.hpp b/src/microros/subscribers/led2/led2_subscriber.hpp new file mode 100644 index 0000000..4e61633 --- /dev/null +++ b/src/microros/subscribers/led2/led2_subscriber.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include "microros_node.hpp" + +void led2_register(MicrorosExecutor* node); diff --git a/src/microros/subscribers/servo0/CMakeLists.txt b/src/microros/subscribers/servo0/CMakeLists.txt new file mode 100644 index 0000000..27d91b5 --- /dev/null +++ b/src/microros/subscribers/servo0/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.20.0) + +target_include_directories(app PRIVATE .) + +target_sources(app PRIVATE + servo0_subscriber.cpp +) \ No newline at end of file diff --git a/src/microros/subscribers/servo0/servo0_subscriber.cpp b/src/microros/subscribers/servo0/servo0_subscriber.cpp new file mode 100644 index 0000000..3c9595d --- /dev/null +++ b/src/microros/subscribers/servo0/servo0_subscriber.cpp @@ -0,0 +1,45 @@ +#include "servo0_subscriber.hpp" + +#include +#include + +#include "devices.hpp" + + +std_msgs__msg__Float32 servo_msg; + +const uint32_t min_pulse_ns = 500000; +const uint32_t max_pulse_ns = 2500000; + + +void servo_callback(const void *msgin){ + const std_msgs__msg__Float32 *msg = (const std_msgs__msg__Float32 *)msgin; + + float angle = msg->data; + + // Clamp angle to valid range + if (angle < 0.0f) angle = 0.0f; + if (angle > 180.0f) angle = 180.0f; + + uint32_t pulse_ns = min_pulse_ns + (angle / 180.0f) * (max_pulse_ns - min_pulse_ns); + + printk("Set servo to %d.%d° = %d.%d ms°\n", int(angle), int(angle * 100) % 100, pulse_ns / 1000000, (pulse_ns / 1000) % 1000); + + int ret = pwm_set_pulse_dt(&servo0, pulse_ns); + + if (ret == 0) { + printk("PWM pulse set successfully.\n"); + } else { + printk("Failed to set PWM pulse! Error: %d\n", ret); + } +} + + +void servo0_register(MicrorosExecutor* node) { + node->add_subscription( + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32), + "/nucleo/servo0", + &servo_msg, + servo_callback + ); +} diff --git a/src/microros/subscribers/servo0/servo0_subscriber.hpp b/src/microros/subscribers/servo0/servo0_subscriber.hpp new file mode 100644 index 0000000..4c00dd5 --- /dev/null +++ b/src/microros/subscribers/servo0/servo0_subscriber.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include "microros_node.hpp" + +void servo0_register(MicrorosExecutor* node);