diff --git a/fw/Core/Src/main.c b/fw/Core/Src/main.c
index eda6b45..bcfddbf 100644
--- a/fw/Core/Src/main.c
+++ b/fw/Core/Src/main.c
@@ -21,7 +21,7 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include <stdbool.h>
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -60,6 +60,30 @@ uint16_t ADC_ReadChannel(uint32_t channel);
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
 
+void H_bridge_turn_off()
+{
+  HAL_GPIO_WritePin(LS_A_GPIO_Port, LS_A_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(LS_B_GPIO_Port, LS_B_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(HS_A_GPIO_Port, HS_A_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(HS_B_GPIO_Port, HS_B_Pin, GPIO_PIN_RESET);
+}
+
+void H_bridge_set_direction(bool positive)
+{
+  H_bridge_turn_off();
+  if (positive)
+  {
+    HAL_GPIO_WritePin(HS_A_GPIO_Port, HS_A_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(LS_B_GPIO_Port, LS_B_Pin, GPIO_PIN_SET);
+  }
+  else
+  {
+
+    HAL_GPIO_WritePin(HS_B_GPIO_Port, HS_B_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(LS_A_GPIO_Port, LS_A_Pin, GPIO_PIN_SET);
+  }
+}
+
 /* USER CODE END 0 */
 
 /**
@@ -94,12 +118,16 @@ int main(void)
   MX_ADC1_Init();
   MX_USB_PCD_Init();
   /* USER CODE BEGIN 2 */
-
+  H_bridge_turn_off();
+  H_bridge_set_direction(true);
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
   GPIO_PinState state = GPIO_PIN_RESET;
+  const uint32_t polarity_switch_period = 1000;
+  uint32_t next_polarity_switch_ms = HAL_GetTick() + polarity_switch_period; 
+  bool polarity = true;
   while (1)
   {
     /* USER CODE END WHILE */
@@ -107,8 +135,9 @@ int main(void)
     uint16_t val_ch0 = ADC_ReadChannel(ADC_CHANNEL_0);
     uint16_t val_ch1 = ADC_ReadChannel(ADC_CHANNEL_1);
     // Convert to voltage (12-bit, 3.3V reference)
-    float voltage_ch0 = (val_ch0 * 3.3f) / 4095.0f;
-    float voltage_ch1 = (val_ch1 * 3.3f) / 4095.0f;
+    const float adc_voltage_divider = 2.0f;
+    float voltage_ch0 = (val_ch0 * 3.3f) / 4095.0f * adc_voltage_divider;
+    float voltage_ch1 = (val_ch1 * 3.3f) / 4095.0f * adc_voltage_divider;
 
     // simple regulation
     const float target_voltage = 3.0f;
@@ -119,6 +148,12 @@ int main(void)
       state = GPIO_PIN_RESET;
     }
     HAL_GPIO_WritePin(VDD_LEDL_EN_GPIO_Port, VDD_LEDL_EN_Pin, state);
+    if (HAL_GetTick() > next_polarity_switch_ms)
+    {
+        next_polarity_switch_ms = HAL_GetTick() + polarity_switch_period;
+        polarity = !polarity;
+        H_bridge_set_direction(polarity);
+    }
     /* USER CODE BEGIN 3 */
   }
   /* USER CODE END 3 */