Minor changes to Pico firmware.

Source Code/pico_ws/.devcontainer/uros_agent/uros_agent_run.sh

@@ -6,4 +6,4 @@
 
 cd ~/pico_ws/uros_agent \
 && source install/local_setup.bash \
-&& ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -v 4
+&& ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/$1 -v 4

Source Code/pico_ws/.devcontainer/vscode_config/settings.json

@@ -12,6 +12,7 @@
     "cmake.buildDirectory": "${workspaceFolder}/build",
     "cmake.buildBeforeRun": true,
     "cmake.configureOnOpen": true,
+    "cmake.deleteBuildDirOnCleanConfigure": true,
     "cortex-debug.openocdPath": "openocd",
 
     "cmake.options.statusBarVisibility": "hidden",

Source Code/pico_ws/libmicroros/src/remote_pico_coms/CMakeLists.txt

@@ -46,6 +46,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
                             "msg/ButtonStates.msg"
+                            "msg/SwitchStates.msg"
                             "msg/JoystickState.msg"
                             "msg/PotentiometerState.msg"
                             "srv/GetJoystickConfig.srv"

Source Code/pico_ws/libmicroros/src/remote_pico_coms/msg/ButtonStates.msg

@@ -1,5 +1,5 @@
 #  The ROS remote project - Raspberry Pi Pico Communication Messages Package
-#  Button & switch states message.
+#  Momentary button states message.
 #  Copyright 2024 Samyar Sadat Akhavi
 #  Written by Samyar Sadat Akhavi, 2024.
 #
@@ -18,15 +18,7 @@
 
 builtin_interfaces/Time time
 
-
-# ---- Toggle switches ----
-bool right_top_toggle_sw
-bool left_key_sw
-bool left_top_toggle_sw
-
 # ---- Buttons ----
-bool right_e_stop_btn
-bool right_kd2_btn
 bool left_green_right_btn
 bool left_red_btn
 bool left_green_kd2_btn

Source Code/pico_ws/libmicroros/src/remote_pico_coms/msg/SwitchStates.msg

@@ -0,0 +1,29 @@
+#  The ROS remote project - Raspberry Pi Pico Communication Messages Package
+#  Permanent switch states message.
+#  Copyright 2024 Samyar Sadat Akhavi
+#  Written by Samyar Sadat Akhavi, 2024.
+#
+#  This program is free software: you can redistribute it and/or modify
+#  it under the terms of the GNU General Public License as published by
+#  the Free Software Foundation, either version 3 of the License, or
+#  (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#
+#  You should have received a copy of the GNU General Public License
+#  along with this program.  If not, see <https: www.gnu.org/licenses/>.
+
+builtin_interfaces/Time time
+
+
+# ---- Toggle switches ----
+bool right_top_toggle_sw
+bool left_key_sw
+bool left_top_toggle_sw
+
+# ---- Buttons ----
+bool right_e_stop_btn
+bool right_kd2_btn

Source Code/pico_ws/src/pico/Pico.c++

@@ -33,11 +33,12 @@
 alarm_pool_t *core_1_alarm_pool;
 
 // ---- Timer execution times storage (milliseconds) ----
-uint32_t last_btn_state_publish_time, last_joystick_state_publish_time, last_potentiometer_state_publish_time;
+uint32_t last_sw_state_publish_time, last_btn_state_publish_time;
+uint32_t last_joystick_state_publish_time, last_potentiometer_state_publish_time;
 
 // ---- Timers ----
-struct repeating_timer btn_state_publish_rt, joystick_publish_rt, potentiometer_publish_rt;
-TaskHandle_t btn_state_publish_th, joystick_publish_th, potentiometer_publish_th;
+struct repeating_timer sw_state_publish_rt, btn_state_publish_rt, joystick_publish_rt, potentiometer_publish_rt;
+TaskHandle_t btn_state_publish_th, sw_state_publish_th, joystick_publish_th, potentiometer_publish_th;
 TimerHandle_t waiting_for_agent_timer, fast_led_flash_handler_timer;
 TimerHandle_t slow_led_flash_handler_timer, led_fade_handler_timer;
 
@@ -59,6 +60,7 @@ void clean_shutdown()
     write_log("A clean shutdown has been triggered. The program will now shut down.", LOG_LVL_FATAL, FUNCNAME_ONLY);
 
     // Stop all repeating timers
+    cancel_repeating_timer(&sw_state_publish_rt);
     cancel_repeating_timer(&btn_state_publish_rt);
     cancel_repeating_timer(&joystick_publish_rt);
     cancel_repeating_timer(&potentiometer_publish_rt);
@@ -101,6 +103,14 @@ void vApplicationMallocFailedHook()
 
 
 // ---- Timer callbacks for task notification ----
+bool publish_sw_state_notify(struct repeating_timer *rt)
+{
+    BaseType_t higher_prio_woken;
+    vTaskNotifyGiveFromISR(sw_state_publish_th, &higher_prio_woken);
+    portYIELD_FROM_ISR(higher_prio_woken);
+    return true;
+}
+
 bool publish_btn_state_notify(struct repeating_timer *rt)
 {
     BaseType_t higher_prio_woken;
@@ -126,14 +136,6 @@ bool publish_potentiometer_notify(struct repeating_timer *rt)
 }
 
 
-// ---- IRQ callback ----
-void irq_call(uint pin, uint32_t events)
-{
-    publish_btn_state_notify(NULL);
-}
-
-
-
 // ------- MicroROS subscriber & service callbacks ------- 
 
 // ---- Get/set joystick configuration services ----
@@ -236,6 +238,7 @@ void uros_post_exec_call()
 void start_timers()
 {
     write_log("Starting hardware timers...", LOG_LVL_INFO, FUNCNAME_ONLY);
+    alarm_pool_add_repeating_timer_ms(core_1_alarm_pool, sw_state_pub_rt_interval, publish_sw_state_notify, NULL, &sw_state_publish_rt);
     alarm_pool_add_repeating_timer_ms(core_1_alarm_pool, btn_state_pub_rt_interval, publish_btn_state_notify, NULL, &btn_state_publish_rt);
     alarm_pool_add_repeating_timer_ms(core_1_alarm_pool, joystick_pub_rt_interval, publish_joystick_notify, NULL, &joystick_publish_rt);
     alarm_pool_add_repeating_timer_ms(core_1_alarm_pool, potentiometer_pub_rt_interval, publish_potentiometer_notify, NULL, &potentiometer_publish_rt);
@@ -307,23 +310,6 @@ void setup(void *parameters)
     init_pin(joystick_x_axis_pin, INPUT_ADC);
     init_pin(potentiometer_pin, INPUT_ADC);
 
-    // Interrupts
-    gpio_set_irq_enabled_with_callback(right_top_toggle_sw_pin, GPIO_IRQ_EDGE_FALL, true, irq_call);
-    gpio_set_irq_enabled(left_key_sw_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(left_top_toggle_sw_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(right_e_stop_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(right_kd2_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(left_green_right_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(left_red_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(left_green_kd2_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(left_red_kd2_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(left_green_left_btn_pin, GPIO_IRQ_EDGE_FALL, true);
-    gpio_set_irq_enabled(right_top_toggle_sw_pin, GPIO_IRQ_EDGE_RISE, true);
-    gpio_set_irq_enabled(left_key_sw_pin, GPIO_IRQ_EDGE_RISE, true);
-    gpio_set_irq_enabled(left_top_toggle_sw_pin, GPIO_IRQ_EDGE_RISE, true);
-    gpio_set_irq_enabled(right_e_stop_btn_pin, GPIO_IRQ_EDGE_RISE, true);
-    gpio_set_irq_enabled(right_kd2_btn_pin, GPIO_IRQ_EDGE_RISE, true);
-
     // Force SMPS into PWM mode
     init_pin(smps_power_save_pin, OUTPUT);
     gpio_put(smps_power_save_pin, HIGH);
@@ -337,10 +323,12 @@ void setup(void *parameters)
     write_log("Creating timer tasks...", LOG_LVL_INFO, FUNCNAME_ONLY);
     xTaskCreate(publish_joystick_state, "joystick_publish", TIMER_TASK_STACK_DEPTH, NULL, configMAX_PRIORITIES - 3, &joystick_publish_th);
     xTaskCreate(publish_potentiometer_state, "potentiometer_publish", TIMER_TASK_STACK_DEPTH, NULL, configMAX_PRIORITIES - 4, &potentiometer_publish_th);
-    xTaskCreate(publish_btn_states, "btn_states_publish", TIMER_TASK_STACK_DEPTH, NULL, configMAX_PRIORITIES - 5, &btn_state_publish_th);
-    vTaskCoreAffinitySet(joystick_publish_th, (1 << 1));        // Lock task to core 1
+    xTaskCreate(publish_btn_states, "btn_states_publish", TIMER_TASK_STACK_DEPTH, NULL, configMAX_PRIORITIES - 4, &btn_state_publish_th);
+    xTaskCreate(publish_sw_states, "sw_states_publish", TIMER_TASK_STACK_DEPTH, NULL, configMAX_PRIORITIES - 4, &sw_state_publish_th);
+    //vTaskCoreAffinitySet(joystick_publish_th, (1 << 1));      // Lock task to core 1
     vTaskCoreAffinitySet(potentiometer_publish_th, (1 << 1));   // Lock task to core 1
     vTaskCoreAffinitySet(btn_state_publish_th, (1 << 1));       // Lock task to core 1
+    vTaskCoreAffinitySet(sw_state_publish_th, (1 << 1));        // Lock task to core 1
 
     // Create FreeRTOS timers
     write_log("Creating FreeRTOS software timers...", LOG_LVL_INFO, FUNCNAME_ONLY);
@@ -380,7 +368,7 @@ void setup1(void *parameters)
 // ******** END OF MAIN PROGRAM *********
 // *********** STARTUP & INIT ***********
 
-// ---- Main function (Runs setup, loop, and loop1) ----
+// ---- Entrypoint ----
 int main() 
 {
     // UART & USB STDIO outputs

Source Code/pico_ws/src/pico/helpers/Definitions.h

@@ -48,7 +48,7 @@
 #define right_kd2_btn_pin         5
 #define left_green_right_btn_pin  6
 #define left_red_btn_pin          7
-#define left_green_kd2_btn_pin   20
+#define left_green_kd2_btn_pin    20
 #define left_red_kd2_btn_pin      21
 #define left_green_left_btn_pin   22
 
@@ -67,13 +67,14 @@
 
 // ---- MicroROS node config ----
 #define UROS_NODE_NAME                     "pico"
-#define UROS_NODE_NAMESPACE                "remote_io"
+#define UROS_NODE_NAMESPACE                ""
 #define UROS_DOMAIN_ID                     75
 #define AGENT_WAITING_LED_TOGGLE_DELAY_MS  500   // In milliseconds
 #define AGENT_AVAIL_LED_TOGGLE_DELAY_MS    250   // In milliseconds
 
 // ---- Repeating timer intervals ----
-#define btn_state_pub_rt_interval      500   // In milliseconds
+#define sw_state_pub_rt_interval       100   // In milliseconds
+#define btn_state_pub_rt_interval      50    // In milliseconds
 #define joystick_pub_rt_interval       50    // In milliseconds
 #define potentiometer_pub_rt_interval  80    // In milliseconds
 #define led_slow_flash_interval        800   // In milliseconds

Source Code/pico_ws/src/pico/helpers/IO_Helpers_General.c++

@@ -63,7 +63,7 @@ int16_t calc_joystick_reading(uint16_t adc_reading, bool inverted, float center_
     // Inversion
     if (inverted)
     {
-        adc_reading = map(adc_reading, 0, 4095, 4095, 0);
+        adc_reading = 4095 - adc_reading;
     }
 
     // Center offset
@@ -152,12 +152,12 @@ uint16_t get_potentiometer_val()
 
         if (!potentiometer_inverted)
         {
-            return map(adc_reading, 0, 4095, 0, 1024);
+            return (uint16_t) map(adc_reading, 0, 4095, 0, 1024);
         }
 
         else
         {
-            return map(adc_reading, 0, 4095, 1024, 0);
+            return (uint16_t) map(adc_reading, 0, 4095, 1024, 0);
         }
     }
 

Source Code/pico_ws/src/pico/helpers/Sensor_Publishers.c++

@@ -34,21 +34,52 @@
 extern uRosPublishingHandler *pub_handler;
 
 // ---- Timer execution times storage (milliseconds) ----
-extern uint32_t last_btn_state_publish_time, last_joystick_state_publish_time, last_potentiometer_state_publish_time;
+extern uint32_t last_sw_state_publish_time, last_btn_state_publish_time;
+extern uint32_t last_joystick_state_publish_time, last_potentiometer_state_publish_time;
 
 
 
 // ------- Functions ------- 
 
-// ---- Button states ----
+// ---- Permanent switch states ----
+void publish_sw_states(void *parameters)
+{
+    while (true)
+    {
+        xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);   // Wait for notification indefinitely
+
+        // Check execution time
+        check_exec_interval(last_sw_state_publish_time, (sw_state_pub_rt_interval + 15), "Publish interval exceeded limits!", true);
+
+        uint32_t timestamp_sec = to_ms_since_boot(get_absolute_time()) / 1000;
+        uint32_t timestamp_nanosec = (to_ms_since_boot(get_absolute_time()) - (timestamp_sec * 1000)) * 1000000;
+
+        switch_state_msg.time.sec = timestamp_sec;
+        switch_state_msg.time.nanosec = timestamp_nanosec;
+
+        switch_state_msg.left_key_sw = !gpio_get(left_key_sw_pin);
+        switch_state_msg.left_top_toggle_sw = !gpio_get(left_top_toggle_sw_pin);
+        switch_state_msg.right_e_stop_btn = !gpio_get(right_e_stop_btn_pin);
+        switch_state_msg.right_kd2_btn = !gpio_get(right_kd2_btn_pin);
+        switch_state_msg.right_top_toggle_sw = !gpio_get(right_top_toggle_sw_pin);
+
+        uRosPublishingHandler::PublishItem_t pub_item;
+        pub_item.publisher = &switch_state_pub;
+        pub_item.message = &switch_state_msg;
+        xQueueSendToBack(pub_handler->get_queue_handle(), (void *) &pub_item, 0);
+    }
+}
+
+
+// ---- Momentary button states ----
 void publish_btn_states(void *parameters)
 {
     while (true)
     {
         xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);   // Wait for notification indefinitely
 
         // Check execution time
-        check_exec_interval(last_btn_state_publish_time, (btn_state_pub_rt_interval + 15), "Publish interval exceeded limits!", true);
+        check_exec_interval(last_btn_state_publish_time, (btn_state_pub_rt_interval + 10), "Publish interval exceeded limits!", true);
 
         uint32_t timestamp_sec = to_ms_since_boot(get_absolute_time()) / 1000;
         uint32_t timestamp_nanosec = (to_ms_since_boot(get_absolute_time()) - (timestamp_sec * 1000)) * 1000000;
@@ -59,13 +90,8 @@ void publish_btn_states(void *parameters)
         button_state_msg.left_green_kd2_btn = !gpio_get(left_green_kd2_btn_pin);
         button_state_msg.left_green_left_btn = !gpio_get(left_green_left_btn_pin);
         button_state_msg.left_green_right_btn = !gpio_get(left_green_right_btn_pin);
-        button_state_msg.left_key_sw = gpio_get(left_key_sw_pin);
         button_state_msg.left_red_btn = !gpio_get(left_red_btn_pin);
         button_state_msg.left_red_kd2_btn = !gpio_get(left_red_kd2_btn_pin);
-        button_state_msg.left_top_toggle_sw = !gpio_get(left_top_toggle_sw_pin);
-        button_state_msg.right_e_stop_btn = !gpio_get(right_e_stop_btn_pin);
-        button_state_msg.right_kd2_btn = !gpio_get(right_kd2_btn_pin);
-        button_state_msg.right_top_toggle_sw = !gpio_get(right_top_toggle_sw_pin);
 
         uRosPublishingHandler::PublishItem_t pub_item;
         pub_item.publisher = &button_state_pub;

Source Code/pico_ws/src/pico/helpers/uROS_Init.c++

@@ -42,9 +42,10 @@ std_msgs__msg__Empty e_stop_msg;
 
 // ---- Publishers ----
 
-// Button, joystick, potentiometer states
-rcl_publisher_t button_state_pub, joystick_state_pub, potentiometer_state_pub;
+// Button, switch, joystick, potentiometer states
+rcl_publisher_t button_state_pub, switch_state_pub, joystick_state_pub, potentiometer_state_pub;
 remote_pico_coms__msg__ButtonStates button_state_msg;
+remote_pico_coms__msg__SwitchStates switch_state_msg;
 remote_pico_coms__msg__JoystickState joystick_state_msg;
 remote_pico_coms__msg__PotentiometerState potentiometer_state_msg;
 
@@ -93,6 +94,7 @@ void init_subs_pubs()
 
     const rosidl_message_type_support_t *empty_type = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Empty);
     const rosidl_message_type_support_t *button_state_type = ROSIDL_GET_MSG_TYPE_SUPPORT(remote_pico_coms, msg, ButtonStates);
+    const rosidl_message_type_support_t *switch_state_type = ROSIDL_GET_MSG_TYPE_SUPPORT(remote_pico_coms, msg, SwitchStates);
     const rosidl_message_type_support_t *joystick_state_type = ROSIDL_GET_MSG_TYPE_SUPPORT(remote_pico_coms, msg, JoystickState);
     const rosidl_message_type_support_t *potentiometer_state_type = ROSIDL_GET_MSG_TYPE_SUPPORT(remote_pico_coms, msg, PotentiometerState);
     const rosidl_service_type_support_t *get_joystick_config_type = ROSIDL_GET_SRV_TYPE_SUPPORT(remote_pico_coms, srv, GetJoystickConfig);
@@ -103,10 +105,10 @@ void init_subs_pubs()
 
     // ---- Services ----
     write_log("Initializing services...", LOG_LVL_INFO, FUNCNAME_ONLY);
-    bridge->init_service(&get_joystick_config_srv, get_joystick_config_type, "joystick/get_config");
-    bridge->init_service(&set_joystick_config_srv, set_joystick_config_type, "joystick/set_config");
-    bridge->init_service(&get_led_states_srv, get_led_states_type, "leds/get_states");
-    bridge->init_service(&set_led_states_srv, set_led_states_type, "leds/set_states");
+    bridge->init_service(&get_joystick_config_srv, get_joystick_config_type, "inputs/joystick/get_config");
+    bridge->init_service(&set_joystick_config_srv, set_joystick_config_type, "inputs/joystick/set_config");
+    bridge->init_service(&get_led_states_srv, get_led_states_type, "outputs/leds/get_states");
+    bridge->init_service(&set_led_states_srv, set_led_states_type, "outputs/leds/set_states");
     bridge->init_service(&run_self_test_srv, run_self_test_type, "self_test/pico");
 
 
@@ -125,10 +127,12 @@ void init_subs_pubs()
     diag_uros_init();
 
     // Sensor state topics
-    bridge->init_publisher(&button_state_pub, button_state_type, "buttons/states");
-    bridge->init_publisher(&joystick_state_pub, joystick_state_type, "joystick/state");
-    bridge->init_publisher(&potentiometer_state_pub, potentiometer_state_type, "potentiometer/state");
+    bridge->init_publisher(&button_state_pub, button_state_type, "inputs/buttons");
+    bridge->init_publisher(&switch_state_pub, switch_state_type, "inputs/switches");
+    bridge->init_publisher(&joystick_state_pub, joystick_state_type, "inputs/joystick");
+    bridge->init_publisher(&potentiometer_state_pub, potentiometer_state_type, "inputs/potentiometer");
     remote_pico_coms__msg__ButtonStates__init(&button_state_msg);
+    remote_pico_coms__msg__SwitchStates__init(&switch_state_msg);
     remote_pico_coms__msg__JoystickState__init(&joystick_state_msg);
     remote_pico_coms__msg__PotentiometerState__init(&potentiometer_state_msg);
 

Source Code/pico_ws/src/pico/helpers/uROS_Init.h

@@ -31,6 +31,7 @@
 #include <std_srvs/srv/set_bool.h>
 #include <nav_msgs/msg/odometry.h>
 #include <remote_pico_coms/msg/button_states.h>
+#include <remote_pico_coms/msg/switch_states.h>
 #include <remote_pico_coms/msg/joystick_state.h>
 #include <remote_pico_coms/msg/potentiometer_state.h>
 #include <remote_pico_coms/srv/get_joystick_config.h>
@@ -52,9 +53,10 @@ extern std_msgs__msg__Empty e_stop_msg;
 
 // ---- Publishers ----
 
-// Button, joystick, potentiometer states
-extern rcl_publisher_t button_state_pub, joystick_state_pub, potentiometer_state_pub;
+// Button, switch, joystick, potentiometer states
+extern rcl_publisher_t button_state_pub, switch_state_pub, joystick_state_pub, potentiometer_state_pub;
 extern remote_pico_coms__msg__ButtonStates button_state_msg;
+extern remote_pico_coms__msg__SwitchStates switch_state_msg;
 extern remote_pico_coms__msg__JoystickState joystick_state_msg;
 extern remote_pico_coms__msg__PotentiometerState potentiometer_state_msg;