From a202c1e92c13a2b4dd7679e2f7c0645e723dad07 Mon Sep 17 00:00:00 2001 From: Jakub Tomczak Date: Wed, 15 Dec 2021 13:40:16 +0100 Subject: [PATCH] ABI added --- encoder/ABI.c | 80 ++++++++++++++++++++++++++++++++++++++ encoder/ABI.h | 15 +++++++ encoder/encoder_datatype.h | 16 +++++++- encoder/encoder_hwconf.c | 39 ++++++++++++++++--- encoder/encoder_hwconf.h | 1 + encoder/encoders.c | 28 +++++++++++++ encoder/encoders.h | 1 + 7 files changed, 173 insertions(+), 7 deletions(-) create mode 100644 encoder/ABI.c create mode 100644 encoder/ABI.h diff --git a/encoder/ABI.c b/encoder/ABI.c new file mode 100644 index 00000000..367902d8 --- /dev/null +++ b/encoder/ABI.c @@ -0,0 +1,80 @@ + + +#include "encoder/ABI.h" + +#include "ch.h" +#include "hal.h" +#include "stm32f4xx_conf.h" +#include "hw.h" +#include "mc_interface.h" +#include "utils.h" +#include + +static float last_enc_angle = 0.0; +static ABI_config_t abi_config_now = {0}; +static uint32_t enc_counts = 10000; + +void ABI_deinit(void){ + nvicDisableVector(HW_ENC_EXTI_CH); + + TIM_DeInit(HW_ENC_TIM); + + palSetPadMode(abi_config_now.incremental_config.gpio_A.port, abi_config_now.incremental_config.gpio_A.pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(abi_config_now.incremental_config.gpio_B.port, abi_config_now.incremental_config.gpio_B.pin, PAL_MODE_INPUT_PULLUP); + + last_enc_angle = 0.0; +} + +encoders_ret_t ABI_init(ABI_config_t *abi_config){ + + EXTI_InitTypeDef EXTI_InitStructure; + + // Initialize variables + enc_counts = abi_config->counts; + + palSetPadMode(abi_config->incremental_config.gpio_A.port, abi_config->incremental_config.gpio_A.pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF)); + palSetPadMode(abi_config->incremental_config.gpio_B.port, abi_config->incremental_config.gpio_B.pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF)); +// palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF)); + + // Enable timer clock + HW_ENC_TIM_CLK_EN(); + + // Enable SYSCFG clock + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); + + TIM_EncoderInterfaceConfig (HW_ENC_TIM, TIM_EncoderMode_TI12, + TIM_ICPolarity_Rising, + TIM_ICPolarity_Rising); + TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1); + + // Filter + HW_ENC_TIM->CCMR1 |= 6 << 12 | 6 << 4; + HW_ENC_TIM->CCMR2 |= 6 << 4; + + TIM_Cmd(HW_ENC_TIM, ENABLE); + + // Interrupt on index pulse + + // Connect EXTI Line to pin + SYSCFG_EXTILineConfig(HW_ENC_EXTI_PORTSRC, HW_ENC_EXTI_PINSRC); + + // Configure EXTI Line + EXTI_InitStructure.EXTI_Line = HW_ENC_EXTI_LINE; + EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; + EXTI_InitStructure.EXTI_LineCmd = ENABLE; + EXTI_Init(&EXTI_InitStructure); + + // Enable and set EXTI Line Interrupt to the highest priority + nvicEnableVector(HW_ENC_EXTI_CH, 0); + abi_config->is_init = 1; + + abi_config_now = *abi_config; + return ENCODERS_OK; +} + +float ABI_read_deg(void){ + last_enc_angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts; + return last_enc_angle; +} + diff --git a/encoder/ABI.h b/encoder/ABI.h new file mode 100644 index 00000000..5c5ca8da --- /dev/null +++ b/encoder/ABI.h @@ -0,0 +1,15 @@ + + +#ifndef ENCODER_ABI_H_ +#define ENCODER_ABI_H_ + +#include "datatypes.h" +#include "encoder/encoder_datatype.h" + +void ABI_deinit(void); +encoders_ret_t ABI_init(ABI_config_t *abi_config); + +float ABI_read_deg(void); + + +#endif /* ENCODER_ABI_H_ */ diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h index 01884960..f556186f 100644 --- a/encoder/encoder_datatype.h +++ b/encoder/encoder_datatype.h @@ -23,7 +23,8 @@ typedef enum{ ENCODERS_TYPE_MT6816, ENCODERS_TYPE_AD2S1205_SPI, ENCODERS_TYPE_SINCOS, - ENCODERS_TYPE_TS5700N8501 + ENCODERS_TYPE_TS5700N8501, + ENCODERS_TYPE_ABI }encoders_type_t; typedef struct{ @@ -38,6 +39,11 @@ typedef struct{ encoders_gpio_t gpio_sck; }encoders_spi_config_t; +typedef struct{ + encoders_gpio_t gpio_A; + encoders_gpio_t gpio_B; +}encoders_incremental_config_t; + typedef struct { bool is_init; encoders_refresh_rate_hz_t refresh_rate_hz; //TODO: REWRITE TO POINTER OF AS504x_config_t @@ -56,10 +62,18 @@ typedef struct { encoders_spi_config_t spi_config; }AD2S1205_config_t; +typedef struct { + bool is_init; + uint32_t counts; + encoders_incremental_config_t incremental_config; +}ABI_config_t; + typedef struct{ encoders_type_t encoder_type; encoders_refresh_rate_hz_t refresh_rate_hz; encoders_spi_config_t spi_config; + uint32_t counts; // FOR INCREMENTAL INTERFACE + encoders_incremental_config_t incremental_config; // FOR INCREMENTAL INTERFACE }encoders_config_t; diff --git a/encoder/encoder_hwconf.c b/encoder/encoder_hwconf.c index 2d8ec3fb..35ed97e7 100644 --- a/encoder/encoder_hwconf.c +++ b/encoder/encoder_hwconf.c @@ -12,10 +12,15 @@ #include "utils.h" #include +#define ENCODERS_CONFIG_UNUSED 0 +#define ENCODERS_INCREMENTAL_UNUSED {{ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}} +#define ENCODERS_SPI_UNUSED {{ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}} +#define ENCODERS_ABI_COUNTER_DEFAULT_VALUE 10000ul + encoders_config_t conf_AS5047 = { ENCODERS_TYPE_AS504x, AS5047_SAMPLE_RATE_HZ, - { + {//SPI {//NSS HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 }, @@ -28,13 +33,15 @@ {//SCK HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 } - } + }, + ENCODERS_CONFIG_UNUSED, + ENCODERS_INCREMENTAL_UNUSED }; encoders_config_t conf_MT6816 = { ENCODERS_TYPE_MT6816, MT6816_SAMPLE_RATE_HZ, - { + {//SPI {//NSS HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 }, @@ -47,13 +54,15 @@ {//SCK HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 } - } + }, + ENCODERS_CONFIG_UNUSED, + ENCODERS_INCREMENTAL_UNUSED }; encoders_config_t conf_AD2S1205 = { ENCODERS_TYPE_AD2S1205_SPI, AD2S1205_SAMPLE_RATE_HZ, - { + {//SPI {//NSS HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 }, @@ -66,6 +75,24 @@ {//SCK HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 } - } + }, + ENCODERS_CONFIG_UNUSED, + ENCODERS_INCREMENTAL_UNUSED }; + encoders_config_t conf_ABI = { + ENCODERS_TYPE_ABI, + ENCODERS_CONFIG_UNUSED, + ENCODERS_SPI_UNUSED, + ENCODERS_ABI_COUNTER_DEFAULT_VALUE, + {// INCREMENTAL PROTOCOL + { + HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 + }, + { + HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 + } + + } + }; + diff --git a/encoder/encoder_hwconf.h b/encoder/encoder_hwconf.h index ec472776..5f060c74 100644 --- a/encoder/encoder_hwconf.h +++ b/encoder/encoder_hwconf.h @@ -16,5 +16,6 @@ extern encoders_config_t conf_AS5047; extern encoders_config_t conf_MT6816; extern encoders_config_t conf_AD2S1205; +extern encoders_config_t conf_ABI; #endif /* ENCODER_ENCODER_HWCONF_H_ */ diff --git a/encoder/encoders.c b/encoder/encoders.c index 8d1b6219..7436c022 100644 --- a/encoder/encoders.c +++ b/encoder/encoders.c @@ -74,6 +74,26 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) index_found = true; return ENCODERS_OK; } + else if(encoder_config->encoder_type == ENCODERS_TYPE_ABI) + { + ABI_config_t abi_config; + encoders_ret_t encoder_ret; + + abi_config.incremental_config = encoder_config->incremental_config; + abi_config.counts = encoder_config->counts; + + encoder_ret = ABI_init(&abi_config); + + if(ENCODERS_OK != encoder_ret || !abi_config.is_init) + { + encoder_type_now = ENCODERS_TYPE_NONE; + index_found = false; + return ENCODERS_ERROR; + } + encoder_type_now = ENCODERS_TYPE_ABI; + index_found = true; + return ENCODERS_OK; + } else { encoder_type_now = ENCODERS_TYPE_NONE; @@ -97,6 +117,10 @@ float encoders_read_deg(void) { return AD2S1205_read_deg(); } + else if(encoder_type_now == ENCODERS_TYPE_ABI) + { + return ABI_read_deg(); + } return 0.0; } @@ -124,6 +148,10 @@ void encoders_deinit(void) { AD2S1205_deinit(); } + else if(encoder_type_now == ENCODERS_TYPE_ABI) + { + ABI_deinit(); + } } float encoders_spi_get_error_rate(void) { diff --git a/encoder/encoders.h b/encoder/encoders.h index 55068f9e..b38a819f 100644 --- a/encoder/encoders.h +++ b/encoder/encoders.h @@ -8,6 +8,7 @@ #include "encoder/MT6816.h" #include "encoder/AD2S1205.h" #include "encoder/encoder_hwconf.h" +#include "encoder/ABI.h" #include "hal.h" // GENERIC GLOBAL void encoders_deinit(void);