跳至主要內容

FOC算法实现过程记录:12.重构:有感FOC驱动完整实现

丁毅桂小于 1 分钟

12.重构

实现效果

A BiliBili video

具体实现

12.refactor_all.ino
#include <Arduino.h>
#include <Wire.h>
#include "BLDCMotor.hpp"
#include "BLDCDriver.hpp"
#include "Sensor.hpp"
#include "foc_utils.h"
#include "arduino/ArduinoBLDCDriver.hpp"
#include "arduino/ArduinoCurrentSensor.hpp"
#include "arduino/ArduinoCommand.hpp"
#include "arduino/AS5600Sensor.hpp"
#include "Command.hpp"

#define Ua 5
#define Ub 9
#define Uc 6
#define En 8

#define Ia A0
#define Ib A2
#define I_R 0.01f // 10mΩ
#define I_A 50    // 50倍

BLDCMotor motor = BLDCMotor(7, 12);
Sensor *sensor = new AS5600Sensor();
BLDCDriver *driver = new ArduinoBLDCDriver(Ua, Ub, Uc, En);
CurrentSensor *currentSensor = new ArduinoCurrentSensor(Ia, Ib, I_R, I_A);
ArduinoCommand command;

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

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

  command.connectMotor(&motor);
  command.connectSerial(&Serial);
  motor.connectCommand(&command);

  motor.initFOC();
  motor.setMode(BLDCControlMode::Current);
}

void loop()
{
  motor.loopFOC();
  command.update();
}
BLDCMotor.hpp
#ifndef __BLDCMotor_H__
#define __BLDCMotor_H__
#include <stdint.h>
#include "foc_utils.h"
#include "BLDCDriver.hpp"
#include "Sensor.hpp"
#include "CurrentSensor.hpp"
#include "Command.hpp"
#include "PIDControler.hpp"
#include "LowPassFilter.hpp"

enum BLDCControlMode : uint8_t
{
    Unknow = 0,
    Voltage = 1,
    Current = 2,
    VelocityCurrent = 3,
    PositionCurrent = 4,
    PositionVelocityCurrent = 5,
};
enum MotorDirectrion : int8_t
{
    UNKNOW = 0,
    ANTI_CLOCK_WISE = 1,
    CLOCK_WISE = -1,
};

class BLDCMotor
{
public:
    // ####################################################################################
    uint8_t pole_pairs;                 // 极对数
    float power_supply_voltage = 12.0f; // 供电电压
    // ####################################################################################
    float limit_voltage = 12.0f;   // 限制电压
    float limit_current = 10.0f;   // 限制电流
    float limit_velocity = 130.0f; // 限制速度
    // ####################################################################################
    float target = 0;                                               // taget
    BLDCControlMode controlMode = BLDCControlMode::VelocityCurrent; // mode
    MotorDirectrion direction = MotorDirectrion::ANTI_CLOCK_WISE;   // directron
    // ####################################################################################
    LowPassFilter current_q_filter{10};
    LowPassFilter current_d_filter{10};
    LowPassFilter shaft_velocity_filter{50};
    LowPassFilter shaft_angle_filter{100};
    // ####################################################################################
    PIDControler pid_iq_controller;
    PIDControler pid_id_controller;
    PIDControler pid_velocity_controller;
    PIDControler pid_position_controller;
    // ####################################################################################
    BLDCDriver *driver = nullptr;
    Sensor *sensor = nullptr;
    CurrentSensor *currentSensor = nullptr;
    Command *command = nullptr;

public:
    // ####################################################################################
    BLDCMotor(uint8_t pole_pairs, float power_supply_voltage);
    // ####################################################################################
    void connectDriver(BLDCDriver *driver);
    void connectSensor(Sensor *sensor);
    void connectCurrentSensor(CurrentSensor *currentSensor);
    void connectCommand(Command *command);
    // ####################################################################################
    void initFOC();
    void loopFOC();
    // ####################################################################################
    /**
     * 设置控制模式
     */
    void setMode(BLDCControlMode mode);
    /**
     * 设置目标值（电压/力矩/速度/位置）
     */
    void setTarget(float target);
    /**
     * 设置旋转方向
     */
    void setDirection(MotorDirectrion direction);
    // ####################################################################################
    /**
     * 获取电角度
     */
    float electricalAngle();
    /**
     * 设置相电压
     * @param u_d float [-1,1]
     * @param u_q float [-1,1]
     * @param e_angle float [0,2PI]
     */
    void setPhraseVoltage(float u_d, float u_q, float e_angle);
    /**
     * 开环电压控制
     */
    void open_loop_voltage_control(float target_ud, float target_uq);
    // ####################################################################################
    /**
     * 获取dq轴电流
     */
    CurrentDQ getCurrentDQ();
    /**
     * 闭环电流控制
     */
    void close_loop_current_control(float target);
    // ####################################################################################
    /**
     * 获取速度
     */
    float shaftVelocity();
    /**
     * 闭环速度控制
     */
    void close_loop_velocity_current_control(float target);
    // ####################################################################################
    /**
     * 获取位置
     */
    float shaftAngle();
    /**
     * 闭环位置控制: 位置-力矩-闭环控制
     */
    void close_loop_position_current_control(float target);
    /**
     * 闭环位置控制: 位置-速度-力矩-闭环控制
     */
    void close_loop_position_velocity_current_control(float target);
    // ####################################################################################
};

#endif
Sensor.hpp
#ifndef _Sensor_H_
#define _Sensor_H_

#include <stdint.h>
#include "Timer.hpp"
#include "config.h"

class BLDCMotor;
class Sensor
{
  friend class BLDCMotor;

public:
  /**
   * @return void
   */
  virtual void initSensor() = 0;
  /**
   * @return [0,2PI]
   */
  virtual float readSensor() = 0;

  void connectMotor(BLDCMotor *motor);
  void alignSensor();
  void alignDirectron();
  void align();
  void update();
  float getPositon();
  float getPositons();
  float getVelocity();
  enum SensorDirectrion : int8_t
  {
    UNKNOW = 0,
    CLOCK_WISE = 1,
    ANTI_CLOCK_WISE = -1,
  };

private:
  BLDCMotor *motor = nullptr;
  Timer *timer = new ConcreteTimer();
  SensorDirectrion directron = SensorDirectrion::UNKNOW;
  float offset = 0;
  float position_pre, position_now = 0, d_position;
  float rotations_pre, rotation_now = 0, d_rotation;
  float positions_pre, positions_now = 0, d_positions;
  float velocity = 0;
};

#endif
CurrentSensor.hpp
#ifndef __CurrentSensor_H__
#define __CurrentSensor_H__
#include <stdint.h>
#include "foc_utils.h"
#include <math.h>
#include "BLDCMotor.hpp"
#define M_SQRT3 1.73205080756887719000
// CurrentABC
typedef struct
{
    float a;
    float b;
    float c;
} CurrentABC;

// CurrentAB
typedef struct
{
    float alpha;
    float beta;
} CurrentAB;

// CurrentDQ
typedef struct
{
    float d;
    float q;
} CurrentDQ;

// CurrentDC
typedef float CurrentDC;

class CurrentSensor
{

public:
    virtual void initSensor();
    virtual CurrentABC readSensor();
    friend class BLDCMotor;

private:
public:
    CurrentABC offset = {0, 0, 0};
    CurrentABC cache = {0, 0, 0};
    void alignSensor()
    {
        uint16_t times = 1000;
        CurrentABC average = {0, 0, 0};
        for (uint16_t n = 1; n <= times; n++)
        {
            CurrentABC current = this->readSensor();
            average.a += current.a;
            average.b += current.b;
            average.c += current.c;
            delay(2);
        }
        average.a /= times;
        average.b /= times;
        average.c /= times;
        this->offset = average;
    }

public:
    void update()
    {
        cache = this->readSensor();
    }

    CurrentABC getCurrentABC()
    {
        return {
            .a = cache.a - offset.a,
            .b = cache.b - offset.b,
            .c = cache.c - offset.c,
        };
    }

    CurrentAB getCurrentAB()
    {
        CurrentABC i = this->getCurrentABC();
        // 克拉克变换，等赋值形式
        return {
            .alpha = i.a,                      // i_alpha = 1 * i_a + 0 * i_b
            .beta = (i.a + 2 * i.b) / M_SQRT3, // i_beta = 1/sqrt(3) * i_a + 2 / sqrt(3) * i_b = (i_a + 2 * i_b)/sqrt(3)
        };
    }

    CurrentDC getCurrentDC()
    {
        CurrentAB i = this->getCurrentAB();
        return sqrtf(i.alpha * i.alpha + i.beta * i.beta); // 向量模长
    }
    /**
     * @param e_theta [0,2PI]
     */
    CurrentDQ getCurrentDQ(float e_theta)
    {
        float sin, cos;
        _sincos(e_theta, &sin, &cos);
        CurrentAB i = this->getCurrentAB();
        return {
            .d = cos * i.alpha + sin * i.beta,
            .q = -sin * i.alpha + cos * i.beta,
        };
    }
};
#endif
贡献者: YiguiDing
MIT Licensed | Copyright © 2019-present Yigui-Ding