diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h
index 2b6934b..843aa80 100644
--- a/Core/Inc/FreeRTOSConfig.h
+++ b/Core/Inc/FreeRTOSConfig.h
@@ -69,7 +69,7 @@ extern uint32_t SystemCoreClock;
 #define configUSE_SB_COMPLETED_CALLBACK          ( 0 )
 #define configUSE_MINI_LIST_ITEM                ( 1 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)16*1024)
+#define configTOTAL_HEAP_SIZE                    ((size_t)32768)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configHEAP_CLEAR_MEMORY_ON_FREE          0
 #define configUSE_TRACE_FACILITY                 1
@@ -79,6 +79,7 @@ extern uint32_t SystemCoreClock;
 #define configQUEUE_REGISTRY_SIZE                8
 #define configCHECK_FOR_STACK_OVERFLOW           2
 #define configUSE_RECURSIVE_MUTEXES              1
+#define configUSE_MALLOC_FAILED_HOOK             1
 #define configUSE_COUNTING_SEMAPHORES            1
 #define configUSE_PORT_OPTIMISED_TASK_SELECTION  0
 /* USER CODE BEGIN MESSAGE_BUFFER_LENGTH_TYPE */
diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c
index 6a15ab5..0d3f78d 100644
--- a/Core/Src/freertos.c
+++ b/Core/Src/freertos.c
@@ -53,7 +53,7 @@ osThreadId_t defaultTaskHandle;
 const osThreadAttr_t defaultTask_attributes = {
   .name = "defaultTask",
   .stack_size = 512 * 4,
-  .priority = (osPriority_t) osPriorityNormal,
+  .priority = (osPriority_t) osPriorityBelowNormal7,
 };
 
 /* Private function prototypes -----------------------------------------------*/
@@ -68,6 +68,7 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
 
 /* Hook prototypes */
 void vApplicationStackOverflowHook(xTaskHandle xTask, char *pcTaskName);
+void vApplicationMallocFailedHook(void);
 
 /* USER CODE BEGIN 4 */
 void StackOverflowHook(TaskHandle_t xTask, char *pcTaskName);
@@ -82,6 +83,24 @@ void Drv_InterfaceInit(void);
 void MyTask_Init(void);
 /* USER CODE END 4 */
 
+/* USER CODE BEGIN 5 */
+void MallocFailedHook(void);
+void vApplicationMallocFailedHook(void)
+{
+   MallocFailedHook();
+   /* vApplicationMallocFailedHook() will only be called if
+   configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h. It is a hook
+   function that will get called if a call to pvPortMalloc() fails.
+   pvPortMalloc() is called internally by the kernel whenever a task, queue,
+   timer or semaphore is created. It is also called by various parts of the
+   demo application. If heap_1.c or heap_2.c are used, then the size of the
+   heap available to pvPortMalloc() is defined by configTOTAL_HEAP_SIZE in
+   FreeRTOSConfig.h, and the xPortGetFreeHeapSize() API function can be used
+   to query the size of free heap space that remains (although it does not
+   provide information on how the remaining heap might be fragmented). */
+}
+/* USER CODE END 5 */
+
 /**
   * @brief  FreeRTOS initialization
   * @param  None
diff --git a/robot b/robot
index 793bf2b..c5cc930 160000
--- a/robot
+++ b/robot
@@ -1 +1 @@
-Subproject commit 793bf2b4663df1869ca744b765597e2bf3d9b545
+Subproject commit c5cc930fea45f1c004e5b9d8466c8c2ff5173858
diff --git a/robot_h743.ioc b/robot_h743.ioc
index 0237d54..e351044 100644
--- a/robot_h743.ioc
+++ b/robot_h743.ioc
@@ -210,10 +210,11 @@ FDCAN2.NominalTimeSeg2=2
 FDCAN2.RxFifo0ElmtsNbr=32
 FDCAN2.TxFifoQueueElmtsNbr=32
 FREERTOS.FootprintOK=true
-FREERTOS.IPParameters=Tasks01,configUSE_NEWLIB_REENTRANT,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,configUSE_STATS_FORMATTING_FUNCTIONS,configTOTAL_HEAP_SIZE
-FREERTOS.Tasks01=defaultTask,24,512,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.IPParameters=Tasks01,configUSE_NEWLIB_REENTRANT,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,configUSE_STATS_FORMATTING_FUNCTIONS,configTOTAL_HEAP_SIZE,configUSE_MALLOC_FAILED_HOOK
+FREERTOS.Tasks01=defaultTask,23,512,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configCHECK_FOR_STACK_OVERFLOW=2
-FREERTOS.configTOTAL_HEAP_SIZE=16*1024
+FREERTOS.configTOTAL_HEAP_SIZE=32768
+FREERTOS.configUSE_MALLOC_FAILED_HOOK=1
 FREERTOS.configUSE_NEWLIB_REENTRANT=0
 FREERTOS.configUSE_STATS_FORMATTING_FUNCTIONS=1
 File.Version=6