diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index 0052bcf..e632eff 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -29,7 +29,14 @@ float STM32HWEncoder::getSensorAngle() {
   Set the current angle using CNT register
 */
 void STM32HWEncoder::setCurrentAngle(float set_angle) {
-    encoder_handle.Instance->CNT = set_angle * cpr / _2PI;
+    angle_prev = _normalizeAngle(set_angle);
+    full_rotations = (int32_t)((set_angle - angle_prev) / _2PI);
+    float check_rounding_error = full_rotations * _2PI + angle_prev;
+    if (check_rounding_error > set_angle + _PI)
+        full_rotations -= 1;
+    else if (check_rounding_error < set_angle - _PI)
+        full_rotations += 1;
+    encoder_handle.Instance->CNT = (int32_t)(angle_prev * cpr / _2PI);
 }
 /*
   Modify encoder count directly
@@ -75,7 +82,7 @@ void STM32HWEncoder::init() {
 
     TIM_Encoder_InitTypeDef encoder_config;
 
-    encoder_config.EncoderMode = encoder_mode;
+    encoder_config.EncoderMode = encoder_mode;;
 
     encoder_config.IC1Polarity = TIM_ICPOLARITY_RISING;
     encoder_config.IC1Selection = TIM_ICSELECTION_DIRECTTI;