projet_voilier/keil_project/Services/IncrEncoder.c
Cavailles Kevin 2c500e9b15 getAngle done
2020-11-11 14:53:30 +01:00

117 lines
3.2 KiB
C

#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;
void INCR_ENCODER_Init(void){
// GPIO initialization for channels a and b of the encoder
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
LL_GPIO_InitTypeDef channel_a_pin_conf, channel_b_pin_conf;
channel_a_pin_conf.Mode = LL_GPIO_MODE_FLOATING;
channel_a_pin_conf.Pin = LL_GPIO_PIN_6;
LL_GPIO_Init(GPIOA, &channel_a_pin_conf);
channel_b_pin_conf.Mode = LL_GPIO_MODE_FLOATING;
channel_b_pin_conf.Pin = LL_GPIO_PIN_7;
LL_GPIO_Init(GPIOA, &channel_b_pin_conf);
// timer init
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
LL_TIM_InitTypeDef tim3_init_struct;
tim3_init_struct.Autoreload= counts_per_revolution*4-1;
tim3_init_struct.Prescaler=0;
tim3_init_struct.ClockDivision=LL_TIM_CLOCKDIVISION_DIV1;
tim3_init_struct.CounterMode=LL_TIM_COUNTERMODE_UP;
tim3_init_struct.RepetitionCounter=0;
LL_TIM_Init(TIM3, &tim3_init_struct);
// timer as encoder init
LL_TIM_ENCODER_InitTypeDef encoder_init_struct;
encoder_init_struct.EncoderMode = LL_TIM_ENCODERMODE_X4_TI12;
encoder_init_struct.IC1Polarity = LL_TIM_IC_POLARITY_RISING;
encoder_init_struct.IC2Polarity = LL_TIM_IC_POLARITY_RISING;
encoder_init_struct.IC1ActiveInput = LL_TIM_ACTIVEINPUT_DIRECTTI;
encoder_init_struct.IC2ActiveInput = LL_TIM_ACTIVEINPUT_DIRECTTI;
encoder_init_struct.IC1Prescaler = LL_TIM_ICPSC_DIV1;
encoder_init_struct.IC2Prescaler= LL_TIM_ICPSC_DIV1;
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_EnableCounter(TIM3);
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOA);
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_AFIO);
// gpio init for index (interrup) pin
LL_GPIO_InitTypeDef index_pin_conf;
index_pin_conf.Pin = LL_GPIO_PIN_5;
index_pin_conf.Mode = LL_GPIO_MODE_FLOATING;
LL_GPIO_Init(GPIOC, &index_pin_conf);
// exti init (interrupt)
LL_EXTI_InitTypeDef exti;
exti.Line_0_31 = LL_EXTI_LINE_5;
exti.LineCommand = ENABLE;
exti.Mode = LL_EXTI_MODE_IT;
exti.Trigger = LL_EXTI_TRIGGER_RISING;
LL_EXTI_Init(&exti);
LL_GPIO_AF_SetEXTISource(LL_GPIO_AF_EXTI_PORTA, LL_GPIO_AF_EXTI_LINE5);
// enable interrupt from exti in NVIC
NVIC_SetPriority(EXTI9_5_IRQn, 12); // prio??
NVIC_EnableIRQ(EXTI9_5_IRQn);
}
void EXTI9_5_IRQHandler(void){
index_passed = 1;
// reset counter = encoder position to 0 position
LL_TIM_WriteReg(TIM3,CNT,0);
// clear pending (EXTI necessary ?)
LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_5);
NVIC_ClearPendingIRQ(EXTI9_5_IRQn);
}
int INCR_ENCODER_IsAbsolute(void)
{
return index_passed;
};
int INCR_ENCODER_GetAngle(void)
{
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) ) ;
}
};