diff --git a/Src/tim.c b/Src/tim.c
deleted file mode 100644
index a45bd84..0000000
--- a/Src/tim.c
+++ /dev/null
@@ -1,220 +0,0 @@
-/* USER CODE BEGIN Header */
-/**
- ******************************************************************************
- * @file    tim.c
- * @brief   This file provides code for the configuration
- *          of the TIM instances.
- ******************************************************************************
- * @attention
- *
- * Copyright (c) 2024 STMicroelectronics.
- * All rights reserved.
- *
- * This software is licensed under terms that can be found in the LICENSE file
- * in the root directory of this software component.
- * If no LICENSE file comes with this software, it is provided AS-IS.
- *
- ******************************************************************************
- */
-/* USER CODE END Header */
-/* Includes ------------------------------------------------------------------*/
-#include "tim.h"
-
-/* USER CODE BEGIN 0 */
-
-/* USER CODE END 0 */
-
-TIM_HandleTypeDef htim3;
-
-/* TIM1 init function */
-void MX_TIM1_Init(void)
-{
-
-  /* USER CODE BEGIN TIM1_Init 0 */
-
-  /* USER CODE END TIM1_Init 0 */
-
-  LL_TIM_InitTypeDef TIM_InitStruct = {0};
-  LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};
-  LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = {0};
-
-  LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
-  /* Peripheral clock enable */
-  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);
-
-  /* USER CODE BEGIN TIM1_Init 1 */
-
-  /* USER CODE END TIM1_Init 1 */
-  TIM_InitStruct.Prescaler = 0;
-  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_CENTER_UP;
-  TIM_InitStruct.Autoreload = 65535;
-  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
-  TIM_InitStruct.RepetitionCounter = 0;
-  LL_TIM_Init(TIM1, &TIM_InitStruct);
-  LL_TIM_EnableARRPreload(TIM1);
-  LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1);
-  TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1;
-  TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
-  TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE;
-  TIM_OC_InitStruct.CompareValue = 0;
-  TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
-  TIM_OC_InitStruct.OCNPolarity = LL_TIM_OCPOLARITY_HIGH;
-  TIM_OC_InitStruct.OCIdleState = LL_TIM_OCIDLESTATE_LOW;
-  TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW;
-  LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);
-  LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1);
-  LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2);
-  LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct);
-  LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2);
-  LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3);
-  LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct);
-  LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3);
-  LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4);
-  TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_LOW;
-  LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH4, &TIM_OC_InitStruct);
-  LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH4);
-  LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_OC4REF);
-  LL_TIM_DisableMasterSlaveMode(TIM1);
-  TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE;
-  TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE;
-  TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF;
-  TIM_BDTRInitStruct.DeadTime = 0;
-  TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE;
-  TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH;
-  TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE;
-  LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct);
-  /* USER CODE BEGIN TIM1_Init 2 */
-
-  /* USER CODE END TIM1_Init 2 */
-  LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
-    /**TIM1 GPIO Configuration
-    PA8     ------> TIM1_CH1
-    PA9     ------> TIM1_CH2
-    PA10     ------> TIM1_CH3
-    */
-  GPIO_InitStruct.Pin = LL_GPIO_PIN_8|LL_GPIO_PIN_9|LL_GPIO_PIN_10;
-  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
-  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
-  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
-  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
-  GPIO_InitStruct.Alternate = LL_GPIO_AF_1;
-  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-}
-/* TIM2 init function */
-void MX_TIM2_Init(void)
-{
-
-  /* USER CODE BEGIN TIM2_Init 0 */
-
-  /* USER CODE END TIM2_Init 0 */
-
-  LL_TIM_InitTypeDef TIM_InitStruct = {0};
-
-  /* Peripheral clock enable */
-  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
-
-  /* USER CODE BEGIN TIM2_Init 1 */
-
-  /* USER CODE END TIM2_Init 1 */
-  TIM_InitStruct.Prescaler = 0;
-  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
-  TIM_InitStruct.Autoreload = 4294967295;
-  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
-  LL_TIM_Init(TIM2, &TIM_InitStruct);
-  LL_TIM_DisableARRPreload(TIM2);
-  LL_TIM_SetClockSource(TIM2, LL_TIM_CLOCKSOURCE_INTERNAL);
-  LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_UPDATE);
-  LL_TIM_DisableMasterSlaveMode(TIM2);
-  /* USER CODE BEGIN TIM2_Init 2 */
-
-  /* USER CODE END TIM2_Init 2 */
-
-}
-/* TIM3 init function */
-void MX_TIM3_Init(void)
-{
-
-  /* USER CODE BEGIN TIM3_Init 0 */
-
-  /* USER CODE END TIM3_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-
-  /* USER CODE BEGIN TIM3_Init 1 */
-
-  /* USER CODE END TIM3_Init 1 */
-  htim3.Instance = TIM3;
-  htim3.Init.Prescaler = 89;
-  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim3.Init.Period = 99;
-  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM3_Init 2 */
-  if (HAL_TIM_Base_Start_IT(&htim3) != HAL_OK)
-  {
-    /* Starting Error */
-    Error_Handler();
-  }
-  /* USER CODE END TIM3_Init 2 */
-
-}
-
-void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
-{
-
-  if(tim_baseHandle->Instance==TIM3)
-  {
-  /* USER CODE BEGIN TIM3_MspInit 0 */
-
-  /* USER CODE END TIM3_MspInit 0 */
-    /* TIM3 clock enable */
-    __HAL_RCC_TIM3_CLK_ENABLE();
-
-    /* TIM3 interrupt Init */
-    HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
-    HAL_NVIC_EnableIRQ(TIM3_IRQn);
-  /* USER CODE BEGIN TIM3_MspInit 1 */
-
-  /* USER CODE END TIM3_MspInit 1 */
-  }
-}
-
-void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
-{
-
-  if(tim_baseHandle->Instance==TIM3)
-  {
-  /* USER CODE BEGIN TIM3_MspDeInit 0 */
-
-  /* USER CODE END TIM3_MspDeInit 0 */
-    /* Peripheral clock disable */
-    __HAL_RCC_TIM3_CLK_DISABLE();
-
-    /* TIM3 interrupt Deinit */
-    HAL_NVIC_DisableIRQ(TIM3_IRQn);
-  /* USER CODE BEGIN TIM3_MspDeInit 1 */
-
-  /* USER CODE END TIM3_MspDeInit 1 */
-  }
-}
-
-/* USER CODE BEGIN 1 */
-
-/* USER CODE END 1 */
diff --git a/.clang-format b/firmware/.clang-format
similarity index 100%
rename from .clang-format
rename to firmware/.clang-format
diff --git a/.gitignore b/firmware/.gitignore
similarity index 100%
rename from .gitignore
rename to firmware/.gitignore
diff --git a/.gitlab-ci.yml b/firmware/.gitlab-ci.yml
similarity index 100%
rename from .gitlab-ci.yml
rename to firmware/.gitlab-ci.yml
diff --git a/Inc/adc.h b/firmware/Inc/adc.h
similarity index 100%
rename from Inc/adc.h
rename to firmware/Inc/adc.h
diff --git a/Inc/can.h b/firmware/Inc/can.h
similarity index 100%
rename from Inc/can.h
rename to firmware/Inc/can.h
diff --git a/Inc/gpio.h b/firmware/Inc/gpio.h
similarity index 100%
rename from Inc/gpio.h
rename to firmware/Inc/gpio.h
diff --git a/Inc/main.h b/firmware/Inc/main.h
similarity index 90%
rename from Inc/main.h
rename to firmware/Inc/main.h
index 41be1d3..54202de 100644
--- a/Inc/main.h
+++ b/firmware/Inc/main.h
@@ -29,7 +29,6 @@ extern "C" {
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_hal.h"
 
-#include "stm32f4xx_ll_tim.h"
 #include "stm32f4xx_ll_usart.h"
 #include "stm32f4xx_ll_rcc.h"
 #include "stm32f4xx_ll_system.h"
@@ -79,6 +78,12 @@ void Error_Handler(void);
 #define AS5045_CS_GPIO_Port GPIOB
 #define EN_W_Pin LL_GPIO_PIN_6
 #define EN_W_GPIO_Port GPIOC
+#define DRV_FAULT_Pin LL_GPIO_PIN_7
+#define DRV_FAULT_GPIO_Port GPIOC
+#define DRV_RESET_Pin LL_GPIO_PIN_8
+#define DRV_RESET_GPIO_Port GPIOC
+#define DRV_SLEEP_Pin LL_GPIO_PIN_9
+#define DRV_SLEEP_GPIO_Port GPIOC
 #define EN_U_Pin LL_GPIO_PIN_11
 #define EN_U_GPIO_Port GPIOA
 #define EN_V_Pin LL_GPIO_PIN_12
diff --git a/Inc/spi.h b/firmware/Inc/spi.h
similarity index 100%
rename from Inc/spi.h
rename to firmware/Inc/spi.h
diff --git a/Inc/stm32_assert.h b/firmware/Inc/stm32_assert.h
similarity index 100%
rename from Inc/stm32_assert.h
rename to firmware/Inc/stm32_assert.h
diff --git a/Inc/stm32f4xx_hal_conf.h b/firmware/Inc/stm32f4xx_hal_conf.h
similarity index 100%
rename from Inc/stm32f4xx_hal_conf.h
rename to firmware/Inc/stm32f4xx_hal_conf.h
diff --git a/Inc/stm32f4xx_it.h b/firmware/Inc/stm32f4xx_it.h
similarity index 100%
rename from Inc/stm32f4xx_it.h
rename to firmware/Inc/stm32f4xx_it.h
diff --git a/Inc/tim.h b/firmware/Inc/tim.h
similarity index 88%
rename from Inc/tim.h
rename to firmware/Inc/tim.h
index c0b9ce7..5066305 100644
--- a/Inc/tim.h
+++ b/firmware/Inc/tim.h
@@ -32,6 +32,10 @@ extern "C" {
 
 /* USER CODE END Includes */
 
+extern TIM_HandleTypeDef htim1;
+
+extern TIM_HandleTypeDef htim2;
+
 extern TIM_HandleTypeDef htim3;
 
 /* USER CODE BEGIN Private defines */
@@ -42,6 +46,8 @@ void MX_TIM1_Init(void);
 void MX_TIM2_Init(void);
 void MX_TIM3_Init(void);
 
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+
 /* USER CODE BEGIN Prototypes */
   extern volatile FlagStatus flag_10kHz;
 /* USER CODE END Prototypes */
diff --git a/Inc/usart.h b/firmware/Inc/usart.h
similarity index 100%
rename from Inc/usart.h
rename to firmware/Inc/usart.h
diff --git a/Lib/README b/firmware/Lib/README
similarity index 100%
rename from Lib/README
rename to firmware/Lib/README
diff --git a/STM32F446RETX_FLASH.ld b/firmware/STM32F446RETX_FLASH.ld
similarity index 100%
rename from STM32F446RETX_FLASH.ld
rename to firmware/STM32F446RETX_FLASH.ld
diff --git a/STM32F446RETX_RAM.ld b/firmware/STM32F446RETX_RAM.ld
similarity index 100%
rename from STM32F446RETX_RAM.ld
rename to firmware/STM32F446RETX_RAM.ld
diff --git a/Src/adc.c b/firmware/Src/adc.c
similarity index 84%
rename from Src/adc.c
rename to firmware/Src/adc.c
index 83a1dce..27f567d 100644
--- a/Src/adc.c
+++ b/firmware/Src/adc.c
@@ -74,15 +74,15 @@ void MX_ADC2_Init(void)
   hadc2.Instance = ADC2;
   hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
   hadc2.Init.Resolution = ADC_RESOLUTION_12B;
-  hadc2.Init.ScanConvMode = DISABLE;
+  hadc2.Init.ScanConvMode = ENABLE;
   hadc2.Init.ContinuousConvMode = DISABLE;
   hadc2.Init.DiscontinuousConvMode = DISABLE;
   hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
   hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
   hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
-  hadc2.Init.NbrOfConversion = 1;
+  hadc2.Init.NbrOfConversion = 3;
   hadc2.Init.DMAContinuousRequests = DISABLE;
-  hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc2.Init.EOCSelection = ADC_EOC_SEQ_CONV;
   if (HAL_ADC_Init(&hadc2) != HAL_OK)
   {
     Error_Handler();
@@ -97,6 +97,24 @@ void MX_ADC2_Init(void)
   {
     Error_Handler();
   }
+
+  /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
+  */
+  sConfig.Channel = ADC_CHANNEL_8;
+  sConfig.Rank = 2;
+  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
+  */
+  sConfig.Channel = ADC_CHANNEL_9;
+  sConfig.Rank = 3;
+  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
   /* USER CODE BEGIN ADC2_Init 2 */
 
   /* USER CODE END ADC2_Init 2 */
diff --git a/Src/can.c b/firmware/Src/can.c
similarity index 100%
rename from Src/can.c
rename to firmware/Src/can.c
diff --git a/Src/gpio.c b/firmware/Src/gpio.c
similarity index 87%
rename from Src/gpio.c
rename to firmware/Src/gpio.c
index 630d2c5..d67dfa2 100644
--- a/Src/gpio.c
+++ b/firmware/Src/gpio.c
@@ -55,7 +55,8 @@ void MX_GPIO_Init(void)
   LL_GPIO_SetOutputPin(AS5045_CS_GPIO_Port, AS5045_CS_Pin);
 
   /**/
-  LL_GPIO_ResetOutputPin(GPIOC, EN_W_Pin|LED1_Pin|LED2_Pin|LED3_Pin);
+  LL_GPIO_ResetOutputPin(GPIOC, EN_W_Pin|DRV_RESET_Pin|DRV_SLEEP_Pin|LED1_Pin
+                          |LED2_Pin|LED3_Pin);
 
   /**/
   LL_GPIO_ResetOutputPin(GPIOA, EN_U_Pin|EN_V_Pin);
@@ -79,6 +80,21 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
   LL_GPIO_Init(EN_W_GPIO_Port, &GPIO_InitStruct);
 
+  /**/
+  GPIO_InitStruct.Pin = DRV_FAULT_Pin;
+  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
+  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
+  LL_GPIO_Init(DRV_FAULT_GPIO_Port, &GPIO_InitStruct);
+
+  /**/
+  GPIO_InitStruct.Pin = DRV_RESET_Pin|DRV_SLEEP_Pin|LED1_Pin|LED2_Pin
+                          |LED3_Pin;
+  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
+  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
+  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
+  LL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
   /**/
   GPIO_InitStruct.Pin = EN_U_Pin|EN_V_Pin;
   GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
@@ -87,14 +103,6 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
   LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
-  /**/
-  GPIO_InitStruct.Pin = LED1_Pin|LED2_Pin|LED3_Pin;
-  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
-  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
-  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
-  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
-  LL_GPIO_Init(GPIOC, &GPIO_InitStruct);
-
   /**/
   GPIO_InitStruct.Pin = spi1_cs_Pin;
   GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
diff --git a/Src/main.c b/firmware/Src/main.c
similarity index 64%
rename from Src/main.c
rename to firmware/Src/main.c
index f68c786..79d60fd 100644
--- a/Src/main.c
+++ b/firmware/Src/main.c
@@ -20,15 +20,14 @@
 #include "main.h"
 #include "adc.h"
 #include "can.h"
-#include "gpio.h"
 #include "spi.h"
 #include "tim.h"
 #include "usart.h"
-
+#include "gpio.h"
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include <math.h>
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -60,17 +59,60 @@ static void MX_NVIC_Init(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
+#define PI 3.14159265359f
+#define TWO_PI (2 * PI)
+#define MAX_CURRENT 3.0f // Max phase current
+#define POLE_PAIRS 3 // Motor's number of pole pairs
+#define TARGET_ANGLE 120.0f // Target angle in degrees
+#define TARGET_ANGLE_RAD (TARGET_ANGLE * PI / 180.0f) // 
 
+void StartMotorControl(void) {
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
+    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
+}
+
+void StopMotorControl(void) {
+    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
+    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
+    HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
+}
+
+void UpdateMotor(float a, float b, float c) {
+
+    uint32_t pwm_value_a = (uint32_t)((a / MAX_CURRENT) * ((float)htim1.Init.Period));
+    uint32_t pwm_value_b = (uint32_t)((b / MAX_CURRENT) * ((float)htim1.Init.Period));
+    uint32_t pwm_value_c = (uint32_t)((c / MAX_CURRENT) * ((float)htim1.Init.Period));
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm_value_a);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pwm_value_b);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pwm_value_c);
+}
+
+void RunMotorToAngle(float rotorAngle) {
+    float phaseA, phaseB, phaseC;
+    float rotorRadians = fmod(rotorAngle, 360.0f) * (PI / 180.0f);
+    if (rotorAngle >= TARGET_ANGLE) {
+      // StopMotor(); // Stop the motor
+    }
+    phaseA = MAX_CURRENT * sinf(rotorRadians);
+    phaseB = MAX_CURRENT * sinf(rotorRadians - TWO_PI / 3.0f);
+    phaseC = MAX_CURRENT * sinf(rotorRadians + TWO_PI / 3.0f);
+    // char buf[100];
+    // /// print all phases currents
+    // sprintf(buf, "Phase A B C: %f %f %f\n", phaseA, phaseB, phaseC);
+    // USART1_PutString(buf);
+    UpdateMotor(phaseA, phaseB, phaseC);
+}
 /* USER CODE END 0 */
 
 /**
- * @brief  The application entry point.
- * @retval int
- */
+  * @brief  The application entry point.
+  * @retval int
+  */
 int main(void)
 {
   /* USER CODE BEGIN 1 */
-  int a = 0;
+
   /* USER CODE END 1 */
 
   /* MCU Configuration--------------------------------------------------------*/
@@ -105,6 +147,16 @@ int main(void)
   MX_NVIC_Init();
   /* USER CODE BEGIN 2 */
   AS5045_CS_Init();
+
+
+  /// INIT DRV8313
+  HAL_GPIO_WritePin(DRV_RESET_GPIO_Port, DRV_RESET_Pin, GPIO_PIN_SET);
+  HAL_GPIO_WritePin(DRV_SLEEP_GPIO_Port, DRV_SLEEP_Pin, GPIO_PIN_SET);
+  HAL_Delay(100);
+  HAL_GPIO_WritePin(EN_U_GPIO_Port, EN_U_Pin, GPIO_PIN_SET);
+  HAL_GPIO_WritePin(EN_V_GPIO_Port, EN_V_Pin, GPIO_PIN_SET);
+  HAL_GPIO_WritePin(EN_W_GPIO_Port, EN_W_Pin, GPIO_PIN_SET);
+
   /* USER CODE END 2 */
 
   /* Infinite loop */
@@ -112,8 +164,12 @@ int main(void)
   float angle_values[200];
   uint8_t angle_index = 0;
   float ADC123_ANGLE[4] = {0, 0, 0, 0};
+  StartMotorControl();
+
   while (1)
   {
+    // Ready to MERGE
+    RunMotorToAngle(ADC123_ANGLE[3] + 10.0f);
     if (flag_10kHz == SET)
     {
       flag_10kHz = RESET;
@@ -126,11 +182,11 @@ int main(void)
       {
         angle_index = 0;
         float average = 0;
-        for (int i = 0; i < 20; i++)
+        for (int i = 0; i < 200; i++)
         {
           average += angle_values[i];
         }
-        average /= 20.0;
+        average /= 200.0;
         ADC123_ANGLE[0] = GetCurrentFromADC(ReadADCValue(&hadc2, ADC_CHANNEL_15)); // Read ADC
         ADC123_ANGLE[1] = GetCurrentFromADC(ReadADCValue(&hadc2, ADC_CHANNEL_8));  // Read ADC
         ADC123_ANGLE[2] = GetCurrentFromADC(ReadADCValue(&hadc2, ADC_CHANNEL_9));  // Read ADC
@@ -138,6 +194,7 @@ int main(void)
         USART1_PutString("\xDE\xAD");
         SendFloatsWithLL((uint8_t *)ADC123_ANGLE);
         USART1_PutString("\xBE\xAF");
+        // break;
       }
       if (status == 0)
       {
@@ -160,6 +217,7 @@ int main(void)
         {
           // Linearity Alarm error
         }
+        // StopMotorControl();
       }
 
       // sprintf(buf, "Average: %f\r\n", degrees);
@@ -175,22 +233,22 @@ int main(void)
 }
 
 /**
- * @brief System Clock Configuration
- * @retval None
- */
+  * @brief System Clock Configuration
+  * @retval None
+  */
 void SystemClock_Config(void)
 {
   RCC_OscInitTypeDef RCC_OscInitStruct = {0};
   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
 
   /** Configure the main internal regulator output voltage
-   */
+  */
   __HAL_RCC_PWR_CLK_ENABLE();
   __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
 
   /** Initializes the RCC Oscillators according to the specified parameters
-   * in the RCC_OscInitTypeDef structure.
-   */
+  * in the RCC_OscInitTypeDef structure.
+  */
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
   RCC_OscInitStruct.HSEState = RCC_HSE_ON;
   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
@@ -206,15 +264,16 @@ void SystemClock_Config(void)
   }
 
   /** Activate the Over-Drive mode
-   */
+  */
   if (HAL_PWREx_EnableOverDrive() != HAL_OK)
   {
     Error_Handler();
   }
 
   /** Initializes the CPU, AHB and APB buses clocks
-   */
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
+  */
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
@@ -227,20 +286,20 @@ void SystemClock_Config(void)
 }
 
 /**
- * @brief NVIC Configuration.
- * @retval None
- */
+  * @brief NVIC Configuration.
+  * @retval None
+  */
 static void MX_NVIC_Init(void)
 {
   /* TIM1_BRK_TIM9_IRQn interrupt configuration */
-  NVIC_SetPriority(TIM1_BRK_TIM9_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 1, 0));
-  NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
+  HAL_NVIC_SetPriority(TIM1_BRK_TIM9_IRQn, 1, 0);
+  HAL_NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
   /* TIM1_CC_IRQn interrupt configuration */
-  NVIC_SetPriority(TIM1_CC_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 3, 0));
-  NVIC_EnableIRQ(TIM1_CC_IRQn);
+  HAL_NVIC_SetPriority(TIM1_CC_IRQn, 3, 0);
+  HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);
   /* TIM2_IRQn interrupt configuration */
-  NVIC_SetPriority(TIM2_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(), 4, 0));
-  NVIC_EnableIRQ(TIM2_IRQn);
+  HAL_NVIC_SetPriority(TIM2_IRQn, 4, 0);
+  HAL_NVIC_EnableIRQ(TIM2_IRQn);
   /* ADC_IRQn interrupt configuration */
   HAL_NVIC_SetPriority(ADC_IRQn, 2, 0);
   HAL_NVIC_EnableIRQ(ADC_IRQn);
@@ -251,9 +310,9 @@ static void MX_NVIC_Init(void)
 /* USER CODE END 4 */
 
 /**
- * @brief  This function is executed in case of error occurrence.
- * @retval None
- */
+  * @brief  This function is executed in case of error occurrence.
+  * @retval None
+  */
 void Error_Handler(void)
 {
   /* USER CODE BEGIN Error_Handler_Debug */
@@ -265,14 +324,14 @@ void Error_Handler(void)
   /* USER CODE END Error_Handler_Debug */
 }
 
-#ifdef USE_FULL_ASSERT
+#ifdef  USE_FULL_ASSERT
 /**
- * @brief  Reports the name of the source file and the source line number
- *         where the assert_param error has occurred.
- * @param  file: pointer to the source file name
- * @param  line: assert_param error line source number
- * @retval None
- */
+  * @brief  Reports the name of the source file and the source line number
+  *         where the assert_param error has occurred.
+  * @param  file: pointer to the source file name
+  * @param  line: assert_param error line source number
+  * @retval None
+  */
 void assert_failed(uint8_t *file, uint32_t line)
 {
   /* USER CODE BEGIN 6 */
diff --git a/Src/spi.c b/firmware/Src/spi.c
similarity index 100%
rename from Src/spi.c
rename to firmware/Src/spi.c
diff --git a/Src/stm32f4xx_hal_msp.c b/firmware/Src/stm32f4xx_hal_msp.c
similarity index 100%
rename from Src/stm32f4xx_hal_msp.c
rename to firmware/Src/stm32f4xx_hal_msp.c
diff --git a/Src/stm32f4xx_it.c b/firmware/Src/stm32f4xx_it.c
similarity index 94%
rename from Src/stm32f4xx_it.c
rename to firmware/Src/stm32f4xx_it.c
index 513b196..c4c7c4c 100644
--- a/Src/stm32f4xx_it.c
+++ b/firmware/Src/stm32f4xx_it.c
@@ -57,6 +57,8 @@
 /* External variables --------------------------------------------------------*/
 extern ADC_HandleTypeDef hadc2;
 extern SPI_HandleTypeDef hspi2;
+extern TIM_HandleTypeDef htim1;
+extern TIM_HandleTypeDef htim2;
 extern TIM_HandleTypeDef htim3;
 /* USER CODE BEGIN EV */
 volatile FlagStatus flag_10kHz = RESET;
@@ -222,7 +224,7 @@ void TIM1_BRK_TIM9_IRQHandler(void)
   /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 0 */
 
   /* USER CODE END TIM1_BRK_TIM9_IRQn 0 */
-
+  HAL_TIM_IRQHandler(&htim1);
   /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 1 */
 
   /* USER CODE END TIM1_BRK_TIM9_IRQn 1 */
@@ -236,6 +238,7 @@ void TIM1_CC_IRQHandler(void)
   /* USER CODE BEGIN TIM1_CC_IRQn 0 */
 
   /* USER CODE END TIM1_CC_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim1);
   /* USER CODE BEGIN TIM1_CC_IRQn 1 */
 
   /* USER CODE END TIM1_CC_IRQn 1 */
@@ -249,6 +252,7 @@ void TIM2_IRQHandler(void)
   /* USER CODE BEGIN TIM2_IRQn 0 */
 
   /* USER CODE END TIM2_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim2);
   /* USER CODE BEGIN TIM2_IRQn 1 */
 
   /* USER CODE END TIM2_IRQn 1 */
diff --git a/Src/system_stm32f4xx.c b/firmware/Src/system_stm32f4xx.c
similarity index 100%
rename from Src/system_stm32f4xx.c
rename to firmware/Src/system_stm32f4xx.c
diff --git a/firmware/Src/tim.c b/firmware/Src/tim.c
new file mode 100644
index 0000000..ff31813
--- /dev/null
+++ b/firmware/Src/tim.c
@@ -0,0 +1,323 @@
+/* USER CODE BEGIN Header */
+/**
+ ******************************************************************************
+ * @file    tim.c
+ * @brief   This file provides code for the configuration
+ *          of the TIM instances.
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2024 STMicroelectronics.
+ * All rights reserved.
+ *
+ * This software is licensed under terms that can be found in the LICENSE file
+ * in the root directory of this software component.
+ * If no LICENSE file comes with this software, it is provided AS-IS.
+ *
+ ******************************************************************************
+ */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "tim.h"
+
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+TIM_HandleTypeDef htim1;
+TIM_HandleTypeDef htim2;
+TIM_HandleTypeDef htim3;
+
+/* TIM1 init function */
+void MX_TIM1_Init(void)
+{
+
+  /* USER CODE BEGIN TIM1_Init 0 */
+
+  /* USER CODE END TIM1_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+  TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
+
+  /* USER CODE BEGIN TIM1_Init 1 */
+
+  /* USER CODE END TIM1_Init 1 */
+  htim1.Instance = TIM1;
+  htim1.Init.Prescaler = 0;
+  htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
+  htim1.Init.Period = 2399;
+  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim1.Init.RepetitionCounter = 0;
+  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
+  if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
+  sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
+  sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
+  sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
+  sBreakDeadTimeConfig.DeadTime = 0;
+  sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
+  sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
+  sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
+  if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM1_Init 2 */
+  if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1) != HAL_OK)
+  {
+    /* PWM Generation Error */
+    Error_Handler();
+  }
+  /* USER CODE END TIM1_Init 2 */
+  HAL_TIM_MspPostInit(&htim1);
+
+}
+/* TIM2 init function */
+void MX_TIM2_Init(void)
+{
+
+  /* USER CODE BEGIN TIM2_Init 0 */
+
+  /* USER CODE END TIM2_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM2_Init 1 */
+
+  /* USER CODE END TIM2_Init 1 */
+  htim2.Instance = TIM2;
+  htim2.Init.Prescaler = 89;
+  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim2.Init.Period = 3999;
+  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM2_Init 2 */
+  if (HAL_TIM_Base_Start_IT(&htim2) != HAL_OK)
+  {
+    /* Starting Error */
+    Error_Handler();
+  }
+  /* USER CODE END TIM2_Init 2 */
+
+}
+/* TIM3 init function */
+void MX_TIM3_Init(void)
+{
+
+  /* USER CODE BEGIN TIM3_Init 0 */
+
+  /* USER CODE END TIM3_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM3_Init 1 */
+
+  /* USER CODE END TIM3_Init 1 */
+  htim3.Instance = TIM3;
+  htim3.Init.Prescaler = 89;
+  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim3.Init.Period = 99;
+  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM3_Init 2 */
+  if (HAL_TIM_Base_Start_IT(&htim3) != HAL_OK)
+  {
+    /* Starting Error */
+    Error_Handler();
+  }
+  /* USER CODE END TIM3_Init 2 */
+
+}
+
+void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
+{
+
+  if(tim_baseHandle->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspInit 0 */
+
+  /* USER CODE END TIM1_MspInit 0 */
+    /* TIM1 clock enable */
+    __HAL_RCC_TIM1_CLK_ENABLE();
+  /* USER CODE BEGIN TIM1_MspInit 1 */
+
+  /* USER CODE END TIM1_MspInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspInit 0 */
+
+  /* USER CODE END TIM2_MspInit 0 */
+    /* TIM2 clock enable */
+    __HAL_RCC_TIM2_CLK_ENABLE();
+  /* USER CODE BEGIN TIM2_MspInit 1 */
+
+  /* USER CODE END TIM2_MspInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspInit 0 */
+
+  /* USER CODE END TIM3_MspInit 0 */
+    /* TIM3 clock enable */
+    __HAL_RCC_TIM3_CLK_ENABLE();
+
+    /* TIM3 interrupt Init */
+    HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM3_IRQn);
+  /* USER CODE BEGIN TIM3_MspInit 1 */
+
+  /* USER CODE END TIM3_MspInit 1 */
+  }
+}
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(timHandle->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspPostInit 0 */
+
+  /* USER CODE END TIM1_MspPostInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM1 GPIO Configuration
+    PA8     ------> TIM1_CH1
+    PA9     ------> TIM1_CH2
+    PA10     ------> TIM1_CH3
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_10;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM1_MspPostInit 1 */
+
+  /* USER CODE END TIM1_MspPostInit 1 */
+  }
+
+}
+
+void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
+{
+
+  if(tim_baseHandle->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspDeInit 0 */
+
+  /* USER CODE END TIM1_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM1_CLK_DISABLE();
+
+    /* TIM1 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM1_BRK_TIM9_IRQn);
+    HAL_NVIC_DisableIRQ(TIM1_CC_IRQn);
+  /* USER CODE BEGIN TIM1_MspDeInit 1 */
+
+  /* USER CODE END TIM1_MspDeInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspDeInit 0 */
+
+  /* USER CODE END TIM2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM2_CLK_DISABLE();
+
+    /* TIM2 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM2_IRQn);
+  /* USER CODE BEGIN TIM2_MspDeInit 1 */
+
+  /* USER CODE END TIM2_MspDeInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspDeInit 0 */
+
+  /* USER CODE END TIM3_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM3_CLK_DISABLE();
+
+    /* TIM3 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM3_IRQn);
+  /* USER CODE BEGIN TIM3_MspDeInit 1 */
+
+  /* USER CODE END TIM3_MspDeInit 1 */
+  }
+}
+
+/* USER CODE BEGIN 1 */
+
+/* USER CODE END 1 */
diff --git a/Src/usart.c b/firmware/Src/usart.c
similarity index 100%
rename from Src/usart.c
rename to firmware/Src/usart.c
diff --git a/Startup/startup_stm32f446retx.s b/firmware/Startup/startup_stm32f446retx.s
similarity index 100%
rename from Startup/startup_stm32f446retx.s
rename to firmware/Startup/startup_stm32f446retx.s
diff --git a/processor32f446.ioc b/firmware/cubemx_config.ioc
similarity index 87%
rename from processor32f446.ioc
rename to firmware/cubemx_config.ioc
index 918a3eb..bd862cd 100644
--- a/processor32f446.ioc
+++ b/firmware/cubemx_config.ioc
@@ -59,34 +59,37 @@ Mcu.Pin12=PB13
 Mcu.Pin13=PB14
 Mcu.Pin14=PB15
 Mcu.Pin15=PC6
-Mcu.Pin16=PA8
-Mcu.Pin17=PA9
-Mcu.Pin18=PA10
-Mcu.Pin19=PA11
+Mcu.Pin16=PC7
+Mcu.Pin17=PC8
+Mcu.Pin18=PC9
+Mcu.Pin19=PA8
 Mcu.Pin2=PC1
-Mcu.Pin20=PA12
-Mcu.Pin21=PA13
-Mcu.Pin22=PA14
-Mcu.Pin23=PC10
-Mcu.Pin24=PC11
-Mcu.Pin25=PC12
-Mcu.Pin26=PD2
-Mcu.Pin27=PB6
-Mcu.Pin28=PB7
-Mcu.Pin29=PB8
+Mcu.Pin20=PA9
+Mcu.Pin21=PA10
+Mcu.Pin22=PA11
+Mcu.Pin23=PA12
+Mcu.Pin24=PA13
+Mcu.Pin25=PA14
+Mcu.Pin26=PC10
+Mcu.Pin27=PC11
+Mcu.Pin28=PC12
+Mcu.Pin29=PD2
 Mcu.Pin3=PA0-WKUP
-Mcu.Pin30=PB9
-Mcu.Pin31=VP_SYS_VS_Systick
-Mcu.Pin32=VP_TIM1_VS_no_output4
-Mcu.Pin33=VP_TIM2_VS_ClockSourceINT
-Mcu.Pin34=VP_TIM3_VS_ClockSourceINT
+Mcu.Pin30=PB6
+Mcu.Pin31=PB7
+Mcu.Pin32=PB8
+Mcu.Pin33=PB9
+Mcu.Pin34=VP_SYS_VS_Systick
+Mcu.Pin35=VP_TIM1_VS_ClockSourceINT
+Mcu.Pin36=VP_TIM2_VS_ClockSourceINT
+Mcu.Pin37=VP_TIM3_VS_ClockSourceINT
 Mcu.Pin4=PA1
 Mcu.Pin5=PA2
 Mcu.Pin6=PA3
 Mcu.Pin7=PC5
 Mcu.Pin8=PB0
 Mcu.Pin9=PB1
-Mcu.PinsNb=35
+Mcu.PinsNb=38
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F446RETx
@@ -197,6 +200,18 @@ PC6.GPIO_PuPd=GPIO_PULLDOWN
 PC6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
 PC6.Locked=true
 PC6.Signal=GPIO_Output
+PC7.GPIOParameters=GPIO_Label
+PC7.GPIO_Label=DRV_FAULT
+PC7.Locked=true
+PC7.Signal=GPIO_Input
+PC8.GPIOParameters=GPIO_Label
+PC8.GPIO_Label=DRV_RESET
+PC8.Locked=true
+PC8.Signal=GPIO_Output
+PC9.GPIOParameters=GPIO_Label
+PC9.GPIO_Label=DRV_SLEEP
+PC9.Locked=true
+PC9.Signal=GPIO_Output
 PD2.GPIOParameters=GPIO_Speed,GPIO_PuPd,GPIO_Label
 PD2.GPIO_Label=spi1_cs
 PD2.GPIO_PuPd=GPIO_PULLDOWN
@@ -228,14 +243,14 @@ ProjectManager.MainLocation=Src
 ProjectManager.NoMain=false
 ProjectManager.PreviousToolchain=STM32CubeIDE
 ProjectManager.ProjectBuild=false
-ProjectManager.ProjectFileName=processor32f446.ioc
-ProjectManager.ProjectName=processor32f446
+ProjectManager.ProjectFileName=cubemx_config.ioc
+ProjectManager.ProjectName=cubemx_config
 ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x400
 ProjectManager.TargetToolchain=Other Toolchains (GPDSC)
 ProjectManager.ToolChainLocation=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-LL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_TIM1_Init-TIM1-false-LL-true,4-MX_USART1_UART_Init-USART1-false-LL-true,5-MX_USART2_UART_Init-USART2-false-LL-true,6-MX_TIM2_Init-TIM2-false-LL-true,7-MX_SPI2_Init-SPI2-false-HAL-true,8-MX_CAN1_Init-CAN1-false-HAL-true,9-MX_CAN2_Init-CAN2-false-HAL-true,10-MX_TIM3_Init-TIM3-false-HAL-true,11-MX_ADC2_Init-ADC2-false-HAL-true
+ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-LL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_TIM1_Init-TIM1-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-LL-true,5-MX_USART2_UART_Init-USART2-false-LL-true,6-MX_TIM2_Init-TIM2-false-HAL-true,7-MX_SPI2_Init-SPI2-false-HAL-true,8-MX_CAN1_Init-CAN1-false-HAL-true,9-MX_CAN2_Init-CAN2-false-HAL-true,10-MX_TIM3_Init-TIM3-false-HAL-true,11-MX_ADC2_Init-ADC2-false-HAL-true
 RCC.AHBFreq_Value=180000000
 RCC.APB1CLKDivider=RCC_HCLK_DIV4
 RCC.APB1Freq_Value=45000000
@@ -307,15 +322,16 @@ TIM1.BreakState=TIM_BREAK_DISABLE
 TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
 TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
-TIM1.Channel-PWM\ Generation4\ No\ Output=TIM_CHANNEL_4
 TIM1.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1
-TIM1.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Channel-PWM Generation4 No Output,TIM_MasterOutputTrigger,AutoReloadPreload,BreakState,OffStateRunMode,OffStateIDLEMode,CounterMode,OCPolarity_4
-TIM1.OCPolarity_4=TIM_OCPOLARITY_LOW
+TIM1.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,TIM_MasterOutputTrigger,AutoReloadPreload,BreakState,OffStateRunMode,OffStateIDLEMode,CounterMode,Period
 TIM1.OffStateIDLEMode=TIM_OSSI_DISABLE
 TIM1.OffStateRunMode=TIM_OSSR_DISABLE
-TIM1.TIM_MasterOutputTrigger=TIM_TRGO_OC4REF
-TIM2.IPParameters=TIM_MasterOutputTrigger
-TIM2.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
+TIM1.Period=2399
+TIM1.TIM_MasterOutputTrigger=TIM_TRGO_RESET
+TIM2.IPParameters=TIM_MasterOutputTrigger,Prescaler,Period
+TIM2.Period=3999
+TIM2.Prescaler=89
+TIM2.TIM_MasterOutputTrigger=TIM_TRGO_RESET
 TIM3.IPParameters=Period,Prescaler
 TIM3.Period=99
 TIM3.Prescaler=89
@@ -325,8 +341,8 @@ USART2.IPParameters=VirtualMode
 USART2.VirtualMode=VM_ASYNC
 VP_SYS_VS_Systick.Mode=SysTick
 VP_SYS_VS_Systick.Signal=SYS_VS_Systick
-VP_TIM1_VS_no_output4.Mode=PWM Generation4 No Output
-VP_TIM1_VS_no_output4.Signal=TIM1_VS_no_output4
+VP_TIM1_VS_ClockSourceINT.Mode=Internal
+VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT
 VP_TIM2_VS_ClockSourceINT.Mode=Internal
 VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT
 VP_TIM3_VS_ClockSourceINT.Mode=Internal
diff --git a/extra_script.py b/firmware/extra_script.py
similarity index 100%
rename from extra_script.py
rename to firmware/extra_script.py
diff --git a/AS5045B_DS000397_2-00.pdf b/firmware/knowledges/AS5045B_DS000397_2-00.pdf
similarity index 100%
rename from AS5045B_DS000397_2-00.pdf
rename to firmware/knowledges/AS5045B_DS000397_2-00.pdf
diff --git a/firmware/knowledges/TI-DRV8313.pdf b/firmware/knowledges/TI-DRV8313.pdf
new file mode 100644
index 0000000..e4d72c5
Binary files /dev/null and b/firmware/knowledges/TI-DRV8313.pdf differ
diff --git a/motorBoard.pdf b/firmware/knowledges/motorBoard.pdf
similarity index 100%
rename from motorBoard.pdf
rename to firmware/knowledges/motorBoard.pdf
diff --git a/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf b/firmware/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf
similarity index 100%
rename from rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf
rename to firmware/knowledges/rm0390-stm32f446xx-advanced-armbased-32bit-mcus-stmicroelectronics.pdf
diff --git a/platformio.ini b/firmware/platformio.ini
similarity index 94%
rename from platformio.ini
rename to firmware/platformio.ini
index ecb0cec..cf83127 100644
--- a/platformio.ini
+++ b/firmware/platformio.ini
@@ -16,9 +16,12 @@ platform = ststm32
 board = genericSTM32F446RE
 framework = stm32cube
 board_build.stm32cube.custom_config_header = yes
+;ADD BSD_VISIBLE flag
 build_flags = 
     -D USE_FULL_LL_DRIVER
     -D USE_HAL_DRIVER
+	-D_BSD_SOURCE 
+
 upload_protocol = stlink
 debug_tool = stlink
 monitor_speed = 115200
diff --git a/startup_stm32f446retx.s b/firmware/startup_stm32f446retx.s
similarity index 100%
rename from startup_stm32f446retx.s
rename to firmware/startup_stm32f446retx.s
diff --git a/stm32pio.ini b/firmware/stm32pio.ini
similarity index 100%
rename from stm32pio.ini
rename to firmware/stm32pio.ini
diff --git a/test/README b/firmware/test/README
similarity index 100%
rename from test/README
rename to firmware/test/README
diff --git a/vizualize.py b/firmware/vizualize.py
similarity index 100%
rename from vizualize.py
rename to firmware/vizualize.py