2208モータをセンサ付きFOC制御で回してみた。

 前回はSimpleFOCでOpenloopで回したんだけど、モータドライバ用アダプタを3Dプリントしてもらったのでセンサ付きFOCを試してみた。Openloopで回しただけなのにSimpleFOCで回したというタイトルはまずかったかな…

アダプタは今回もOnshapeで設計したのでフリープランなのでMars Power 2208とかで検索すれば出てくるかも。MT6701に付属のΦ4で2mm厚のマグネットをモータの軸に接着してMT6701と磁石の間が1.7mmぐらいになる感じにしておいた。

まずはアダプタをモータに装着して、その上から基板を装着する感じ。

モータにはあらかじめMT6701付属の磁石を貼り付けておいた。意外と真ん中に装着するのが難しい。アロンアルファでシャフトにダイレクトに貼り付け。

スケッチはこんな感じでとりあえずSimpleFOCで前回同様にキャリブレーションを行った。

#include "SPI.h"
#include "SimpleFOC.h"
#include "SimpleFOCDrivers.h"
#include "encoders/MT6701/MagneticSensorMT6701SSI.h"

#define MOTOR_POLE 7

#define CS1 PA15
#define SCK1 PB3
#define MISO1 PB4
#define MOSI1 PB5
#define DRV_EN PB12
#define TX3_SCL2 PB10
#define RX3_SDA2 PB11
#define CAN_RX PB8
#define CAN_TX PB9
#define V_CURR PA0
#define W_CURR PA1
#define U_CURR PA2
#define TEMP PA3
#define VOLTAGE PA4
#define LED_BUILTIN PC13


HardwareSerial Serial3(TX3_SCL2, RX3_SDA2);
SPIClass SPI_1(MOSI1, MISO1, SCK1);
MagneticSensorMT6701SSI sensor(CS1);
//InlineCurrentSense current_sense = InlineCurrentSense(0.01, -50 * 4, W_CURR, V_CURR, U_CURR);
InlineCurrentSense current_sense = InlineCurrentSense(0.01, -50 * 4, W_CURR, V_CURR, _NC);
BLDCMotor motor = BLDCMotor(MOTOR_POLE);
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, DRV_EN);  //W V U EN
float target_angle = 0;
Commander command = Commander(Serial3);


void toggleLED() {
  static bool state = false;
  state = !state;
  if (state) {
    GPIOC->BSRR = (1 << 13);
  } else {
    GPIOC->BSRR = (1 << (13 + 16));
  }
}

void doTarget(char* cmd) {
  command.scalar(&target_angle, cmd);
}

void doMotor(char* cmd) {
  command.motor(&motor, cmd);
}

void doAnalog(char* cmd) {
  if (cmd[0] == 'T') Serial3.println(analogRead(TEMP));
  else if (cmd[0] == 'V') {
    Serial3.print("Voltage: ");
    Serial3.println(analogRead(VOLTAGE) * 0.0032);
  }
};

void setup() {
  analogReadResolution(12);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  Serial3.setTx(TX3_SCL2);
  Serial3.setRx(RX3_SDA2);
  Serial3.begin(115200);
  SimpleFOCDebug::enable(&Serial3);

  sensor.init(&SPI_1);
  motor.linkSensor(&sensor);
  driver.voltage_power_supply = 8;
  driver.init();
  motor.linkDriver(&driver);
  current_sense.linkDriver(&driver);
  // control loop type and torque mode
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::velocity;
  motor.motion_downsample = 0.0;

  // velocity loop PID
  motor.PID_velocity.P = 1.5;
  motor.PID_velocity.I = 10;
  motor.PID_velocity.D = 0.0;
  motor.PID_velocity.output_ramp = 1000.0;
  motor.PID_velocity.limit = 2.0;
  // Low pass filtering time constant
  motor.LPF_velocity.Tf = 0.05;
  // angle loop PID
  motor.P_angle.P = 20.0;
  motor.P_angle.I = 0.0;
  motor.P_angle.D = 0.0;
  motor.P_angle.output_ramp = 0.0;
  motor.P_angle.limit = 40.0;
  // Low pass filtering time constant
  motor.LPF_angle.Tf = 0.0;
  // current q loop PID
  motor.PID_current_q.P = 1.5;
  motor.PID_current_q.I = 15.0;
  motor.PID_current_q.D = 0.0;
  motor.PID_current_q.output_ramp = 1000.0;
  motor.PID_current_q.limit = 5.0;
  // Low pass filtering time constant
  motor.LPF_current_q.Tf = 0.005;
  // current d loop PID
  motor.PID_current_d.P = 1.5;
  motor.PID_current_d.I = 15.0;
  motor.PID_current_d.D = 0.0;
  motor.PID_current_d.output_ramp = 1000.0;
  motor.PID_current_d.limit = 5.0;
  // Low pass filtering time constant
  motor.LPF_current_d.Tf = 0.02;
  // Limits
  motor.velocity_limit = 200.0;
  motor.voltage_limit = 5.0;
  motor.current_limit = 2.0;
  // sensor zero offset - home position
  motor.sensor_offset = 0.0;
  // general settings
  // motor phase resistance
  motor.phase_resistance = 1.06;
  // pwm modulation settings
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.modulation_centered = 1.0;

  motor.useMonitoring(Serial3);

  motor.linkCurrentSense(&current_sense);
  current_sense.init();
  Serial3.print("current_sense.gain:\t");
  Serial3.print(current_sense.gain_a);
  Serial3.print(", ");
  Serial3.print(current_sense.gain_b);
  Serial3.print(", ");
  Serial3.println(current_sense.gain_c);
  Serial3.print("current_sense.offset_i:\t");
  Serial3.print(current_sense.offset_ia);
  Serial3.print(", ");
  Serial3.print(current_sense.offset_ib);
  Serial3.print(", ");
  Serial3.println(current_sense.offset_ic);

  motor.init();
  //motor.sensor_direction = Direction::CW;
  motor.initFOC();

  command.add('T', doTarget, "target angle");
  command.add('M', doMotor, "motor");
  command.add('A', doAnalog, "analog read");
  motor.monitor_downsample = 0;
  //motor.monitor_variables = 0;

  Serial3.println(F("Motor ready."));
  Serial3.println(F("Set the target angle using serial terminal:"));

  _delay(1000);
}

void loop() {
  motor.loopFOC();
  motor.move(target_angle);
  motor.monitor();
  command.run();
}

今回はインライン電流センサを２つに減らしてみたので使っていない層のピンアサインは"_NC"を指定している。ドキュメントだと2つのポートを指定しただけでも大丈夫そうに書いてあったんだけど初期化のときに"current_sense.offset_ia"が0になってしまって、bとcに値が入っていた。

とりあえず電流センサを２つに減らしても問題なく動いている。もともと2層だけのモータドライバも多いし。

SimpleFOC Studioでキャリブをしているけど9Vでも500mAぐらいしか流れないのでもっと電圧を高めたい感じ。しかしながらTMC6300が11Vまでという…
センサレスのときよりは断然トルクが上がっているのでギアボックスに組み込んでちゃんと回るかな。