tuber/robot/modules/sc60224/encoder.cpp

#include "stm32g0xx_hal.h"
#include "encoder.h"


TIM_HandleTypeDef htim1;

#define TEST_BUF_LEN    256
uint32_t test_index;
uint32_t test_buf[TEST_BUF_LEN];

#define RESOLUTION      1024

uint8_t direction;
uint32_t forw_cnt;
uint32_t back_cnt;
uint32_t forw_turns;
uint32_t back_turns;


//
void init_encoder(void)
{
    TIM_Encoder_InitTypeDef sConfig = {0};
    TIM_MasterConfigTypeDef sMasterConfig = {0};
    
    __HAL_RCC_TIM1_CLK_ENABLE();
    
    init_gpio_encoder();

    htim1.Instance = TIM1;
    htim1.Init.Prescaler = 0;
    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim1.Init.Period = 65535;
    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim1.Init.RepetitionCounter = 0;
    htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; //TIM_AUTORELOAD_PRELOAD_ENABLE;
    
    sConfig.EncoderMode = TIM_ENCODERMODE_TI1; //TIM_ENCODERMODE_TI12;
    
    sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
    sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
    sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
    sConfig.IC1Filter = 0;
    
    sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
    sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
    sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
    sConfig.IC2Filter = 0;
    
    HAL_TIM_Encoder_Init(&htim1, &sConfig);
    
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    
    HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig);
    
    //HAL_NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 6, 0);
    //HAL_NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn);
    
    //__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
    
    HAL_NVIC_SetPriority(TIM1_CC_IRQn, 6, 0);
    HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);
    
    //HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
    //HAL_TIM_Encoder_Start_IT(&htim1, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start_IT(&htim1, TIM_CHANNEL_1);
    
    
}

//
void init_gpio_encoder(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    
    GPIO_InitStruct.Pin = GPIO_PIN_8;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF2_TIM1;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = GPIO_PIN_3;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
    
#if 0    
    GPIO_InitStruct.Pin = GPIO_PIN_0;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF1_TIM1; //GPIO_AF2_TIM1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
    
    GPIO_InitStruct.Pin = GPIO_PIN_3;
    GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
#endif    
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static uint32_t cnt = 0;
    
    cnt++;
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance->CR1 & TIM_CR1_DIR) {
        direction = 1;
        back_cnt++;
        if (back_cnt >= RESOLUTION) {
            back_cnt = 0;
            back_turns++;
        }
    }
    else {
        direction = 0;
        forw_cnt++;
        if (forw_cnt >= RESOLUTION) {
            forw_cnt = 0;
            forw_turns++;
        }
    }
    
#if 0    
    //test_buf[test_index++] = (uint16_t)TIM1->CNT;
    test_buf[test_index++] = (uint16_t)TIM1->CCR1;
    
    if (test_index == TEST_BUF_LEN)
        test_index = 0;
#endif
}


extern "C" {
void TIM1_BRK_UP_TRG_COM_IRQHandler(void)
{
    HAL_TIM_IRQHandler(&htim1);
    
#if 0  
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
    {
        TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
        
        
        //encDir = (TIM1->CR1 & TIM_CR1_DIR ? true : false);

#if 0        
        if (!encInit)
            return;
        
        xSemaphoreGiveFromISR(semphEncoder, &xHigherPriorityTaskWoken);
#endif        
    }
#endif    
    //HAL_TIM_IRQHandler(&htim1);
}

void TIM1_CC_IRQHandler(void)
{
    HAL_TIM_IRQHandler(&htim1);
}

}