08.实现CurrentSensor获取电流

08.实现CurrentSensor获取电流

具体实现

CurrentSensor.cpp

#include "Arduino.h"
#include "CurrentSensor.hpp"

#define M_SQRT3 1.7320508F

CurrentSensor::CurrentSensor(void (*initHardWare)(), CurrentABC (*readHardWare)())
{
    this->initHardWare = initHardWare;
    this->readHardWare = readHardWare;
}

void CurrentSensor::initSensor()
{
    this->initHardWare();
}

CurrentABC CurrentSensor::readSensor()
{
    return this->readHardWare();
}

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

CurrentABC CurrentSensor::getCurrentABC()
{
    CurrentABC i = this->readHardWare();
    i.a -= offset.a;
    i.b -= offset.b;
    return i;
}

#define M_1_SQRT3 0.57735026919f // (1 / sqrtf(3))
#define M_2_SQRT3 1.15470053838f // (2 / sqrtf(3))

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

CurrentDC CurrentSensor::getCurrentDC()
{
    CurrentAB i = this->getCurrentAB();
    return sqrtf(i.alpha * i.alpha + i.beta * i.beta);
}

CurrentDQ CurrentSensor::getCurrentDQ(uint16_t e_theta)
{
    int16_t sin, cos;
    _sincos(e_theta, &sin, &cos);
    CurrentAB i = this->getCurrentAB();
    return {
        .d = (cos * i.alpha + sin * i.beta) / (INT16_MAX),
        .q = (sin * -i.alpha + cos * i.beta) / (INT16_MAX),
    };
}

CurrentSensor.hpp

#ifndef __CurrentSensor_H__
#define __CurrentSensor_H__
#include <stdint.h>
#include "foc_utils.h"
#include "BLDCDriver.hpp"

typedef struct
{
    float a;
    float b;
} CurrentABC;

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

typedef float CurrentDC;

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

class CurrentSensor
{
private:
    void (*initHardWare)();
    CurrentABC (*readHardWare)();

public:
    CurrentABC offset = {0, 0};

public:
    CurrentSensor(void (*initHardWare)(), CurrentABC (*readHardWare)());
    void initSensor();
    void alignSensor();
    CurrentABC readSensor();
    CurrentABC getCurrentABC();
    CurrentAB getCurrentAB();
    CurrentDC getCurrentDC();
    CurrentDQ getCurrentDQ(uint16_t theta);
};
#endif

实现效果

08.current_sensor.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.initFOC();

  currentSensor.initSensor();
  currentSensor.alignSensor();
}

#include "LowPassFilter.hpp"
LowPassFilter filterA(200);
LowPassFilter filterB(200);
LowPassFilter filterC(200);
LowPassFilter filterD(200);
void loop()
{
  motor.loopFOC();

  CurrentABC iabc = currentSensor.getCurrentABC();
  Serial.print("i.a:");
  Serial.print(filterA(iabc.a));
  Serial.print(',');
  Serial.print("i.b:");
  Serial.print(filterB(iabc.b));
  Serial.print(',');

  CurrentAB i = currentSensor.getCurrentAB();
  Serial.print("i.alpha:");
  Serial.print(filterA(i.alpha));
  Serial.print(',');
  Serial.print("i.beta:");
  Serial.print(filterB(i.beta));
  Serial.print(',');

  CurrentDC dc = currentSensor.getCurrentDC();
  Serial.print("i.dc:");
  Serial.print(filterC(dc));
  Serial.print(',');

  CurrentDQ dq = currentSensor.getCurrentDQ(motor.electricalAngle());
  Serial.print("i.dq.q:");
  Serial.print(filterD(dq.q));
  Serial.print(',');
  Serial.print('\n');
}