Контроля угла поворота вала редуктора DC мотора

int max_counter = 704; // количество тиков на оборот вала редуктора
 
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if(htim == &htim2)
    {
        GetEncoderData();
    }
}
 
void GetEncoderData()
{
    cur_pulse = __HAL_TIM_GET_COUNTER(&htim2); // counter = TIM2 -> CNT (get by register)
    mot_direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(&htim2);
 
    if (prv_pulse < cur_pulse)
        counter++;
    else
        counter--;
 
    if (counter > max_counter) counter = 0;
    if (counter < -max_counter) counter = 0;
 
    prv_pulse = cur_pulse;
 
    angle = ((float)abs(counter) / ((float)max_counter)) * 360.0;
    // speed = ((float)abs(counter) / ((float)max_counter)) * 60.0;
 
    if (angle > 360)
        angle = 0;
 
}

float kp = 3.3;
float ki = 0.45;
float kd = 0.9;
float allw_error = 1.5; // разрешенная ошибка ПИД-а
 
void PIDCalculator()
{
    if (prv_setted_angle != setted_angle)
        stop_flag = 0;
 
    __e = abs(e);
 
    e = setted_angle - angle;
    de = (e - e_) / dt;
    def = 0.833 * def_ + 0.166 * de;
    ie = ie + e * dt;
    pid_result = kp * e + ki * ie + kd * def;
    pid_result = fabsf(pid_result);
 
    if (pid_result > htim2.Init.Period)
        pid_result = htim2.Init.Period;
 
    if (e > 0 && stop_flag == 0)
    {
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, 1); // 1
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, 0); // 0
 
        if (abs((int)e) < allw_error)
        {
            stop_flag = 1;
            prv_setted_angle = setted_angle;
        }
    }
    else if (e < 0 && stop_flag == 0)
    {
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, 0); // 0
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, 1); // 1
 
        if (abs(e) < allw_error)
        {
            stop_flag = 1;
            prv_setted_angle = setted_angle;
        }
    }
    else if (abs(e) < allw_error)
    {
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, 1);
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_12, 1);
 
        stop_flag = 1;
        prv_setted_angle = setted_angle;
        speed = 0;
    }
 
    // Limiting integration sum for integral anti wind-up.
    if (ie > 100000)
        ie = 100000;
    else if (ie < -100000)
        ie = -100000;
 
    if (abs(e) > allw_error)
        stop_flag = 0;
 
    if (stop_flag == 1)
        pid_result = 0;
 
 
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pid_result);
 
    def_ = def;
    e_ = e;
    t += dt;
}

Hal_Delay(1)