From b8ea95d7906f7b99746165d37c22b41f20836a3d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jasper=20G=C3=BCldenstein?= <me@jgueldenstein.de>
Date: Sat, 14 Nov 2020 16:29:49 +0100
Subject: [PATCH] incr_encoder angle calculation

---
 keil_project/Services/IncrEncoder.c | 25 ++++++++++++++++++++++---
 keil_project/Services/IncrEncoder.h |  2 +-
 2 files changed, 23 insertions(+), 4 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) ) ;
+	}
 };
diff --git a/keil_project/Services/IncrEncoder.h b/keil_project/Services/IncrEncoder.h
index 3640e9f..7767b5e 100644
--- a/keil_project/Services/IncrEncoder.h
+++ b/keil_project/Services/IncrEncoder.h
@@ -1,5 +1,5 @@
 #ifndef INCR_ENCODER
-//codé par Kévin Cavailles et Jasper Güldenstein
+
 /**
   */
 void INCR_ENCODER_Init(void);