diff --git a/Inc/firmware/microros_allocators.hpp b/Inc/firmware/microros_allocators.hpp
index 363570a..f41cfa5 100644
--- a/Inc/firmware/microros_allocators.hpp
+++ b/Inc/firmware/microros_allocators.hpp
@@ -3,11 +3,12 @@
 #include <cstdint>
 #include <cstdlib>
 
-uint32_t max_used_heap();
-void free_all_heap();
+uint32_t heap_get_current_pointer();
+void heap_free_all();
+void heap_set_current_pointer(uint32_t pointer);
 
 void* microros_allocate(size_t size, void* state);
 void microros_deallocate(void* pointer, void* state);
 void* microros_reallocate(void* pointer, size_t size, void* state);
 void* microros_zero_allocate(size_t number_of_elements, size_t size_of_element,
-                           void* state);
+                             void* state);
diff --git a/Src/firmware/mainf.cpp b/Src/firmware/mainf.cpp
index 7a93cee..f43b931 100644
--- a/Src/firmware/mainf.cpp
+++ b/Src/firmware/mainf.cpp
@@ -72,6 +72,8 @@ static std::atomic_bool publish_param_trigger(true);
 static bool mecanum_wheels = false;
 static std::atomic_bool controller_replacement(false);
 
+static uint32_t reset_pointer_position;
+
 #define WHEEL_WRAPPER(NAME)                         \
   constexpr const char* NAME##_cmd_pwm_topic =      \
       "~/wheel_" #NAME "/cmd_pwm_duty";             \
@@ -367,7 +369,7 @@ static void finiROS() {
   (void)!rcl_init_options_fini(&init_options);
   rclc_support_fini(&support);
 
-  free_all_heap();
+  heap_free_all();
 }
 
 static uint8_t uart_rbuffer[2048];
@@ -401,6 +403,7 @@ void setup() {
 
 void initController() {
   mecanum_wheels = params.mecanum_wheels;
+  reset_pointer_position = heap_get_current_pointer();
   if (mecanum_wheels) {
     rclc_publisher_init_best_effort(
         &wheel_odom_mecanum_pub, &node,
@@ -424,6 +427,8 @@ void finiController() {
   } else {
     (void)!rcl_publisher_fini(&wheel_odom_pub, &node);
   }
+  controller->~RobotController();
+  heap_set_current_pointer(reset_pointer_position);
 }
 
 void loop() {
diff --git a/Src/firmware/microros_allocators.cpp b/Src/firmware/microros_allocators.cpp
index e690b69..64dd9ea 100644
--- a/Src/firmware/microros_allocators.cpp
+++ b/Src/firmware/microros_allocators.cpp
@@ -6,14 +6,18 @@
 static uint8_t heap[UROS_HEAP_SIZE];
 static size_t current_pointer = 0;
 
-uint32_t max_used_heap() {
+uint32_t heap_get_current_pointer() {
   return current_pointer;
 }
 
-void free_all_heap() {
+void heap_free_all() {
   current_pointer = 0;
 }
 
+void heap_set_current_pointer(uint32_t pointer) {
+  current_pointer = pointer;
+}
+
 void assert_position() {
   if (current_pointer >= sizeof(heap)) {
     while (1) {
@@ -56,7 +60,7 @@ void *microros_reallocate(void *pointer, size_t size, void *state) {
 }
 
 void *microros_zero_allocate(size_t number_of_elements, size_t size_of_element,
-                           void *state) {
+                             void *state) {
   size_t size = number_of_elements * size_of_element;
 
   if (size % 4 != 0) {
diff --git a/platformio.ini b/platformio.ini
index e001e05..5219d5a 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -25,7 +25,7 @@ board_microros_distro = humble-fictionlab
 board_microros_transport = serial
 board_microros_user_meta = colcon.meta
 lib_deps =
-	https://github.com/fictionlab/diff_drive_lib.git#1.5
+	https://github.com/fictionlab/diff_drive_lib.git#1.6
 	https://github.com/fictionlab/micro_ros_platformio#fictionlab-v1
 lib_compat_mode = off