电角度与机械角度

  1. float zero_mechanical_angle = 0.36;
  2. float zero_electrical_angle = 0.891460538;
  4. float last_mechanical_angle = 0;
  5. uint32_t pre_tick;

机械角：机械角就是空间几何角度，用圆规能够测量到的。因为电机是圆形的，所以它的范围是0-360度。
电角度：从定子来看，定子电流变化一个完整的周期定义为电角度的0-360度，称为一个电周期。一个电周期可以在空间360度完成也可以在空间180度或者90度或者60度完成,这和电动机(发电机也是这样）绕组布置成几对磁级有关。
从转子来看, 转子上分布有N对磁钢，电角度以一对磁钢为基准，形成电磁场的绕组经过一对磁钢的N极、S极、再回到另一对磁钢的N极时，完成从N、S、再到 N极一对磁钢的跨越，这就是电角度的一周。从N极只到 S极，则转子只运行了180°。一台电动机的转子转动一圈有多少电角度，视磁钢对数而定，只有一对磁钢的，电角度和自然角度一致，有两对磁钢的，为2X360°，5对磁钢的，为5X360°，依此类推。
电角度=机械角度×极对数
FOC - 图1

FOC的电角度校准

  1. void FOC_electrical_angle_calibration() {
  2.     // FOC calibration
  3.     FOC_SVPWM(0, 8, 0);//为什么8？
  4.     HAL_Delay(400);
  5.     zero_electrical_angle = FOC_electrical_angle();
  6.     HAL_Delay(100);
  7.     FOC_SVPWM(0, 0, 0);
  8. }

FOC_SVPWM

image.png
去掉了电流采样，所以简单了很多，有位置传感器，所以不用观测器估算位置。对空间矢量作用时间可以直接利用下面的公式
image.png
image.png
Udc表示电源电压（在代码中是voltage_limit），Uref表示设置的力矩大小（在代码中是target_voltage），Ts表示PWM周期（代码中没有把Ts体现出来，代码中的T1、T2是周期的百分比）。

  1. void FOC_SVPWM(float Uq, float Ud, float angle) {
  3.     int sector;
  5.     // Nice video explaining the SpaceVectorModulation (FOC_SVPWM) algorithm
  6.     // https://www.youtube.com/watch?v=QMSWUMEAejg
  8.     // the algorithm goes
  9.     // 1) Ualpha, Ubeta
  10.     // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
  11.     // 3) angle_el = atan2(Ubeta, Ualpha)
  12.     //
  13.     // equivalent to 2)  because the magnitude does not change is:
  14.     // Uout = sqrt(Ud^2 + Uq^2)
  15.     // equivalent to 3) is
  16.     // angle_el = angle_el + atan2(Uq,Ud)
  18.     float Uout = _sqrt(Ud * Ud + Uq * Uq) / VOLTAGE_LIMIT; // Actually, Uout is a ratio
  19.     angle = _normalizeAngle(angle + atan2(Uq, Ud));
  21.     // find the sector we are in currently
  22.     sector = floor(angle / _PI_3) + 1;
  23.     // calculate the duty cycles
  24.     float T1 = _SQRT3 * _sin(sector * _PI_3 - angle) * Uout;
  25.     float T2 = _SQRT3 * _sin(angle - (sector - 1.0f) * _PI_3) * Uout;
  26.     // two versions possible
  27. //    float T0 = 0; // pulled to 0 - better for low power supply voltage
  28.     float T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
  31.     // calculate the duty cycles(times)
  32.     float Ta, Tb, Tc;
  33.     switch (sector) {
  34.         case 1:
  35.             Ta = T1 + T2 + T0 / 2;
  36.             Tb = T2 + T0 / 2;
  37.             Tc = T0 / 2;
  38.             break;
  39.         case 2:
  40.             Ta = T1 + T0 / 2;
  41.             Tb = T1 + T2 + T0 / 2;
  42.             Tc = T0 / 2;
  43.             break;
  44.         case 3:
  45.             Ta = T0 / 2;
  46.             Tb = T1 + T2 + T0 / 2;
  47.             Tc = T2 + T0 / 2;
  48.             break;
  49.         case 4:
  50.             Ta = T0 / 2;
  51.             Tb = T1 + T0 / 2;
  52.             Tc = T1 + T2 + T0 / 2;
  53.             break;
  54.         case 5:
  55.             Ta = T2 + T0 / 2;
  56.             Tb = T0 / 2;
  57.             Tc = T1 + T2 + T0 / 2;
  58.             break;
  59.         case 6:
  60.             Ta = T1 + T2 + T0 / 2;
  61.             Tb = T0 / 2;
  62.             Tc = T1 + T0 / 2;
  63.             break;
  64.         default:
  65.             // possible error state
  66.             Ta = 0;
  67.             Tb = 0;
  68.             Tc = 0;
  69.     }
  71.     // Ta, Tb, Tc range [0,1]
  73.     _writeDutyCycle3PWM(Ta, Tb, Tc);//设置三个通道的占空比
  74. }
  1. void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c) {
  2.     // transform duty cycle from [0,1] to [0,_PWM_RANGE]
  3.     __HAL_TIM_SET_COMPARE(_STM32_TIMER_HANDLE, TIM_CHANNEL_1, _PWM_RANGE*dc_a);
  4.     __HAL_TIM_SET_COMPARE(_STM32_TIMER_HANDLE, TIM_CHANNEL_2, _PWM_RANGE*dc_b);
  5.     __HAL_TIM_SET_COMPARE(_STM32_TIMER_HANDLE, TIM_CHANNEL_3, _PWM_RANGE*dc_c);
  6. }

Clarke&Park

  1. void FOC_Clarke_Park(float Ia, float Ib, float Ic, float angle, float *Id, float *Iq) {
  2.     // Clarke transform
  3.     float mid = (1.f / 3) * (Ia + Ib + Ic);
  4.     float a = Ia - mid;
  5.     float b = Ib - mid;
  6.     float i_alpha = a;
  7.     float i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
  9.     // Park transform
  10.     float ct = _cos(angle);
  11.     float st = _sin(angle);
  12.     *Id = i_alpha * ct + i_beta * st;
  13.     *Iq = i_beta * ct - i_alpha * st;
  15.     return;
  16. }

image.png
c9cf2a6ca6f29954db5cb0a363169fb.jpg09d3e4c0ed33e5a4bbe389f498e2d81.jpg

电角度与机械角度获取

  1. float FOC_get_mechanical_angle() {
  2.     return AS5600_ReadSensorRawData() / SENSOR_VALUE_RANGE * _2PI;
  3. }
  5. float FOC_electrical_angle() {
  7.     return _normalizeAngle(SENSOR_DIRECTION * POLE_PAIR * FOC_get_mechanical_angle() - zero_electrical_angle);
  8. }
  9. // rad/s

速度转化

  1. float FOC_get_velocity() {
  2.     float ts = (float) (HAL_GetTick() - pre_tick) * 1e-3;
  3.     if (ts < 1e-3) ts = 1e-3;
  4.     pre_tick = HAL_GetTick();
  6.     float now_mechanical_angle = FOC_get_mechanical_angle();
  7.     float delta_angle = now_mechanical_angle - last_mechanical_angle;
  8.     last_mechanical_angle = now_mechanical_angle;
  9.     if (fabs(delta_angle) > _PI) {
  10.         if (delta_angle > 0) {
  11.             delta_angle -= _2PI;
  12.         } else {
  13.             delta_angle += _2PI;
  14.         }
  15.     }
  17. //    return delta_angle;
  18.     return delta_angle / ts;
  19. }

开环-电压控制环

  1. void FOC_open_loop_voltage_control_loop(float Uq) {
  2.     float electrical_angle = FOC_electrical_angle();
  4.     // Current sense
  5.     float Id, Iq;
  6.     cs_get_value();
  7.     FOC_Clarke_Park(cs_value[0], cs_value[1], cs_value[2], electrical_angle, &Id, &Iq);
  8.     Id = FOC_low_pass_filter(&lpf_current_d, Id);//低通滤波
  9.     Iq = FOC_low_pass_filter(&lpf_current_q, Iq);
  11.     FOC_SVPWM(Uq, 0, electrical_angle);
  13.     // debug
  14.     printf("%.1f,%.1f,%.1f,%.1f,%.1f\n", cs_value[0], cs_value[1], cs_value[2], Id, Iq);
  15. }

电流采样

  1. uint8_t cs_get_value() {
  2.     for (int i = 0; i < 3; ++i) {
  3.         // Enables ADC, starts conversion of regular group.
  4.         HAL_ADC_Start(&hadc1);
  5.         // Wait for regular group conversion to be completed.
  6.         HAL_ADC_PollForConversion(&hadc1, 100);
  7.         // 判断ADC是否转换成功
  8. //        if (HAL_IS_BIT_SET(HAL_ADC_GetState(&hadc1), HAL_ADC_STATE_REG_EOC)) {
  10.         // Get ADC regular group conversion result.
  11.         // Reading register DR automatically clears ADC flag EOC (ADC group regular end of unitary conversion).
  12.         // 该函数读取寄存器DR同时自动清除了EOC(End Of unitary Conversation)标志位
  14. //        cs_value[i] = HAL_ADC_GetValue(&hadc1) - cs_zero_value;
  15.         cs_value[i] = FOC_low_pass_filter(lpf_cs + i, HAL_ADC_GetValue(&hadc1) - cs_zero_value);
  16. //        } else {
  17. //            HAL_ADC_Stop(&hadc1);
  18.         // 如果转换失败就返回-1
  19. //            return -1;
  20. //        }
  21.     }
  22.     // Stop ADC conversion of regular group, disable ADC peripheral.
  23.     HAL_ADC_Stop(&hadc1);
  25. //    cs_value[0] = -cs_value[1] - cs_value[2] + 3 * cs_zero_value;
  26.     return 0;
  27. }

电流环控制

  1. void FOC_current_control_loop(float target_Iq){
  2.     static float electrical_angle;
  3.     electrical_angle = FOC_electrical_angle();
  5.     // Current sense
  6.     static float Id, Iq;
  7.     cs_get_value();
  8.     FOC_Clarke_Park(cs_value[0], cs_value[1], cs_value[2], electrical_angle, &Id, &Iq);
  9.     Id = FOC_low_pass_filter(&lpf_current_d, Id);
  10.     Iq = FOC_low_pass_filter(&lpf_current_q, Iq);
  12.     // back feed
  13.     static float Uq, Ud;
  14.     Uq = pid_get_u(&pid_current_q, target_Iq, Iq);
  15.     Ud = pid_get_u(&pid_current_d, 0, Id);
  17.     // front feed
  18.     Uq += 0.008 * Iq;
  20.     FOC_SVPWM(Uq, Ud, electrical_angle);
  22.     // debug
  23. //    printf("%.1f,%.1f,%.1f,%.1f,%.1f\n", cs_value[0], cs_value[1], cs_value[2], Id, Iq);
  24. }

速度环控制

  1. void FOC_velocity_control_loop(float target_velocity) {
  2.     static float now_velocity;
  3.     now_velocity = FOC_low_pass_filter(&lpf_velocity, FOC_get_velocity());
  4.     float Uq = pid_get_u(&pid_velocity, target_velocity, now_velocity);
  5.     float electrical_angle = FOC_electrical_angle();
  6.     FOC_SVPWM(Uq, 0, electrical_angle);
  8. //    cs_get_value();
  9. //    printf("%d,%d,%d\n", cs_value[0], cs_value[1], cs_value[2]);
  10. }

位置环控制

  1. void FOC_position_control_loop(float target_angle) {
  2.     target_angle = _normalizeAngle(target_angle);
  3.     float now_angle = _normalizeAngle(FOC_get_mechanical_angle() - zero_mechanical_angle);
  4. //    float now_angle = FOC_get_mechanical_angle();
  6.     float angle_error = target_angle - now_angle;
  7.     if (angle_error < -_PI) target_angle += _2PI;
  8.     else if (angle_error > _PI) target_angle -= _2PI;
  10.     float target_velocity = pid_get_u(&pid_position, target_angle, now_angle);
  11.     float now_velocity = FOC_low_pass_filter(&lpf_velocity, FOC_get_velocity());
  12.     float Uq = pid_get_u(&pid_velocity, target_velocity, now_velocity);
  13.     float electrical_angle = FOC_electrical_angle();
  14.     FOC_SVPWM(Uq, 0, electrical_angle);
  16.     //printf("%.2f,%.2f,%.2f,%.2f\n", target_angle, now_angle, target_velocity, now_velocity);
  17. }

模拟弹簧

  1. void FOC_spring_loop(float target_angle, PID_Datatype *pid) {
  2.     target_angle = _normalizeAngle(target_angle);
  3.     float now_angle = _normalizeAngle(FOC_get_mechanical_angle() - zero_mechanical_angle);
  5.     float angle_error = target_angle - now_angle;
  6.     if (angle_error < -_PI) target_angle += _2PI;
  7.     else if (angle_error > _PI) target_angle -= _2PI;
  9.     float Uq = FOC_low_pass_filter(&lpf_spring, pid_get_u(pid, target_angle, now_angle));
  10.     float electrical_angle = FOC_electrical_angle();
  11.     FOC_SVPWM(Uq, 0, electrical_angle);
  12. }

模拟旋钮

  1. void FOC_knob_loop(uint8_t sector_num) {
  2.     float now_angle = _normalizeAngle(FOC_get_mechanical_angle() - zero_mechanical_angle);
  3.     uint8_t now_sector = (uint8_t) floor(now_angle * sector_num / _2PI);
  4.     float target_angle = now_sector * _2PI / sector_num + _PI / sector_num;
  6.     float angle_error = target_angle - now_angle;
  7.     if (angle_error < -_PI) target_angle += _2PI;
  8.     else if (angle_error > _PI) target_angle -= _2PI;
  10.     float Uq = pid_get_u(&pid_knob, target_angle, now_angle);
  12.     float electrical_angle = FOC_electrical_angle();
  13.     FOC_SVPWM(Uq, 0, electrical_angle);
  15.     char data = now_sector + '0';
  17.     HAL_UART_Transmit(&huart1, &data, 1, 0xff);
  18. }