10.实现close_loop_velocity_control闭环速度控制

10.实现close_loop_velocity_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::connectCommand(Command *command)
{
  this->command = command;
}

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();
  switch (controlMode)
  {
  case ControlMode::Voltage:
    this->open_loop_voltage_control(0, target);
    break;
  case ControlMode::Current:
    this->close_loop_current_control(target);
    break;
  case ControlMode::Velocity:
    this->close_loop_velocity_control(target);
    break;
  case ControlMode::Position:
    this->close_loop_velocity_control(target);
    break;
  default:
    break;
  }
}
/**
 * 设置控制模式
 */
void BLDCMotor::setMode(ControlMode controlMode)
{
  switch (controlMode)
  {
  case ControlMode::Unknow:
    break;
  case ControlMode::Voltage:
    break;
  case ControlMode::Current:
    // 官方 Simple FOC Shield v2.0.4 开发板 with 2808云台电机
    // this->pid_id_controller = PIDControler(2.8, 200, -0.005, this->limit_voltage, 0);
    // this->pid_iq_controller = PIDControler(2.8, 200, -0.005, this->limit_voltage, 0);

    // 艾尔塞电子 Simple FOC Shield v2.0.4 开发板 with 2808云台电机
    // this->pid_id_controller = PIDControler(2, 1000, 0, this->limit_voltage, 0);
    // this->pid_iq_controller = PIDControler(2, 1000, 0, this->limit_voltage, 0);

    // 官方 Simple FOC Shield v2.0.4 开发板 with 2804-100kv 云台电机
    this->pid_id_controller = PIDControler(20, 500, 0, this->limit_voltage, 400);
    this->pid_iq_controller = PIDControler(20, 500, 0, this->limit_voltage, 400); // 完美
    break;
  case ControlMode::Velocity:
    // 官方 Simple FOC Shield v2.0.4 开发板 with 2808云台电机
    // this->pid_id_controller = PIDControler(2.8, 0.01, 0, this->limit_voltage, 0);
    // this->pid_iq_controller = PIDControler(2.8, 0.01, 0, this->limit_voltage, 0);
    // this->pid_velocity_controller = PIDControler(0.5, 2.5, 0, this->limit_current, 0); //  kp<1; ki<=10;

    // 艾尔塞电子 Simple FOC Shield v2.0.4 开发板 with 2808云台电机
    // this->pid_id_controller = PIDControler(2, 1000, 0, this->limit_voltage, 0);
    // this->pid_iq_controller = PIDControler(2, 1000, 0, this->limit_voltage, 0);
    // this->pid_velocity_controller = PIDControler(0, 0, 0, this->limit_current, 0);

    // 官方 Simple FOC Shield v2.0.4 开发板 with 2804-100kv 云台电机
    this->pid_id_controller = PIDControler(20, 500, 0, this->limit_voltage, 400); // kp可以设置到30 但取其 2/3 为 20，余量留给ki发挥 roc设置为400是为了
    this->pid_iq_controller = PIDControler(20, 500, 0, this->limit_voltage, 400);
    this->pid_velocity_controller = PIDControler(0.01, 0.05, 0.0001, this->limit_current, 20); // 非常完美 但当电压为12v时最好设置电流限制为1
    break;
  case ControlMode::Position:
    // this->pid_id_controller = PIDControler(2.8, 0.01, 0, this->limit_voltage, 0);
    // this->pid_iq_controller = PIDControler(2.8, 0.01, 0, this->limit_voltage, 0);
    // this->pid_velocity_controller = PIDControler(0.5, 2.5, 0, this->limit_current, 0);
    // this->pid_position_controller = PIDControler(0, 0, 0, this->limit_velocity, 0);
    break;
  default:
    break;
  }
  this->controlMode = controlMode;
}
void BLDCMotor::setTarget(float target)
{
  this->target = target;
}
/**
 * @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.0000605
 * @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 = (-1 * 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)
{
  uint16_t e_angle = this->electricalAngle();
  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);
  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),
  };
}
#include "foc_utils.h"
/**
 * 电流闭环控制
 */
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);

  static uint8_t idx = 0;
  if (this->command && ++idx % 61 == 0)
  {
    idx = 0;
    this->command->drawDragram(1, target_i_q, current.q);
  }
}
/**
 * 获取机械角速度
 */
float BLDCMotor::shaftVelocity()
{
  return shaft_velocity_filter(this->sensor->getVelocity());
}
void BLDCMotor::close_loop_velocity_control(float target)
{
  float target_velocity = _constrain(-limit_velocity, target, limit_velocity);
  float current_velocity = this->shaftVelocity();
  float error = target_velocity - current_velocity;
  float i_q = this->direction * this->sensor->directron * this->pid_velocity_controller(error);
  this->close_loop_current_control(i_q);

  static uint8_t idx = 0;
  if (this->command && ++idx % 62 == 0)
  {
    idx = 0;
    this->command->drawDragram(2, target_velocity, current_velocity);
  }
}

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"
#include "Command.hpp"

class BLDCMotor : Timer
{

public:
    enum ControlMode : uint8_t
    {
        Unknow = 0,
        Voltage = 1,
        Current = 2,
        Velocity = 3,
        Position = 4
    };
    enum MotorDirectrion : int8_t
    {
        UNKNOW = 0,
        ANTI_CLOCK_WISE = 1,
        CLOCK_WISE = -1,
    };

    // 极对数
    uint8_t polePairs;
    // 供电电压
    float power_supply_voltage = 12.0f;
    // 限制电压
    float limit_voltage = 12.0f;
    // 限制电流
    float limit_current = 10.0f;
    // 限制速度
    float limit_velocity = 130.0f;
    // directron
    MotorDirectrion direction = MotorDirectrion::ANTI_CLOCK_WISE;
    // taget
    float target = 0;
    // mode
    ControlMode controlMode = ControlMode::Velocity;
    // filter
    LowPassFilter current_q_filter{5};
    LowPassFilter current_d_filter{5};
    LowPassFilter shaft_velocity_filter{50};
    LowPassFilter shaft_angle_filter{100};
    // pid-controller
    PIDControler pid_iq_controller;
    PIDControler pid_id_controller;
    PIDControler pid_velocity_controller;
    PIDControler pid_position_controller;

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

public:
    BLDCMotor(uint8_t polePairs, 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(ControlMode mode);
    void setTarget(float target);
    /**
     * 获取电角度
     */
    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 shaftVelocity();
    /**
     * 闭环速度控制
     */
    void close_loop_velocity_control(float target);
};
#endif

实现效果

电流闭环控制效果

速度闭环控制效果

代码

#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"
#include "Command.hpp"

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

      // 20k
      // 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);

      // 10k
      pwmA.begin(10000.0f, 0.0f);
      pwmB.begin(10000.0f, 0.0f);
      pwmC.begin(10000.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(
    []()
    {
      // AS5600 最高支持1Mhz 1000000
      Wire.setClock(1000000); //  100000 (standard mode)  400000 (fast mode) 1000000 (fast mode plus)
      Wire.begin();
    },
    []()
    {
      Wire.beginTransmission(AS5600_ADDR);
      Wire.write(AS5600_ANGLE);
      uint8_t err = Wire.endTransmission(false);
      if (!!err)
      {
        // Wire.endTransmission(true);
        // Serial.print("errorcode:");
        // Serial.println(err);
        return (uint16_t)0;
      }
      Wire.requestFrom(AS5600_ADDR, 2);

      uint16_t data = 0;
      *((uint8_t *)&data + 1) = Wire.read();
      *((uint8_t *)&data + 0) = Wire.read();
      err = Wire.endTransmission(true);
      if (!!err)
      {
        // Serial.print("errorcode:");
        // Serial.println(err);
        return (uint16_t)0;
      }
      delay(1); // 这行代码解决了磁编码器有时无法读取的问题,开始怀疑时排针接触不良,然后尝试焊接排针问题依然存在,然后怀疑是SDA\SCL上拉电阻太多导致,拆掉问题依旧存在,然后怀疑是电机导致的电压波动,然后给AS5600供电加了个1uf和0.1uf电容,问题依旧,最后google搜索后发现别人的代码中有delay遂加上,问题解决.
      // as5600 12bit精度，左移4位变成16位
      data <<= 4;
      // Serial.println(data);
      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电路接反了 加符号
      };
    });

Command 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(BLDCMotor::ControlMode::Current);
}

void loop()
{
  motor.loopFOC();
  command.update();
  // Serial.println(sensor.getPositon());
}