STM32G4-DRV8301-FOC/FOC_Related/ABZ/ABZ.c

#include "tim.h"
#include "ABZ.h"
#include "foc.h"

int realCnt = 0;

ABZ_Encoder encoderDef;

// 变量声明
volatile int32_t ABZ_pulse_count = 0;
volatile int32_t ABZ_prev_pulse_count = 0;
volatile float ABZ_speed = 0.0f;
volatile float ABZ_angle = 0.0f;

// 假设编码器的每转脉冲数
#define ENCODER_PULSES_PER_REV 4096

// 定时器周期 (秒)
#define TIMER_PERIOD 1.0f

void TIM2_M1_ABZ(void)
{
    // 启动编码器模式
    HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);

    // 启动索引中断
    HAL_TIMEx_EnableEncoderIndex(&htim2);
}

void TIM3_M2_ABZ(void)
{
    // 启动编码器模式
    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);

    // 启动索引中断
    HAL_TIMEx_EnableEncoderIndex(&htim3);
}

void Count_M1_ABZ(void)
{
    // Debug：读取编码器计数值
    volatile int32_t encoder_count = __HAL_TIM_GET_COUNTER(&htim2);
}

void Count_M2_ABZ(void)
{
    // Debug：读取编码器计数值
    volatile int32_t encoder_count = __HAL_TIM_GET_COUNTER(&htim3);
}

//// 中断服务函数
// void HAL_TIMEx_EncoderIndexCallback(TIM_HandleTypeDef *htim)
//{
//     if (htim->Instance == TIM3)
//     {
//         // 索引信号中断处理逻辑
//         // 可以在这里重置编码器计数值，或者记录当前计数值
//         __HAL_TIM_SET_COUNTER(htim, 0);
//         // printf("Encoder Index detected!\r\n");
//     }
// }

// 电机速度和角度计算
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM3)
    {
        // 清除定时器更新中断标志
        __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
        // 获取当前脉冲计数
        ABZ_pulse_count = __HAL_TIM_GET_COUNTER(&htim3);

        // 计算脉冲增量
        int32_t pulse_delta = ABZ_pulse_count - ABZ_prev_pulse_count;

        // 计算速度 (PPS: 脉冲每秒)
        // 假设定时器周期为1秒，则速度为脉冲增量
        ABZ_speed = (float)pulse_delta / TIMER_PERIOD;

        // 计算角度 (单位：度)
        // 角度 = 当前计数值 / 每转脉冲数 * 360度
        ABZ_angle = (float)(ABZ_pulse_count % ENCODER_PULSES_PER_REV) / ENCODER_PULSES_PER_REV * 360.0f;

        // 更新上次脉冲计数
        ABZ_prev_pulse_count = ABZ_pulse_count;
    }
}

int UpdataEncoderCnt(void)
{
    // 启动编码器模式
    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);

    // 启动索引中断
    HAL_TIMEx_EnableEncoderIndex(&htim3);
    // 获取电机读数

    encoderDef.cnt = (int32_t)__HAL_TIM_GET_COUNTER(&htim3);

    encoderDef.preCnt = encoderDef.cnt;
    // 计算电机偏移量
    encoderDef.incCnt = ModifyIncCnt(encoderDef.cnt - encoderDef.preCnt);
    // 获取真实的编码器计数值
    realCnt = encoderDef.cnt - encoderDef.offsetCnt;
    // 当计数值为负数时，加上一圈的计数数
    while (realCnt < 0)
    {
        realCnt += ENCODER_PULSES_PER_REV * 4;
    }
    // 计算真实的角度值
    encoderDef.angle = encoderDef.cnt * 2 * PI / ENCODER_PULSES_PER_REV / 4;

    // 计算电角度
    encoderDef.elecAngle = _normalizeAngle(encoderDef.angle);

    // 返回编码器读数
    return encoderDef.cnt;
}

//// 3. 定义中断服务程序
// void TIM3_IRQHandler(void)
//{
//     HAL_TIM_IRQHandler(&htim3);
// }

int16_t ModifyIncCnt(int16_t delta)
{
    // 如果增量超过了一半计数范围，说明是溢出，需要进行调整
    if (delta > ENCODER_HALF_COUNT)
    {
        delta -= (ENCODER_MAX_COUNT + 1);
    }
    else if (delta < -ENCODER_HALF_COUNT)
    {
        delta += (ENCODER_MAX_COUNT + 1);
    }
    return delta;
}