09.实现close_loop_current_control闭环电流控制

09.实现close_loop_current_control闭环电流控制

具体实现

BLDCMotor.cpp

#include "Arduino.h"
#include "BLDCMotor.hpp"

BLDCMotor::BLDCMotor(uint8_t polePairs, float power_supply_voltage)
{
    this->polePairs = polePairs;
    this->power_supply_voltage = power_supply_voltage;
}
void BLDCMotor::connectDriver(BLDCDriver *driver)
{
    this->driver = driver;
}
void BLDCMotor::connectSensor(Sensor *sensor)
{
    this->sensor = sensor;
}
void BLDCMotor::connectCurrentSensor(CurrentSensor *currentSensor)
{
    this->currentSensor = currentSensor;
}
void BLDCMotor::initFOC()
{
    if (this->currentSensor)
    {
        this->currentSensor->initSensor();
        this->currentSensor->alignSensor();
    }
    if (this->driver)
    {
        this->driver->initDriver();
        this->driver->enableDriver(true);
        this->driver->setPhraseVoltage(0, 0, 0);
    }
    if (this->sensor)
    {
        this->sensor->initSensor();
        this->sensor->connectMotor(this);
        this->sensor->alignSensor();
        this->sensor->init();
        this->sensor->update();
    }
}

void BLDCMotor::loopFOC()
{
    if (this->sensor)
        this->sensor->update();
    // this->open_loop_voltage_control(0, 1.0f);
    this->close_loop_current_control(0.12f);
}

/**
 * @details
 *  电角度=机械角度*极对数
 *  @return [0,2PI] => [0,UINT16_MAX]
 */
uint16_t BLDCMotor::electricalAngle()
{
    return this->sensor->getPositon() * polePairs;
}
/**
 * 设置相电压
 * @param u_d int16_t [-32768,32767] 表示 [-1,1] 精度：1/32768 = 0.0000305
 * @param u_q int16_t [-32768,32767] 表示 [-1,1]
 * @param e_angle uint16_t [0,65535] 表示 [0,2PI] 精度：360°/65535 = 0.00549°
 */
void BLDCMotor::setPhraseVoltage(int16_t u_d, int16_t u_q, uint16_t e_angle)
{
    int16_t sin, cos;
    // 计算三角函数
    _sincos(e_angle, &sin, &cos);
    // 帕克逆变换
    int16_t u_alpha = ((cos * (int32_t)u_d) + (-sin * (int32_t)u_q)) / INT16_MAX;
    int16_t u_beta = ((sin * (int32_t)u_d) + (cos * (int32_t)u_q)) / INT16_MAX;
    // 克拉克逆变换(等幅值形式)
    int16_t u_a = u_alpha;
    int16_t u_b = (-u_alpha + _INT16_SQRT3_ * u_beta / INT16_MAX) / 2;
    int16_t u_c = -(u_a + u_b);
    // 设置相电压
    driver->setPhraseVoltage(u_a, u_b, u_c);
}

void BLDCMotor::open_loop_voltage_control(float target_ud, float target_uq)
{
    float voltage_ud = _constrain(-limit_voltage, target_ud, limit_voltage);
    float voltage_uq = _constrain(-limit_voltage, target_uq, limit_voltage);
    int16_t u_d = this->sensor->directron * this->direction * (voltage_ud / power_supply_voltage * INT16_MAX);
    int16_t u_q = this->sensor->directron * this->direction * (voltage_uq / power_supply_voltage * INT16_MAX);
    uint16_t e_angle = this->electricalAngle();
    this->setPhraseVoltage(u_d, u_q, e_angle);
}
/**
 * 获取Q轴电流
 */
CurrentDQ BLDCMotor::getCurrentDQ()
{
    CurrentDQ i = this->currentSensor->getCurrentDQ(this->electricalAngle());
    return {
        .d = this->current_d_filter(i.d),
        .q = this->current_q_filter(i.q),
    };
}
/**
 * 电流闭环控制
 */
void BLDCMotor::close_loop_current_control(float target)
{
    float target_i_d = 0;
    float target_i_q = _constrain(-limit_current, target, limit_current);
    CurrentDQ current = this->getCurrentDQ();
    float error_d = target_i_d - current.d;
    float error_q = target_i_q - current.q;
    float u_d = this->pid_id_controller(error_d);
    float u_q = this->pid_iq_controller(error_q);
    this->open_loop_voltage_control(u_d, u_q);
}
/**
 * 获取机械角度
 */
float BLDCMotor::shaftAngle()
{
    return shaft_angle_filter(this->sensor->getPositons());
}
/**
 * 获取机械角度
 */
float BLDCMotor::shaftVelocity()
{
    return shaft_velocity_filter(this->sensor->getVelocity());
}

BLDCMotor.hpp

#ifndef __BLDCMotor_H__
#define __BLDCMotor_H__
#include <stdint.h>
#include "foc_utils.h"
#include "BLDCDriver.hpp"
#include "LowPassFilter.hpp"
#include "CurrentSensor.hpp"
#include "Sensor.hpp"
#include "Timer.hpp"
#include "pid.hpp"

class BLDCMotor : Timer
{
    enum MotorDirectrion : int8_t
    {
        UNKNOW = 0,
        ANTI_CLOCK_WISE = 1,
        CLOCK_WISE = -1,
    };

public:
    // 极对数
    uint8_t polePairs;
    // 供电电压
    float power_supply_voltage = 12.0f;
    // 限制电压
    float limit_voltage = 4.0f;
    // 限制速度
    float limit_speed = 100.0f;
    // 限制电流
    float limit_current = 4.0f;
    // directron
    MotorDirectrion direction = MotorDirectrion::ANTI_CLOCK_WISE;
    // filter
    LowPassFilter current_q_filter{100};
    LowPassFilter current_d_filter{100};
    LowPassFilter shaft_angle_filter{20};
    LowPassFilter shaft_velocity_filter{20};
    // pid-controller
    PIDControler pid_iq_controller{10, 50, -0.01, 0, 12};
    PIDControler pid_id_controller{10, 50, -0.01, 0, 12};

private:
    //
    BLDCDriver *driver = nullptr;
    Sensor *sensor = nullptr;
    CurrentSensor *currentSensor = nullptr;

public:
    BLDCMotor(uint8_t polePairs, float power_supply_voltage);
    void connectDriver(BLDCDriver *driver);
    void connectSensor(Sensor *sensor);
    void connectCurrentSensor(CurrentSensor *currentSensor);
    void initFOC();
    void loopFOC();
    /**
     * 获取电角度
     */
    uint16_t electricalAngle();

    /**
     * 设置相电压
     * @param u_d int16_t [-32768,32767] 表示 [-1,1] 精度：1/32768 = 0.0000305
     * @param u_q int16_t [-32768,32767] 表示 [-1,1]
     * @param e_angle uint16_t [0,65535] 表示 [0,2PI] 精度：360°/65535 = 0.00549°
     */
    void setPhraseVoltage(int16_t u_d, int16_t u_q, uint16_t e_angle);
    /**
     * 开环电压控制
     */
    void open_loop_voltage_control(float target_ud, float target_uq);
    /**
     * 获取dq轴电流
     */
    CurrentDQ getCurrentDQ();
    /**
     * 闭环电流控制
     */
    void close_loop_current_control(float target);
    /**
     * 获取机械角度
     */
    float shaftAngle();
    /**
     * 闭环速度控制
     */
    void close_loop_speed_control(float target);
    /**
     * 获取机械角度
     */
    float shaftVelocity();
    /**
     * 闭环位置控制
     */
    void close_loop_position_control(float target);
};
#endif

实现效果

• 使用pid控制器
• 设置i.d电流为0A
• 设置i.q电流为2.5A

堵转时的pid积分控制电流波形

09.close_loop_current_control.ino

#include <Arduino.h>
#include <Wire.h>
#include "BLDCMotor.hpp"
#include "BLDCDriver.hpp"
#include "Sensor.hpp"
#include "foc_utils.h"
#include "typedef.h"
#include "pwm.h"

BLDCMotor motor = BLDCMotor(7, 12);

#define M1_En 8
#define M1_Ua 5
#define M1_Ub 9
#define M1_Uc 6
PwmOut pwmA(D5);
PwmOut pwmB(D9);
PwmOut pwmC(D6);
BLDCDriver driver = BLDCDriver(
    // init driver
    []()
    {
      pinMode(M1_En, OUTPUT); // enable
      // pinMode(M1_Ua, OUTPUT); // a
      // pinMode(M1_Ub, OUTPUT); // b
      // pinMode(M1_Uc, OUTPUT); // c
      // period 50us = 20,000hz; pulse 0 us = 0%
      pwmA.begin(20000.0f, 0.0f);
      pwmB.begin(20000.0f, 0.0f);
      pwmC.begin(20000.0f, 0.0f);
    },
    // enable or disable driver
    [](bool enable)
    {
      digitalWrite(M1_En, enable ? HIGH : LOW); // enable
    },
    // set pwm to driver
    [](int16_t u_a, int16_t u_b, int16_t u_c)
    {
      // 设置占空比
      // [-32768,32768] => [-1,1] => [-50,50] => [0,100]
      pwmA.pulse_perc(u_a / 32768.0f * 50 + 50); // 设置百分比
      pwmB.pulse_perc(u_b / 32768.0f * 50 + 50);
      pwmC.pulse_perc(u_c / 32768.0f * 50 + 50);
    });

#define AS5600_ADDR 0x36
#define AS5600_RAW_ANGLE 0x0c
#define AS5600_ANGLE 0x0e
Sensor sensor = Sensor(
    []()
    {
      Wire.begin();
    },
    []()
    {
      Wire.beginTransmission(AS5600_ADDR);
      Wire.write(AS5600_ANGLE);
      Wire.endTransmission(false);
      Wire.requestFrom(AS5600_ADDR, 2);
      uint16_t data = 0;
      *((uint8_t *)&data + 1) = Wire.read();
      *((uint8_t *)&data + 0) = Wire.read();
      // Wire.endTransmission();
      // as5600 12bit精度，左移4位变成16位
      data <<= 4;
      return data;
    });

#include "CurrentSensor.hpp"

#define M1_Ia A0
#define M1_Ib A2
#define M1_Ic
#define I_R 0.01f // 10mΩ

CurrentSensor currentSensor = CurrentSensor(
    []()
    {
      pinMode(M1_Ia, INPUT);
      pinMode(M1_Ib, INPUT);
      // Arduino 板上的标准分辨率为 10 位 (0-1023)
      analogReadResolution(14); // UNO R4 支持高达 14 位(0-16383)的分辨率
      analogReference(AR_DEFAULT); // 默认参考电压 5 V
      // analogReference(AR_INTERNAL); // 内置参考电压 1.5 V
    },
    []()
    {
      // [0,16383] => [0,5]
      return CurrentABC{
          // i = u/r
          .a = (analogRead(M1_Ia) / 16383.0f * 5.0f - 2.5f) / 0.01f / 50,
          .b = -(analogRead(M1_Ib) / 16383.0f * 5.0f - 2.5f) / 0.01f / 50, // b电路接反了 加符号
      };
    });

void setup()
{
  Serial.begin(115200);

  motor.connectDriver(&driver);
  motor.connectSensor(&sensor);
  motor.connectCurrentSensor(&currentSensor);

  motor.initFOC();
}

void loop()
{
  motor.loopFOC();
}