diff --git a/src/encoders/stm32hwencoder/README.md b/src/encoders/stm32hwencoder/README.md
index 1bdb1ca..91e806e 100644
--- a/src/encoders/stm32hwencoder/README.md
+++ b/src/encoders/stm32hwencoder/README.md
@@ -6,7 +6,7 @@ Big thank you to @conroy-cheers for originally contributing this code in pull re
 
 ## Warning
 
-This code has not been tested much! Use at your own risk, your milage may vary.
+This code has not been tested much! Use at your own risk, your milage may vary.\
 I have tested on: STM32F401CC/TIM4-PB6,PB7
 
 ## Hardware setup
@@ -17,8 +17,7 @@ Not all of the timers support the encoder mode, so check the datasheet which tim
 
 An excellent option can be to use the STM32CubeIDE software's pin assignment view to quickly check which pins connect to which timer.
 
-Note that the index (I) pin is currently not used.
-
+Right now the index pin can be any pin supporting interrupts.
 
 ## Software setup
 
@@ -46,4 +45,22 @@ void setup() {
     encoder.init();
 }
 
+```
+
+Set the encoder angle:
+
+```c++
+encoder.setCurrentAngle(1.28);
+```
+
+Set the encoder count:
+
+```c++
+encoder.setEncoderCount(693);
+```
+
+Get the encoder timer handle:
+
+```c++
+encoder.getEncoderTimerHandle();
 ```
\ No newline at end of file
diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index 1cea602..528077a 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -15,15 +15,30 @@ STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int pinA, int pinB, int pinI)
     index_found = false;
 }
 
+// function returns encoder handle
+TIM_HandleTypeDef STM32HWEncoder::getEncoderTimerHandle() { return encoder_handle; }
+
 /*
   Shaft angle calculation
 */
 float STM32HWEncoder::getSensorAngle() { 
     return _2PI * encoder_handle.Instance->CNT / static_cast<float>(cpr);
 }
+/* 
+  Set the current angle using CNT register
+*/
+void STM32HWEncoder::setCurrentAngle(float set_angle) {
+    encoder_handle.Instance->CNT = set_angle * cpr / _2PI;
+}
+/*
+  Modify encoder count directly
+*/
+void STM32HWEncoder::setEncoderCount(uint32_t ecount) {
+    encoder_handle.Instance->CNT = ecount;
+}
 
 // getter for index pin
-int STM32HWEncoder::needsSearch() { return false && !index_found; }
+int STM32HWEncoder::needsSearch() { return hasIndex() && !index_found; }
 
 // private function used to determine if encoder has index
 int STM32HWEncoder::hasIndex() { return (_pinI!=NC); }
@@ -34,11 +49,13 @@ void STM32HWEncoder::init() {
     TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_TIM);
     if (!IS_TIM_ENCODER_INTERFACE_INSTANCE(InstanceA)) {
         initialized = false;
+        SIMPLEFOC_DEBUG("STM32HWEncoder pin A doesn't support encoder interface");
         return;
     }
     TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_TIM);
     if (InstanceA != InstanceB) {
         initialized = false;
+        SIMPLEFOC_DEBUG("STM32HWEncoder pin B is not in the same timer as A");
         return;
     }
     pinmap_pinout(_pinA, PinMap_TIM);
@@ -74,17 +91,38 @@ void STM32HWEncoder::init() {
 
     if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
         initialized = false;
+        SIMPLEFOC_DEBUG("Couldn't initialize timer in encoder mode!");
         return;
     }
 
-    // TODO on STM32G4 MCUs we can use the TIMx_ETR pin for the index, and configure how it is handled automatically by the hardware
-    // on non-G4 MCUs we need to use an external interrupt to handle the index signal
-    // attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() {
-    //     encoder_handle.Instance->CNT = 0; // reset counter
-    //     index_found = true;
-    //     // detach interrupt
-    //     detachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)));
-    // }, index_polarity);
+    // TODO: figure out way to check if Index pin is specifically ETR line
+    //if(IS_TIM_ETR_INSTANCE())
+    
+    // Encoder index configuration
+    //TIMEx_EncoderIndexConfigTypeDef encoder_indexconfig = {0};
+    //encoder_indexconfig.Polarity = TIM_ENCODERINDEX_POLARITY_NONINVERTED;
+    //encoder_indexconfig.Prescaler = TIM_ENCODERINDEX_PRESCALER_DIV1;
+    //encoder_indexconfig.Filter = 0;
+    //encoder_indexconfig.FirstIndexEnable = ENABLE;
+    //encoder_indexconfig.Position = TIM_ENCODERINDEX_POSITION_00;
+    //encoder_indexconfig.Direction = TIM_ENCODERINDEX_DIRECTION_UP_DOWN; // Double check this is ok
+    //if (HAL_TIMEx_ConfigEncoderIndex(&encoder_handle, &encoder_indexconfig) != HAL_OK)
+    //{
+    //    SIMPLEFOC_DEBUG("Couldn't configure encoder index pin");
+    //    initialized = false;
+    //    return;
+    //}
+
+    // If index pin provided attach search interrupt
+    if(hasIndex())
+    {
+        attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() {
+            encoder_handle.Instance->CNT = 0; // reset counter
+            index_found = true;
+            // detach interrupt
+            detachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)));
+        }, index_polarity);
+    }
 
     if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) {
         initialized = false;
diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h
index eb8ec5c..a55e3e7 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.h
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h
@@ -21,6 +21,9 @@ class STM32HWEncoder : public Sensor {
     void init() override;
     int needsSearch() override;
     int hasIndex();  // !< function returning 1 if encoder has index pin and 0 if not.
+    void setCurrentAngle(float set_angle); // !< helper function for setting the angle by using the count register
+    void setEncoderCount(uint32_t ecount); // !< function for setting the count register
+    TIM_HandleTypeDef getEncoderTimerHandle(); // !< function for getting the encoder timer handle
 
     bool initialized = false;
     uint32_t cpr;  //!< encoder cpr number