自作モータドライバでSimpleFOC動かしてみた

 モータドライバ用の3Dプリント部品を作ってもらったので、前回組み立てたSimpleFOC モータドライバ基板をモータに組み付けて動かしてみた。

モータとモータドライバ間に挟むアダプタはこんな感じ。9.2mmにしてみた。

モータドライバの裏側にはモータと接続するためのピンヘッダとMT6701を取り付け。MT6701は結局単品で購入するより基板ごと購入したほうが安かったので、144円のI2Cタイプのモジュールを購入して剥がして移植した。

M2x12だとモータへのネジのかかりが少なすぎる気がする…モータ側のネジは3.8mmぐらいは行けそうなのでM2x14にしたほうがいいかなぁ。それ以上行くとコイルに傷をつけたらいやだし。

書き込みはBluepillをBMP化したやつを使用。デバッグはUSBシリアルアダプタを使用した。Arduino環境でSTLink V2を使うときでもCubeProgrammerをインストールしないといけないので、そのまま使えるBMPは便利かもしれない？シリアルポートも付いてるのでこれ1台で全部行けるんだけどswlinkファームだとどのピンかわからなかったので別途USBシリアルを使用している。

電源コネクタはリポバッテリとかでもよく使われてるPHコネクタ、シリアルポートはQwiicとピン配置を合わせてあるので今回はUARTとして使ったけど、I2Cとしても使用できる。CANはSWDと同じコネクタを使いまわし。書き込み用のSWDはRaspberry Pi Pico Hと同じ。JSTのSHコネクタの互換品(フットプリントそのままで装着できた)。

早速ファームウェアを書き込んでみる。

#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);
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 doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

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

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);
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.controller = MotionControlType::angle;
  motor.PID_velocity.P = 0.2f;
  motor.PID_velocity.I = 20;
  motor.voltage_limit = 8;
  motor.LPF_velocity.Tf = 0.01f;
  motor.P_angle.P = 20;
  motor.velocity_limit = 40;
  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.initFOC();

  command.add('T', doTarget, "target angle");
  command.add('M', doMotor, "motor");
  //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();
}

前回モータをテストしたときのスケッチを今回自作したモータドライバ基板用に少し編集した。電源電圧測定回路とか大丈夫かテストするときに気がついたんだけど、デフォルトでADCが10bitで動いてるっぽかったのでanalogReadResolution(12);をしておいた。そうするとInlineCurrentSenseで電流値が4倍に見えてしまうようになったのでInlineCurrentSenseのところでオペアンプのゲインのところを4倍に設定したら電流がうまく表示されるようになった。

とりあえず角度モードでモータが回ったので次はそれぞれの設定値をチューニングしないと。