getAngle done

keil_project/Services/IncrEncoder.c

@@ -1,9 +1,17 @@
+#include <stdlib.h>
+#include <math.h>
+
 #include "IncrEncoder.h"
 #include "stm32f1xx_ll_gpio.h" 
 #include "stm32f1xx_ll_bus.h"
 #include "stm32f1xx_ll_exti.h"
 #include "stm32f1xx_ll_tim.h"
 
+#define RESOLUTION 1
+#define ANGLE_DEBUT 45
+#define INCR_ENCODER_MID_VALUE 719
+
+
 int index_passed = 0;
 int counts_per_revolution = 360;
 
@@ -78,6 +86,7 @@ void INCR_ENCODER_Init(void){
 }
 
 void EXTI9_5_IRQHandler(void){
+	
 	index_passed = 1;
 	// reset counter = encoder position to 0 position
 	LL_TIM_WriteReg(TIM3,CNT,0);
@@ -94,5 +103,15 @@ int INCR_ENCODER_IsAbsolute(void)
 
 int INCR_ENCODER_GetAngle(void)
 {
-	return LL_TIM_ReadReg(TIM3,CNT);
+	int counter_value = LL_TIM_ReadReg(TIM3, CNT);
+	float vabs = abs(counter_value-INCR_ENCODER_MID_VALUE);
+	float vIEAngleDebut = INCR_ENCODER_MID_VALUE -(ANGLE_DEBUT*4);
+	float nbIncrements = 90/RESOLUTION;
+	
+	if(vabs > vIEAngleDebut)
+	{
+		return 0;
+	}else{
+		return 90 - RESOLUTION*floor(vabs/(vIEAngleDebut/nbIncrements) ) ;
+	}
 };