【无感FOC开环拖动V/F和I/F】【2 代码实践】

目录

• • 准备数学函数
  • V/F代码
  • I/F代码
  • 启动开环拖动

本节代码在该硬件上测试运行：点此查看硬件

在该FOC框架基础上修改：stm_foc_2

本节完整keil工程：openloop分支

准备数学函数

一些数学函数比如说把角度归一化之类的：

//my_math.c
//将角度归一化到0~2pi之间
float wrap_rad(float angle)
{
    float r = fmodf(angle, 2.0f * PI);
    if (r < 0.0f)
        r += (2.0f * PI);
    return r;
}

// 快速将角度归一化到0~2pi之间，要求两次调用之间angle的变化不超过2pi
float wrap_0_2pi_fast(float angle)
{
    if (angle >= (2.0f * PI))
    {
        angle -= (2.0f * PI);
    }
    else if (angle < 0.0f)
    {
        angle += (2.0f * PI);
    }
    return angle;
}

// 将value限制到min和max之间
float clamp(float value, float min, float max)
{
    return (value < min) ? min : ((value > max) ? max : value);
}

// 饱和函数
float sat(float value)
{
    return clamp(value, -1.0f, 1.0f);
}

//将弧度转换为三角函数查找表角度
int rad_to_index(float angle)
{
    int index = angle * sin_table_size / (2.0f * PI);
    return (index & (sin_table_size - 1));
}

//将三角函数查找表索引转换为弧度
float index_to_rad(int index)
{
    return index * (2.0f * PI) / sin_table_size;
}

V/F代码

根据上节理论文章，把以下两个公式翻译成C语言即可：
u q = K + ω ψ f ω = ω + a c c ∗ Δ t u_q=K+\omega\psi_f\\\omega=\omega+acc*\Delta t uq=K+ωψfω=ω+acc∗Δt

由于旋转有方向，因此要先根据目标速度正负判断出方向。转子角度根据速度积分得到。

VF函数打算放在adc中断里执行，这样VF能够得到一个精准的计算周期。

motor_control_context.speed是无感速度环目标转速。

motor_adc_dt是进入adc中断间隔时间。

//openloop.c
#define OPENLOOP_ACC_E (POLE_PAIRS * 100.0f)       // VF电角速度爬升速率，弧度每秒
#define OPENLOOP_MAX_SPEED_E (POLE_PAIRS * 70.0f) // VF最大电角速度，不要太大，开环可能跟不上

#define OPENLOOP_VF_K 2.0f // VF控制的K常数
void openloop_VF()
{
    int dir = motor_control_context.speed > 0 ? 1 : -1;
    if (dir == 1)
        openloop_speed_e += OPENLOOP_ACC_E * motor_adc_dt; //\omega=\omega+acc*\Delta t
    else
        openloop_speed_e -= OPENLOOP_ACC_E * motor_adc_dt;
    openloop_speed_e = clamp(openloop_speed_e, -OPENLOOP_MAX_SPEED_E, OPENLOOP_MAX_SPEED_E);

    openloop_theta_e += openloop_speed_e * motor_adc_dt; // 积分出转子角度
    openloop_theta_e = wrap_0_2pi_fast(openloop_theta_e);

    float K = 0;
    if (dir == 1)
        K = OPENLOOP_VF_K;
    else
        K = -OPENLOOP_VF_K;

    openloop_vf_output = (K + PHI_F * openloop_speed_e) / (U_BUS / SQRT3); // u_q=K+\omega\psi_f

    foc_forward_d_input = &zero_const; // svpwm的d输入设置为0
    foc_forward_q_input = &openloop_vf_output; // svpwm的q输入设置为VF的输出
}

在adc中断里，相对于stm_foc_2有感框架，添加了VF调用：

//adc.c
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
  if (hadc->Instance == ADC1)
  {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET); // 为了测量代码耗时

	//编码器读取，可用于对比观察
    extern uint8_t mt6701_rx_data[3];
    encoder_angle_hex = (mt6701_rx_data[1] >> 2) | (mt6701_rx_data[0] << 6);
    rotor_logic_angle_hex_norm =
        (rotor_logic_angle_hex & ((1 << ENCODER_BITS) - 1)) >> (ENCODER_BITS - sin_table_bit); // 三角函数查找表是11bit

    extern DMA_HandleTypeDef hdma_spi1_rx;
    extern DMA_HandleTypeDef hdma_spi1_tx;
    (&hspi1)->State = HAL_SPI_STATE_READY;
    (&hdma_spi1_rx)->State = HAL_DMA_STATE_READY;
    (&hdma_spi1_tx)->State = HAL_DMA_STATE_READY;
    __HAL_UNLOCK(&hdma_spi1_rx);
    __HAL_UNLOCK(&hdma_spi1_tx);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
    HAL_SPI_TransmitReceive_DMA(&hspi1, mt6701_rx_data, mt6701_rx_data, sizeof(mt6701_rx_data));

    openloop_VF();
    int theta_e_hex_norm = rad_to_index(openloop_theta_e);
    
    if (motor_control_context.type != control_type_null)
        foc_forward(*foc_forward_d_input, *foc_forward_q_input, theta_e_hex_norm);

    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET); // 为了测量代码耗时
  }
}

在本节使用的硬件上，用编码器速度进行对比，会发现V/F运行下的开环转速和编码器算出来的转速是对的上的。修改K的大小，用手轻捏电机，能够体会到K越大，电机的开环拖动的力越大。

I/F代码

转子电角速度和转子电角度和V/F一样，速度逐渐提高，角度从速度积分而来。电流环的输入iq_ref，可以随着时间逐渐增大（这样起步柔和一些，iq_ref斜率要快一点），也可以固定一个数值。

//openloop.c
#define OPENLOOP_IF_ACC 5.0f    // IF电流爬升速率，A/s
#define OPENLOOP_IF_IQ_MAX 0.5f // IF最大电流，A
float openloop_iq_ref=0.1f; //给一点起步q轴电流
void openloop_IF()
{
    int dir = motor_control_context.speed > 0 ? 1 : -1;
    if (dir == 1)
        openloop_speed_e += OPENLOOP_ACC_E * motor_adc_dt; //\omega=\omega+acc*\Delta t
    else
        openloop_speed_e -= OPENLOOP_ACC_E * motor_adc_dt;
    openloop_speed_e = clamp(openloop_speed_e, -OPENLOOP_MAX_SPEED_E, OPENLOOP_MAX_SPEED_E);

    openloop_theta_e += openloop_speed_e * motor_adc_dt; // 积分出转子角度
    openloop_theta_e = wrap_0_2pi_fast(openloop_theta_e);

    // IF目标电流逐渐爬升
     if (dir == 1)
         openloop_iq_ref += (OPENLOOP_IF_ACC / MOTOR_MAX_CURRENT) * motor_adc_dt;
     else
         openloop_iq_ref -= (OPENLOOP_IF_ACC / MOTOR_MAX_CURRENT) * motor_adc_dt;

    openloop_iq_ref =
        clamp(openloop_iq_ref, -OPENLOOP_IF_IQ_MAX / MOTOR_MAX_CURRENT, OPENLOOP_IF_IQ_MAX / MOTOR_MAX_CURRENT);

    //IF固定目标电流
    // if (dir == 1)
    //     openloop_iq_ref = OPENLOOP_IF_IQ_MAX / MOTOR_MAX_CURRENT;
    // else
    //     openloop_iq_ref = -OPENLOOP_IF_IQ_MAX / MOTOR_MAX_CURRENT;

    torque_loop_d_input = &zero_const;
    torque_loop_q_input = &openloop_iq_ref;
    foc_forward_d_input = &torque_loop_d_out;
    foc_forward_q_input = &torque_loop_q_out;
}

I/F同样放在adc中断里运行：

//adc.c
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
  if (hadc->Instance == ADC1)
  {
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET); // 为了测量代码耗时

    extern uint8_t mt6701_rx_data[3];
    encoder_angle_hex = (mt6701_rx_data[1] >> 2) | (mt6701_rx_data[0] << 6);
    rotor_logic_angle_hex_norm =(rotor_logic_angle_hex & ((1 << ENCODER_BITS) - 1)) >> (ENCODER_BITS - sin_table_bit); // 三角函数查找表是11bit

    // 读取编码器角度
    extern DMA_HandleTypeDef hdma_spi1_rx;
    extern DMA_HandleTypeDef hdma_spi1_tx;
    (&hspi1)->State = HAL_SPI_STATE_READY;
    (&hdma_spi1_rx)->State = HAL_DMA_STATE_READY;
    (&hdma_spi1_tx)->State = HAL_DMA_STATE_READY;
    __HAL_UNLOCK(&hdma_spi1_rx);
    __HAL_UNLOCK(&hdma_spi1_tx);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
    HAL_SPI_TransmitReceive_DMA(&hspi1, mt6701_rx_data, mt6701_rx_data, sizeof(mt6701_rx_data));

    // float u_1 = ADC_REFERENCE_VOLT * ((float)hadc1.Instance->JDR1 / (1 << ADC_BITS) - 0.5f); //-0.5就是-50%
    // float u_2 = ADC_REFERENCE_VOLT * ((float)hadc2.Instance->JDR1 / (1 << ADC_BITS) - 0.5f);
    float u_1 = (float)hadc1.Instance->JDR1 * (ADC_REFERENCE_VOLT / (1 << ADC_BITS)) - ADC_REFERENCE_VOLT / 2.0f; // 减少浮点运算
    float u_2 = (float)hadc2.Instance->JDR1 * (ADC_REFERENCE_VOLT / (1 << ADC_BITS)) - ADC_REFERENCE_VOLT / 2.0f;
    // float i_1 = u_1 / R_SHUNT / OP_GAIN;//(R_SHUNT * OP_GAIN)正好是1，可以省略
    // float i_2 = u_2 / R_SHUNT / OP_GAIN;
    // motor_i_u = i_1;
    // motor_i_v = i_2;
    motor_i_u = u_1; 
    motor_i_v = u_2;

    arm_clarke_f32(motor_i_u, motor_i_v, &motor_i_alpha, &motor_i_beta);

    int theta_e_hex_norm = rotor_logic_angle_hex_norm;
    if (motor_control_context.type == control_type_openloop)
    {
        if (openloop_type == openloop_type_VF)
            openloop_VF();
        else
            openloop_IF();
        theta_e_hex_norm = rad_to_index(openloop_theta_e);//转子角度切为开环角度
    }
	
    float sin_value = sin_table(theta_e_hex_norm);
    float cos_value = cos_table(theta_e_hex_norm);
    arm_park_f32(motor_i_alpha, motor_i_beta, &motor_i_d, &motor_i_q, sin_value, cos_value);

    //电流环
    torque_loop_d_out = torque_d_loop(*torque_loop_d_input, motor_i_d / MOTOR_MAX_CURRENT);
    torque_loop_q_out = torque_q_loop(*torque_loop_q_input, motor_i_q / MOTOR_MAX_CURRENT);

    //SVPWM
    if (motor_control_context.type != control_type_null)
        foc_forward(*foc_forward_d_input, *foc_forward_q_input, theta_e_hex_norm);

    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET); // 为了测量代码耗时
  }
}

在本节使用的硬件上测试I/F，修改OPENLOOP_IF_IQ_MAX 的大小，用手轻捏电机，能够体会到OPENLOOP_IF_IQ_MAX 越大，电机的开环拖动的力越大。注意，如果空载，负载达不到OPENLOOP_IF_IQ_MAX ，此时电流环会一直努力输出，电机会发热，不要长时间运行。

启动开环拖动

在主函数里的while(1)前面设置好开环反向和开环类型，即可配置开环运行，实际开环代码运行在adc中断里。开环运行随时注意电机的发热情况。

//main.c
...
  openloop_dir = 1;
  openloop_type = openloop_type_IF;
  motor_control_context.type = control_type_openloop;
  while(1)
  {
...

---

开环拖动作为无感FOC启动阶段，能够将电机启动到一定转速，当开环转速达到一定阈值后，即可切入无感观测器。

开环拖动依然需要进行初始D轴定位，否则容易出现失步，而且开环拖动依然会存在不同程度的发热，这是开环的缺点。无感FOC全速度闭环运行也有办法，就是高频注入法，这个不在本系列讲解。