前回はSimpleFOCでOpenloopで回したんだけど、モータドライバ用アダプタを3Dプリントしてもらったのでセンサ付きFOCを試してみた。Openloopで回しただけなのにSimpleFOCで回したというタイトルはまずかったかな…
アダプタは今回もOnshapeで設計したのでフリープランなのでMars Power 2208とかで検索すれば出てくるかも。MT6701に付属のΦ4で2mm厚のマグネットをモータの軸に接着してMT6701と磁石の間が1.7mmぐらいになる感じにしておいた。
まずはアダプタをモータに装着して、その上から基板を装着する感じ。
他のモータ用に作った基板なので配線が外に出てしまったり大きさが微妙だったりするけど、そもそもこのモータの直径まで小さくできるかなぁ？
スケッチはこんな感じでとりあえず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.0;
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 = 5.0;
motor.P_angle.I = 0.0;
motor.P_angle.D = 0.0;
motor.P_angle.output_ramp = 0.0;
motor.P_angle.limit = 200.0;
// Low pass filtering time constant
motor.LPF_angle.Tf = 0.0;
// current q loop PID
motor.PID_current_q.P = 1.1;
motor.PID_current_q.I = 20.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.1;
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(¤t_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::CCW;
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までという…
センサレスのときよりは断然トルクが上がっているのでギアボックスに組み込んでちゃんと回るかな。
