From a76e05e257ecbb523702b120fc960687e28899d3 Mon Sep 17 00:00:00 2001
From: dekutree64 <54822734+dekutree64@users.noreply.github.com>
Date: Wed, 15 Jul 2026 22:18:03 -0500
Subject: [PATCH 1/2] setCurrentAngle handle full rotations

---
 src/encoders/stm32hwencoder/STM32HWEncoder.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index 0052bcf..63a0472 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -29,7 +29,9 @@ 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);
+    encoder_handle.Instance->CNT = (int32_t)(angle_prev * cpr / _2PI);
 }
 /*
   Modify encoder count directly
@@ -75,7 +77,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;

From 4aed129a63e49788a94a27f6213dd72ec1d9af85 Mon Sep 17 00:00:00 2001
From: dekutree64 <54822734+dekutree64@users.noreply.github.com>
Date: Wed, 15 Jul 2026 22:43:32 -0500
Subject: [PATCH 2/2] Added check for rounding errors

Confirmed by simulation on PC that full_rotations can be off by one when very close to the wraparound point
---
 src/encoders/stm32hwencoder/STM32HWEncoder.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index 63a0472..e632eff 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -31,6 +31,11 @@ float STM32HWEncoder::getSensorAngle() {
 void STM32HWEncoder::setCurrentAngle(float set_angle) {
     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);
 }
 /*