diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..b4428e5
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,17 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c11",
+            "cppStandard": "gnu++14",
+            "intelliSenseMode": "linux-gcc-x64",
+            "configurationProvider": "ms-vscode.makefile-tools"
+        }
+    ],
+    "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/launch.json b/.vscode/launch.json
index 2dca7a7..4f80306 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -1,19 +1,66 @@
 {
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
     "configurations": [
         {
-            "name": "(gdb) Launch",
+            "name": "ESP-IDF Debug: Launch",
+            "type": "espidf",
+            "request": "launch",
+            "executable": "${workspaceRoot}/microros_ws/assemblies/mptest/build/mptest.elf",
+            "target": ":3333",
+            "remote": true,
+            "cwd": "${workspaceRoot}/microros_ws/assemblies/mptest",
+        },
+        {
+            "type": "gdb",
+            "request": "launch",
+            "name": "Launch Program",
+            "target": "${workspaceFolder}/microros_ws/assemblies/mptest/build/mptest.elf",
+            "cwd": "${workspaceFolder}/microros_ws/assemblies/mptest",
+            "gdbpath": "/opt/esp/tools/xtensa-esp32-elf/esp-2021r2-patch3-8.4.0/xtensa-esp32-elf/bin/xtensa-esp32-elf-gdb",
+            "autorun": 
+            [
+                "target remote :3333"
+            ]
+        },
+        {
+            "name": "OpenOCD Debug",
             "type": "cppdbg",
             "request": "launch",
-            "program": "${workspaceFolder}/microros_ws/assemblies/esp32-range-buggy/build/range_buggy.elf",
-            "cwd": "${workspaceFolder}/microros_ws/assemblies/esp32-range-buggy/",
+            "gdbpath": "/opt/esp/tools/xtensa-esp32-elf/esp-2021r2-patch3-8.4.0/xtensa-esp32-elf/bin/xtensa-esp32-elf-gdb",
             "miDebuggerPath": "/opt/esp/tools/xtensa-esp32-elf/esp-2021r2-patch3-8.4.0/xtensa-esp32-elf/bin/xtensa-esp32-elf-gdb",
+            "program": "${workspaceFolder}/microros_ws/assemblies/mptest/build/mptest.elf",
+
             "setupCommands": [
-                { "text": "target remote 127.0.0.1:3333"},
-                { "text": "set remote hardware-watchpoint-limit 2"},
-                { "text": "monitor reset halt"},
-                { "text": "flushregs"}
-            ]
+                {
+                    "description": "enable pretty printing for gdb",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                },
+                {
+                    "text": "file '${workspaceFolder}/microros_ws/assemblies/mptest/build/mptest.elf'"
+                },
+                {
+                    "text": "target remote :3333"
+                },
+                {
+                    "text": "set remote hardware-watchpoint-limit 2",
+                },
+                {
+                    "text": "flushregs"
+                },
+                {
+                    "text": "monitor reset halt"
+                },
+                {
+                    "text": "thb app_main"
+                }
+            ],
+            "cwd": "${workspaceFolder}/microros_ws/assemblies/mptest",
+            "externalConsole": false
         }
-    ],
-    "version": "2.0.0"
+
+    ]
 }
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 0db5873..9ac8041 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,3 +1,78 @@
 {
-    "cmake.configureOnOpen": true
+    "files.associations": {
+        "uros_network_interfaces.h": "c",
+        "microbot.h": "c",
+        "rclc.h": "c",
+        "ros_subscriptionspec.h": "c",
+        "string": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "cctype": "cpp",
+        "chrono": "cpp",
+        "cinttypes": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "codecvt": "cpp",
+        "condition_variable": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "set": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "typeinfo": "cpp",
+        "ringbuf.h": "c",
+        "objstr.h": "c",
+        "mpversion.h": "c",
+        "moduplatform.h": "c",
+        "mpconfig.h": "c",
+        "mphal.h": "c",
+        "mphalport.h": "c"
+    },
+    "cmake.configureOnOpen": false,
+    "cmake.sourceDirectory": "${workspaceFolder}/assemblies/esp32-joystick"
 }
\ No newline at end of file
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index 1b284a2..977afaa 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -1,76 +1,118 @@
 // ======================== tasks.json ========================
 {
-    // See https://go.microsoft.com/fwlink/?LinkId=733558
-    // for the documentation about the tasks.json format
-    "version": "2.0.0",
-    "tasks": [{
-        "label": "Configure",
-        "type": "shell",
-        "command": "source /opt/esp/idf/export.sh && idf.py menuconfig",
-        "windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py menuconfig" }
-      },
-      {
-        "label": "Build",
-        "group": "build",
-        "type": "shell",
-        "command": "idf.py build",
-      },
-      {
-        "label": "Get size",
-        "group": "build",
-        "type": "shell",
-        "command": "idf.py size",
-      },
-      {
-        "label": "Clean",
-        "group": "build",
-        "type": "shell",
-        "command": "source /opt/esp/idf/export.sh && idf.py clean",
-      },
-      {
-        "label": "Clean (full)",
-        "group": "build",
-        "type": "shell",
-        "command": "source /opt/esp/idf/export.sh && idf.py fullclean",
-      },
-      {
-        "label": "Flash",
-        "group": "test",
-        "type": "shell",
-        "command": "source /opt/esp/idf/export.sh && idf.py fullclean",
-      },
-      {
-        "label": "Erase flash",
-        "group": "build",
-        "type": "shell",
-        "command": ". $HOME/esp/esp-idf/export.sh && idf.py erase_flash",
-        "windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py erase_flash" }
-      },
-      {
-        "label": "Monitor",
-        "group": "test",
-        "type": "shell",
-        "command": "idf.py monitor",
-        "isBackground": false,
-        //"dependsOn": ["Flash"]
-      },
-      {
-      "label":  "OpenOCD",
-        "type": "shell",
-        "command": ". /opt/esp/idf/export.sh && openocd -f interface/ftdi/esp32_devkitj_v1.cfg -f board/esp32-wrover.cfg",
-        "isBackground": true,
-        "problemMatcher": {
-          "pattern": {
-            "regexp": "^(Info |Warn ):(.*)$", // Ignore errors
-            "severity": 1,
-            "message": 2
-          },
-          "background": {
-            "activeOnStart": false,
-            "beginsPattern": ".",
-            "endsPattern": "Info : Listening on port \\d+ for gdb connections"
-          }
-        }
-      }
-    ]
-  }
+	// See https://go.microsoft.com/fwlink/?LinkId=733558
+	// for the documentation about the tasks.json format
+	"version": "2.0.0",
+	"tasks": [{
+		"label": "Configure",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py menuconfig",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py menuconfig" }
+	  },
+	  {
+		"label": "Build",
+		"group": "build",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py build",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py build" },
+		"problemMatcher": {
+		  "owner": "cpp",
+		  "fileLocation": ["relative", "${workspaceFolder}/build"],
+		  "pattern": {
+			"regexp": "^(.*):(\\d+):(\\d+):\\s+(warning|error):\\s+(.*)$",
+			"file": 1,
+			"line": 2,
+			"column": 3,
+			"severity": 4,
+			"message": 5
+		  }
+		}
+	  },
+	  {
+		"label": "Get size",
+		"group": "build",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py size",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py size" },
+		"problemMatcher": {
+		  "owner": "cpp",
+		  "fileLocation": ["relative", "${workspaceFolder}/build"],
+		  "pattern": {
+			"regexp": "^(.*):(\\d+):(\\d+):\\s+(warning|error):\\s+(.*)$",
+			"file": 1,
+			"line": 2,
+			"column": 3,
+			"severity": 4,
+			"message": 5
+		  }
+		}
+	  },
+	  {
+		"label": "Clean",
+		"group": "build",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py clean",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py clean" }
+	  },
+	  {
+		"label": "Clean (full)",
+		"group": "build",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py fullclean",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py fullclean" }
+	  },
+	  {
+		"label": "Flash",
+		"group": "test",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py flash",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py flash" },
+		"problemMatcher": {
+		  "owner": "cpp",
+		  "fileLocation": ["relative", "${workspaceFolder}/build"],
+		  "pattern": {
+			"regexp": "^(.*):(\\d+):(\\d+):\\s+(warning|error):\\s+(.*)$",
+			"file": 1,
+			"line": 2,
+			"column": 3,
+			"severity": 4,
+			"message": 5
+		  }
+		}
+	  },
+	  {
+		"label": "Erase flash",
+		"group": "build",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py erase_flash",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py erase_flash" }
+	  },
+	  {
+		"label": "Monitor",
+		"group": "test",
+		"type": "shell",
+		"command": ". $HOME/esp/esp-idf/export.sh && idf.py monitor",
+		"windows": { "command": "%userprofile%\\esp\\esp-idf\\export.bat && idf.py monitor" },
+		"isBackground": true,
+		//"dependsOn": ["Flash"]
+	  },
+	  {
+		"label": "OpenOCD",
+		"type": "shell",
+		"command": ". /opt/esp/idf/export.sh && openocd -f interface/ftdi/esp32_devkitj_v1.cfg -f  board/esp32-wrover-kit-1.8v.cfg",
+		"isBackground": true,
+		"problemMatcher": {
+		  "pattern": {
+			"regexp": "^(Info |Warn ):(.*)$", // Ignore errors
+			"severity": 1,
+			"message": 2
+		  },
+		  "background": {
+			"activeOnStart": true,
+			"beginsPattern": ".",
+			"endsPattern": "Info : Listening on port \\d+ for gdb connections"
+		  }
+		}
+	  }
+	]
+  }
\ No newline at end of file
diff --git a/microros_ws/.gitignore b/microros_ws/.gitignore
index 6945aae..1e1b6b2 100644
--- a/microros_ws/.gitignore
+++ b/microros_ws/.gitignore
@@ -1,14 +1,12 @@
 micro_ros_dev
 micro_ros_src
-esp32_toolchain.*
 *.a
 *.out
-./include
+
 archive
+Archive/**
+
 build
 .images
-*~
-Archive/**
-libmicroros.sym
 
-libs/microros
+
diff --git a/microros_ws/extra_packages/microros/.gitignore b/microros_ws/extra_packages/microros/.gitignore
index 530234e..358e990 100644
--- a/microros_ws/extra_packages/microros/.gitignore
+++ b/microros_ws/extra_packages/microros/.gitignore
@@ -1 +1,4 @@
-**/
+esp32_toolchain.cmake
+include/**
+libmicroros.a
+
diff --git a/microros_ws/extra_packages/microros/esp32_toolchain.cmake.in b/microros_ws/extra_packages/microros/esp32_toolchain.cmake.in
new file mode 100644
index 0000000..c14e1b8
--- /dev/null
+++ b/microros_ws/extra_packages/microros/esp32_toolchain.cmake.in
@@ -0,0 +1,145 @@
+message("\r\n Inside esp32 toolchain cmake")
+
+include(CMakeForceCompiler)
+
+set(CMAKE_SYSTEM_NAME Generic)
+
+set(idf_target "@IDF_TARGET@")
+set(idf_path "@IDF_PATH@")
+
+if("${idf_target}" STREQUAL "esp32c3")
+    set(CMAKE_SYSTEM_PROCESSOR riscv)
+else()
+    set(CMAKE_SYSTEM_PROCESSOR xtensa)
+endif()
+
+set(CMAKE_CROSSCOMPILING 1)
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
+set(PLATFORM_NAME "LwIP")
+
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+
+set(CMAKE_C_COMPILER @CMAKE_C_COMPILER@)
+set(CMAKE_CXX_COMPILER @CMAKE_CXX_COMPILER@)
+
+set(CMAKE_C_FLAGS_INIT "@CFLAGS@" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS_INIT "@CXXFLAGS@" CACHE STRING "" FORCE)
+
+add_definitions(-DESP_PLATFORM -DLWIP_IPV4 -DLWIP_IPV6 -DPLATFORM_NAME_FREERTOS)
+
+include_directories(
+        "@BUILD_CONFIG_DIR@"
+        ${idf_path}/components/newlib/platform_include
+        ${idf_path}/components/freertos/include
+        ${idf_path}/components/freertos/${CMAKE_SYSTEM_PROCESSOR}/include
+        ${idf_path}/components/freertos/port/${CMAKE_SYSTEM_PROCESSOR}/include
+        ${idf_path}/components/esp_hw_support/include
+        ${idf_path}/components/hal/include
+        ${idf_path}/components/hal/${idf_target}/include
+        ${idf_path}/components/heap/include
+        ${idf_path}/components/log/include
+        ${idf_path}/components/lwip/include/apps
+        ${idf_path}/components/lwip/include/apps/sntp
+        ${idf_path}/components/lwip/lwip/src/include
+        ${idf_path}/components/lwip/port/esp32/include
+        ${idf_path}/components/lwip/port/esp32/include/arch
+        ${idf_path}/components/lwip/port/esp32/tcp_isn
+        ${idf_path}/components/soc/src/${idf_target}/.
+        ${idf_path}/components/soc/src/${idf_target}/include
+        ${idf_path}/components/soc/include
+        ${idf_path}/components/soc/${idf_target}/include
+        ${idf_path}/components/esp_rom/include
+        ${idf_path}/components/esp_common/include
+        ${idf_path}/components/esp_system/include
+        ${idf_path}/components/${CMAKE_SYSTEM_PROCESSOR}/include
+        ${idf_path}/components/${CMAKE_SYSTEM_PROCESSOR}/${idf_target}/include
+        ${idf_path}/components/${idf_target}/include
+        ${idf_path}/components/driver/include
+        ${idf_path}/components/driver/${idf_target}/include
+        ${idf_path}/components/esp_ringbuf/include
+        ${idf_path}/components/efuse/include
+        ${idf_path}/components/efuse/${idf_target}/include
+        ${idf_path}/components/espcoredump/include
+        ${idf_path}/components/esp_timer/include
+        ${idf_path}/components/esp_ipc/include
+        ${idf_path}/components/soc/soc/${idf_target}/include
+        ${idf_path}/components/soc/soc/${idf_target}/../include
+        ${idf_path}/components/soc/soc/${idf_target}/private_include
+        ${idf_path}/components/vfs/include
+        ${idf_path}/components/esp_wifi/include
+        ${idf_path}/components/esp_wifi/${idf_target}/include
+        ${idf_path}/components/esp_event/include
+        ${idf_path}/components/esp_netif/include
+        ${idf_path}/components/esp_eth/include
+        ${idf_path}/components/tcpip_adapter/include
+        ${idf_path}/components/app_trace/include
+        ${idf_path}/components/mbedtls/port/include
+        ${idf_path}/components/mbedtls/mbedtls/include
+        ${idf_path}/components/mbedtls/esp_crt_bundle/include
+        ${idf_path}/components/bootloader_support/include
+        ${idf_path}/components/app_update/include
+        ${idf_path}/components/spi_flash/include
+        ${idf_path}/components/wpa_supplicant/include
+        ${idf_path}/components/wpa_supplicant/port/include
+        ${idf_path}/components/wpa_supplicant/include/esp_supplicant
+        ${idf_path}/components/nvs_flash/include
+        ${idf_path}/components/pthread/include
+        ${idf_path}/components/perfmon/include
+        ${idf_path}/components/asio/asio/asio/include
+        ${idf_path}/components/asio/port/include
+        ${idf_path}/components/cbor/port/include
+        ${idf_path}/components/coap/port/include
+        ${idf_path}/components/coap/port/include/coap
+        ${idf_path}/components/coap/libcoap/include
+        ${idf_path}/components/coap/libcoap/include/coap2
+        ${idf_path}/components/console
+        ${idf_path}/components/nghttp/port/include
+        ${idf_path}/components/nghttp/nghttp2/lib/includes
+        ${idf_path}/components/esp-tls
+        ${idf_path}/components/esp_adc_cal/include
+        ${idf_path}/components/esp_gdbstub/include
+        ${idf_path}/components/esp_hid/include
+        ${idf_path}/components/tcp_transport/include
+        ${idf_path}/components/esp_http_client/include
+        ${idf_path}/components/esp_http_server/include
+        ${idf_path}/components/esp_https_ota/include
+        ${idf_path}/components/protobuf-c/protobuf-c
+        ${idf_path}/components/protocomm/include/common
+        ${idf_path}/components/protocomm/include/security
+        ${idf_path}/components/protocomm/include/transports
+        ${idf_path}/components/mdns/include
+        ${idf_path}/components/esp_local_ctrl/include
+        ${idf_path}/components/sdmmc/include
+        ${idf_path}/components/esp_serial_slave_link/include
+        ${idf_path}/components/esp_websocket_client/include
+        ${idf_path}/components/expat/expat/expat/lib
+        ${idf_path}/components/expat/port/include
+        ${idf_path}/components/wear_levelling/include
+        ${idf_path}/components/fatfs/diskio
+        ${idf_path}/components/fatfs/vfs
+        ${idf_path}/components/fatfs/src
+        ${idf_path}/components/freemodbus/common/include
+        ${idf_path}/components/idf_test/include
+        ${idf_path}/components/idf_test/include/${idf_target}
+        ${idf_path}/components/jsmn/include
+        ${idf_path}/components/json/cJSON
+        ${idf_path}/components/libsodium/libsodium/src/libsodium/include
+        ${idf_path}/components/libsodium/port_include
+        ${idf_path}/components/mqtt/esp-mqtt/include
+        ${idf_path}/components/openssl/include
+        ${idf_path}/components/spiffs/include
+        ${idf_path}/components/ulp/include
+        ${idf_path}/components/unity/include
+        ${idf_path}/components/unity/unity/src
+        ${idf_path}/components/wifi_provisioning/include
+
+        ${idf_path}/components/freertos/include/esp_additions/freertos
+        ${idf_path}/components/freertos/include/esp_additions
+        ${idf_path}/components/esp_hw_support/include
+        ${idf_path}/components/esp_hw_support/include/soc
+        ${idf_path}/components/esp_hw_support/include/soc/${idf_target}
+        ${idf_path}/components/esp_hw_support/port/${idf_target}/.
+
+    )