tuber/robot/modules/sc60224/encoder.cpp

#include "stm32g0xx_hal.h"
#include "encoder.h"
#include "uart_bridge.h"
#include <stdio.h>
#include <string.h>


TIM_HandleTypeDef htim1;

#define TX_BUF_SIZE     60 

#define RESOLUTION      1024

static uint8_t tx_buf[TX_BUF_SIZE];

uint32_t forw_cnt_irq;
uint32_t back_cnt_irq;

uint8_t direction_irq;
uint32_t forw_turns_irq;
uint32_t back_turns_irq;

uint8_t direction = 1;
uint32_t forw_turns = 123456789;
uint32_t back_turns = 987654321;

bool access_turns = true;

//
static void create_data_pack_encoder(void);
static void init_gpio_encoder(void);

//
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_CC_IRQn, 6, 0);
    HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);
    
    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);
}

//
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance->CR1 & TIM_CR1_DIR) {
        direction_irq = 1;
        back_cnt_irq++;
        if (back_cnt_irq >= RESOLUTION) {
            back_cnt_irq = 0;
            back_turns_irq++;
        }
    }
    else {
        direction_irq = 0;
        forw_cnt_irq++;
        if (forw_cnt_irq >= RESOLUTION) {
            forw_cnt_irq = 0;
            forw_turns_irq++;
        }
    }
    
    while (!access_turns) {}

#if 0    
    direction = direction_irq;
    forw_turns = forw_turns_irq;
    back_turns = forw_turns_irq;
#endif    
}

//
static void create_data_pack_encoder(void)
{
    access_turns = false;
    sprintf((char*)tx_buf, "{%u,%u,%u,}\n", forw_turns, back_turns, direction);
    access_turns = true;
}

//
void send_data_pack_encoder(void)
{
    create_data_pack_encoder();
    ub_send_turns_pack(tx_buf, strlen((char*)tx_buf));
}


extern "C" {

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

}