05.实现Sensor获取转子位置、速度

05.实现Sensor获取转子位置、速度

具体实现

Sensor.cpp

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

Sensor::Sensor(void (*_initSensorl)(), uint16_t (*_readSensor)())
{
    this->_initSensor = _initSensorl;
    this->_readSensor = _readSensor;
}
void Sensor::initSensor()
{
    this->_initSensor();
}
uint16_t Sensor::readSensor()
{
    return this->_readSensor();
}
void Sensor::connectMotor(BLDCMotor *motor)
{
    this->motor = motor;
}
void Sensor::alignSensor()
{
    if (!this->motor)
        return;
    this->motor->setPhraseVoltage(0, 0.2 * INT16_MAX, _3_PI_2_); // -90°位置施加磁场
    delay(500);
    this->offset = this->_readSensor();
    this->motor->setPhraseVoltage(0, 0, 0);
}
void Sensor::update()
{
    // ####################################
    // 1.记录当前值
    this->prev_angle = this->angle;
    this->prev_full_rotations = this->full_rotations;
    // ####################################
    // 2.更新
    // ----更新角度
    this->angle = this->_readSensor();
    // ----更新圈数
    int32_t d_angle = this->angle - (int32_t)this->prev_angle;
    if (abs(d_angle) > 0.8 * _2PI_)
        this->full_rotations += (d_angle < 0) ? +1 : -1; // 圈数自增/自减
    // ####################################
    // 3.计算
    // ----计算位置
    this->position = this->angle - (int32_t)offset;
    // ----计算位置(带圈数)
    this->positions = this->full_rotations * _2PI_ + this->angle - (int16_t)this->offset;
    // ----计算速度(rad/ms)
    this->velocity = ((this->full_rotations - this->prev_full_rotations) * _2PI_ + (this->angle - (int32_t)this->prev_angle)) / dt_ms();
    // ####################################
}
/**
 * 获取位置
 */
uint16_t Sensor::getPositon()
{
    return this->position;
}
/**
 * 获取位置(带圈数)
 */
int32_t Sensor::getPositons()
{
    return this->positions;
}
/**
 * 获取速度
 * 单位: rad/ms
 */
int32_t Sensor::getVelocity()
{
    return this->velocity;
}

Sensor.hpp

#ifndef _Sensor_H_
#define _Sensor_H_

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

class BLDCMotor;

class Sensor : Timer {

private:
  //
  void (*_initSensor)();
  uint16_t (*_readSensor)();
  //
  BLDCMotor *motor = nullptr;
  //
  uint16_t offset = 0;
  uint16_t angle = 0;
  uint16_t prev_angle = 0;
  int32_t full_rotations = 0;
  int32_t prev_full_rotations = 0;
  //
  uint16_t position = 0;
  int32_t positions = 0;
  int32_t velocity = 0;

public:
  Sensor::Sensor(void (*initSensor)(), uint16_t (*readSensor)());
  void connectMotor(BLDCMotor *motor);
  void initSensor();
  uint16_t readSensor();
  void alignSensor();
  void update();
  uint16_t getPositon();
  int32_t getPositons();
  int32_t getVelocity();
};

#endif

实现效果

位置（不带圈数）

位置（带圈数）

速度（rad/ms）

05.sensor.ino

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

#define AS5600
// #define AS5047P

#ifdef AS5600

#define AS5600_ADDR 0x36
#define AS5600_RAW_ANGLE 0x0c
#define AS5600_ANGLE 0x0e

#elif defined AS5047P

#include <AS5047P.h>
#define AS5047P_CHIP_SELECT_PORT 9
#define AS5047P_CUSTOM_SPI_BUS_SPEED 100000
AS5047P as5047p = AS5047P(AS5047P_CHIP_SELECT_PORT, AS5047P_CUSTOM_SPI_BUS_SPEED);

#endif

Sensor *sensor = new Sensor(
#ifdef AS5600
    []()
    {
      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(true);
      // as5600 12bit精度，左移4位变成16位
      data <<= 4;
      return data;
    }
#elif defined AS5047P
    []()
    {
      pinMode(AS5047P_CHIP_SELECT_PORT, OUTPUT);
      as5047p.initSPI();
    },
    []()
    {
      digitalWrite(AS5047P_CHIP_SELECT_PORT, HIGH);
      // 14bit精度 左移2位变成16位
      uint16_t data = as5047p.readAngleRaw() << 2;
      digitalWrite(AS5047P_CHIP_SELECT_PORT, LOW);
      return data;
    }
#endif
);

void setup()
{
  Serial.begin(115200);
  sensor->initSensor();
}

uint8_t idx = 0;
void loop()
{
  sensor->update();
  if ((++idx %= 10) == 0)
  {
    Serial.print(0);
    Serial.print(sensor->readSensor());
    Serial.print(',');
    Serial.print(sensor->getPositon());
    Serial.print(',');
    Serial.print(sensor->getPositons());
    Serial.print(',');
    Serial.print(sensor->getVelocity());
    Serial.print('\n');
  }
}