07.实现open_loop_velocity_control开环电压控制

丁毅桂小于 1 分钟

07.实现open_loop_velocity_control开环电压控制

具体实现

BLDCMotor.cpp

#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::initFOC()
{
    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();
    }
}
/**
 * @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)
{
    float voltage = _constrain(-limit_voltage, target, limit_voltage);
    int16_t u_q = this->direction * (voltage / power_supply_voltage * INT16_MAX);
    this->setPhraseVoltage(0, u_q, this->electricalAngle());
}

uint8_t idx = 0;
void BLDCMotor::loopFOC()
{
    if (this->sensor)
        this->sensor->update();

    if (++idx == 0)
        Serial.println(this->sensor->getVelocity() / _2PI_);

    this->open_loop_voltage_control(3.0f);
}

BLDCMotor.hpp

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

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

public:
    // 极对数
    uint8_t polePairs;
    // 供电电压
    float power_supply_voltage = 12.0f;
    // 限制电压
    float limit_voltage = 4.0f;
    // directron
    MotorDirectrion direction = MotorDirectrion::CLOCK_WISE;
private:
    //
    BLDCDriver *driver = nullptr;
    Sensor *sensor = nullptr;

public:
    BLDCMotor(uint8_t polePairs, float power_supply_voltage);
    void connectDriver(BLDCDriver *driver);
    void connectSensor(Sensor *sensor);
    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);
};
#endif

实现效果

06.filter.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,32767] => [0,255]
      // 大多数引脚上的 PWM 信号频率约为490 Hz
      // 设置占空比
      // analogWrite(M1_Ua, (u_a + _INT16_N_ONE_) / 255);
      // analogWrite(M1_Ub, (u_b + _INT16_N_ONE_) / 255);
      // analogWrite(M1_Uc, (u_c + _INT16_N_ONE_) / 255);
      // [-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);
      // Serial.print(0);
      // Serial.print(',');
      // Serial.print(100);
      // Serial.print(',');
      // Serial.print(u_a / 32768.0f * 50 + 50);
      // Serial.print(',');
      // Serial.print(u_b / 32768.0f * 50 + 50);
      // Serial.print(',');
      // Serial.print(u_c / 32768.0f * 50 + 50);
      // Serial.print('\n');
    });

#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;
    });

void setup()
{
  Serial.begin(115200);
  motor.connectDriver(&driver);
  motor.connectSensor(&sensor);
  motor.initFOC();
}

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