From 2c500e9b159011b247cc15a2b091dc4cac0b2b0f Mon Sep 17 00:00:00 2001
From: Cavailles Kevin <kc.mailpro@gmail.com>
Date: Wed, 11 Nov 2020 14:53:30 +0100
Subject: [PATCH] getAngle done

---
 keil_project/Services/IncrEncoder.c | 25 ++++++++++++++++++++++---
 1 file changed, 22 insertions(+), 3 deletions(-)

diff --git a/keil_project/Services/IncrEncoder.c b/keil_project/Services/IncrEncoder.c
index bc9f795..b9a8b9b 100644
--- a/keil_project/Services/IncrEncoder.c
+++ b/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;
 
@@ -45,9 +53,9 @@ void INCR_ENCODER_Init(void){
 	encoder_init_struct.IC1Filter = LL_TIM_IC_FILTER_FDIV1 ;
 	encoder_init_struct.IC2Filter = LL_TIM_IC_FILTER_FDIV1 ;
 	
-  LL_TIM_ENCODER_Init(TIM3, &encoder_init_struct);
+	LL_TIM_ENCODER_Init(TIM3, &encoder_init_struct);
 	
-  LL_TIM_EnableCounter(TIM3);
+	LL_TIM_EnableCounter(TIM3);
 	LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
 	LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_AFIO);
 
@@ -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) ) ;
+	}
 };