Ардуиновская библиотека SimpleFOC и контроллер мотора для Xiaomi M365

В предыдущей статье я рассказывал о реверс-инжиниринге контроллера двигателя для самоката Xiaomi M365 и обещался рассказать о процессе натягивании совы на глобус использования ардуиновской библиотеки SimpleFOC с этим контроллером и мотор-колесом гироскутера. Выполняю обещание.

Если кому, не приведи Господи, придет в голову повторить описанное, прочитайте первую статью для начала.

Для начала берем мотор и делаем вид, что мы о нем ничего не знаем. Ага, совсем ничего — за несколько лет игрушек (Делаем пепелац из ховерборда, Пепелац 2.0 — электрический скутер-трайк) у меня их скопилось, как у дурака фантиков, не знаю даже, куда бы их пристроить. На популярную нынче робособаку вполне хватит, весить только со столькими движками она будет, как робослон.

Для начала нужно посчитать количество пар полюсов. Самый простой способ — вскрыть движок, подсчитать магниты и полученное число поделить на два. Мы же простых путей не ищем, тем более, что есть путь еще проще.

На любые два провода подаем небольшой ток, после чего магниты вступают в игру — в каких-то положениях они магнитятся к катушкам намного сильнее. Поворачиваем колесо на один оборот и считаем число устойчивых положений за время поворота. У меня получилось 15 полюсов — число загадочным образом совпадает с тем, что получено тупым подсчетом магнитов.

Далее неплохо бы определиться с соединением обмоток — звезда там или треугольник.

Измеряем сопротивление от одного вывода до другого измерительным мостом. Потом оставшийся вывод подключаем к любому из измеряемых. Если у вас звезда, то соотношение сопротивлений будет примерно 2 к 1.5, что мы и имеем. 

Значит, сопротивление одной обмотки примерно 0.25 плюс/минус лапоть — погрешность проводов и измерения игнорируем. Заодно теперь и индуктивность обмотки знаем 750/2=375 uH.

Можно другим методом сопротивление померить — от источника питания подаем ток около 4 ампер и измеряем напряжение на контактах двигателя — R=U/I, если Ом не обманывает.

1.08/4 = 0.27 на 2 обмотки. На одну приходится около 0.15 Ома. Я бы этому результату доверял больше, кто его знает, как этот мост измеряет.

Можем измерить обороты на вольт (RPM/V). Берете любой осциллограф, даже самый низкочастотный игрушечный китайский. Подключаетесь к любым двум выводам двигателя и начинаете его крутить, желательно равномерно. Имеете вот такую картинку:

Частота 32Гц, амплитуда в районе 7 вольт. Считаем с учетом того, что мы имеем 15 пар полюсов и 60 секунд в минуте: 32/15*60/7=18 RPM/V, опять же плюс/минус лапоть.

Теперь у нас есть какой-то минимум информации о двигателе. Если бы была нормальная спецификация, то мы там бы нашли:

• сопротивление обмоток (измерено)
• индуктивность (измерено)
• коэффициент обратной ЭДС (coefficient of back electromotive force -BEMF), это то, что мы смотрели осциллографом
• крутящий момент (torque coefficient)
• коэффициент трения (friction coefficient)
• момент инерции (moment of inertia)

Что есть, то есть. Чего нема — так просто не измерить.

Теперь берем контроллер двигателя для самоката Xiaomi M365, который я описывал в предыдущей статье.

Его схема с исправленными неточностями:

Хотелось бы изобразить управление двигателем примерно как рисуют на умных картинках (что это такое — я не знаю, но выглядит красиво):

Подключаем двигатель к контроллеру и начнем мучить библиотеку simpleFOC.

Библиотека пытается объять необъятное — бесколлекторные и шаговые двигатели, разные типы микропроцессоров, разные схемы подключение двигателей, разные типы датчиков положения двигателя, разные датчики тока обмоток. В итоге получается далеко не все идеально. Как заметил Козьма Прутков: «Специалист подобен флюсу – полнота его односторонняя». А тут далеко не так — распирает со всех сторон. Я как-то смеха ради из одного файла этой библиотеки попытался выкинуть все универсальности и приспособить его только для STM32, причем обращался напрямую к регистрам, убрав ардуиновость и кубовые прибамбасы — от файла ничего не осталось, меньше 5 процентов, если считать строки.

Настало время проверить правильность схемы, последовательно подключая функции библиотеке. Этот процесс описан в официальной документации, но такой конфигурации оборудования, как в нашем контроллере там нет. Давайте пойдем по шагам.

Первый шаг — тестируем датчики Холла. Для инициализации нужны номера входов и количество пар полюсов двигателя:

HallSensor hSensor = HallSensor(PB4, PB5, PB0, 15);

#include <Arduino.h>
#include "SimpleFOC.h"

HallSensor hSensor = HallSensor(PB4, PB5, PB0, 15);
void doA(){hSensor.handleA();}
void doB(){hSensor.handleB();}
void doC(){hSensor.handleC();}

HardwareSerial Serial3(PB11, PB10); // batt
void setup()
{
  Serial3.begin(115200); // BSD
  hSensor.pullup = Pullup::USE_EXTERN;
  hSensor.init();
  hSensor.enableInterrupts(doA, doB, doC);
  Serial3.println("Hall sensor test");
  _delay(1000);
}
 
void loop()
{
  // read sensor and update the internal variables
  hSensor.update();
  // display the angle and the angular velocity to the terminal
 Serial3.print(hSensor.getAngle());
  Serial3.print("\t");
  Serial3.println(hSensor.getVelocity());
  delay(500);
}

Загружаем программу, подключаем терминалку к последовательному порту и крутим двигатель рукой. Если все нормально — на терминалке увидим текущий угол поворота в радианах и скорость вращения:

Дальше проверяем, правильно ли мы определили выводы для управления двигателем

BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15)

#include <Arduino.h>
#include "SimpleFOC.h"

// BLDCDriver6PWM(pwmAh, pwmAl, pwmBh, pwmBl, pwmCh, pwmCl, (en optional))
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);
HardwareSerial Serial3(PB11, PB10); // batt

void setup()
{
  Serial3.begin(115200); // BSD
  // enable more verbose output for debugging
  SimpleFOCDebug::enable(&Serial3);
  // power supply voltage [V]
  driver.voltage_power_supply = 36;
  // Max DC voltage allowed - default voltage_power_supply
  driver.voltage_limit = 42;
  // dead_zone [0,1] - default 0.02f - 2%
  driver.dead_zone = 0.02f;

  // driver init
  if (!driver.init())
  {
    Serial3.println("Driver init failed!");
    return;
  }

  // enable driver
  driver.enable();
  Serial3.println("Driver ready!");
  _delay(1000);
}

void loop()
{
  // setting pwm phase A: 3V, phase B: 6V, phase C: 5V
  driver.setPwm(3,6,5);
  delay(100);
}

После загрузки программы двигатель встанет на тормоз — фазные напряжения мы установили и менять их не собираемся. Если, конечно не напортачили, а то может и волшебный дым пойти. Так что на этом этапе контроллер лучше подключить к блоку питания и ограничить ток одним ампером — у меня средний ток потребления на этом этапе был чуть больше 300мА. А если подключить к аккумулятору — то лучше не ошибаться.

Сообщения терминальной программы:

 Если все пучком, то настало время запустить двигатель безо всяких датчиков и обратных связей. Согласно описанию, тут мы должны задать все определенные нами параметры двигателя. На самом деле к какому-то результату приводит только если задать количество пар полюсов и этим ограничиться. Любые другие параметры приводят в тому, что двигатель не двигается или дергается.

BLDCMotor motor = BLDCMotor(15);

#include <Arduino.h>
#include "SimpleFOC.h"
 
// pp pole pairs number
// R motor phase resistance - [Ohm]
// KV motor KV rating (1/K_bemf) - rpm/V
// L motor phase inductance - [H]
// BLDCMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
//BLDCMotor motor = BLDCMotor(15, 0.15, 18, 0.000375);
BLDCMotor motor = BLDCMotor(15);
// BLDCDriver6PWM(pwmAh, pwmAl, pwmBh, pwmBl, pwmCh, pwmCl, (en optional))
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);


HardwareSerial Serial3(PB11, PB10); // batt
// instantiate the commander
Commander command = Commander(Serial3);
void doTarget(char* cmd) { command.scalar(&motor.target, cmd); }
void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }

 
void setup()
{
  Serial3.begin(115200); // BSD
  // enable more verbose output for debugging
  SimpleFOCDebug::enable(&Serial3);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 36;
  if(!driver.init())
  {
    Serial3.println("Driver init failed!");
    return;
  }
  // link the motor and the driver
  motor.linkDriver(&driver);
 
  // limiting motor movements
  // limit the voltage to be set to the motor
  // start very low for high resistance motors
  // current = voltage / resistance, so try to be well under 1Amp
  motor.voltage_limit = 5; // [V]

  // open loop control config
  motor.controller = MotionControlType::velocity_openloop;

  // init motor hardware
  if(!motor.init())
  {
    Serial3.println("Motor init failed!");
    return;
  }

  // set the target velocity [rad/s]
  motor.target = 3.14; // one rotation per 2 seconds
 
  // add target command T
  command.add('T', doTarget, "target velocity");
  command.add('L', doLimit, "voltage limit");

  Serial3.println("Motor ready!");
  Serial3.println("Set target velocity [rad/s]");
  _delay(1000);
}

void loop()
{
  motor.move(); // open loop velocity movement
  command.run(); // user communication
}

Сообщения на терминале:

Двигатель вращается со скоростью пол оборота в секунду. При этом он заметно рычит — при нормальных условиях он вообще-то должен вращаться практически бесшумно.

Или я что-то неправильно делаю? 

Теперь к предыдущей конфигурации добавим датчики Холла.

По мнению авторов библиотеки, датчики Холла — это плохой вариант. Точность определения угла составляет 4 градуса, и они рекомендуют использовать функцию сглаживания.

SmoothingSensor smooth = SmoothingSensor(hSensor, motor);

#include <Arduino.h>
#include "SimpleFOC.h"
#include "smoothing/SmoothingSensor.h"

BLDCMotor motor = BLDCMotor(15);
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);

HallSensor hSensor = HallSensor(PB4, PB5, PB0, 15);
void doA(){hSensor.handleA();}
void doB(){hSensor.handleB();}
void doC(){hSensor.handleC();}

SmoothingSensor smooth = SmoothingSensor(hSensor, motor);

HardwareSerial Serial3(PB11, PB10); // batt
Commander command = Commander(Serial3);

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

void setup()
{
  Serial3.begin(115200);

  hSensor.pullup = Pullup::USE_EXTERN;
  hSensor.init();
  hSensor.enableInterrupts(doA, doB, doC);
  
  motor.linkSensor(&hSensor);
  smooth.phase_correction = -_PI_6;

  driver.voltage_power_supply = 36;
  driver.voltage_limit = 18;
  if(!driver.init())
  {
    Serial3.println("Driver init failed!");
    return;
  }

  motor.linkDriver(&driver); // link the motor and the driver
  // aligning voltage
  motor.voltage_sensor_align = 5;

  // set motion control loop to be used
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;
  
  // comment out if not needed
  motor.useMonitoring(Serial3);   
  // initialize motor
  if(!motor.init())
  {
    Serial3.println("Motor init failed!");
    return;
  }

  // align sensor and start FOC
  if(!motor.initFOC())
  {
    Serial3.println("FOC init failed!");
    return;
  }

  // set the initial motor target
  motor.target = 2; // Volts

  // add target command M
  command.add('M', doMotor, "Motor");

  Serial3.println(F("Motor ready."));
  Serial3.println(F("Set the target using serial terminal and command M:"));
  _delay(1000);
}

void loop()
{
  motor.loopFOC(); // main FOC algorithm function
  motor.move();    // Motion control function
  command.run();   // user communication
}

На сей раз все крутится без рычания. И никакой разницы, используется SmoothingSensor или нет. Никаких нареканий вообще — все как обычно и как должно быть.

Остался последний шаг — добавить в конфигурацию датчики тока. У наших датчиков тока, как видно из схемы, есть смещение нуля. Хорошая новость — об этом не стоит беспокоиться, при инициализации библиотека все это учтет автоматически. Но входы, к которым подключены датчики тока, сопротивление шунтов и усиление операционных усилителей все-таки придется ввести самостоятельно.

LowsideCurrentSense current_sense = LowsideCurrentSense(0.002f, 8.0f, PA3, PA4, PA5);

#include <Arduino.h>
#include "SimpleFOC.h"
#include "smoothing/SmoothingSensor.h"

// BLDCMotor(pole pair number, phase resistance (optional) );
BLDCMotor motor = BLDCMotor(15);
// BLDCDriver6PWM(pwmAh, pwmAl, pwmBh, pwmBl, pwmCh, pwmCl, (en optional))
BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15);

HallSensor hSensor = HallSensor(PB4, PB5, PB0, 15);
void doA(){hSensor.handleA();}
void doB(){hSensor.handleB();}
void doC(){hSensor.handleC();}

// instantiate the smoothing sensor, providing the real sensor as a constructor argument
SmoothingSensor smooth = SmoothingSensor(hSensor, motor);

// LowsideCurrentSense constructor
// - shunt_resistor - shunt resistor value, gain - current-sense op-amp gain
LowsideCurrentSense current_sense = LowsideCurrentSense(0.002f, 8.0f, PA3, PA4, PA5);

HardwareSerial Serial3(PB11, PB10); // batt
Commander command = Commander(Serial3);
void doMotor(char* cmd) { command.motor(&motor, cmd); }

void setup()
{
  Serial3.begin(115200); // BSD

  hSensor.pullup = Pullup::USE_EXTERN;
  hSensor.init();
  hSensor.enableInterrupts(doA, doB, doC);
  motor.linkSensor(&hSensor);

  smooth.phase_correction = -_PI_6;

  driver.voltage_power_supply = 36;
  driver.voltage_limit = 18;
  if(!driver.init())
  {
    Serial3.println("Driver init failed!");
    return;
  }

  motor.linkDriver(&driver); // link the motor and the driver
  current_sense.linkDriver(&driver); // link driver to cs
  // current sense init hardware
  if(!current_sense.init())
  {
    Serial3.println("Current sense init failed!");
    return;
  }

  motor.linkCurrentSense(&current_sense); // link the current sense to the motor
  // aligning voltage
  motor.voltage_sensor_align = 5;
  // set motion control loop to be used
  motor.torque_controller = TorqueControlType::foc_current;
  motor.controller = MotionControlType::torque;
  motor.useMonitoring(Serial3);
  //motor.monitor_downsample = 100; // set downsampling can be even more > 100
  motor.monitor_downsample = 0; // disable monitor at first - optional
  motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // set monitoring of d and q currents
  // initialize motor
  if(!motor.init())
  {
    Serial3.println("Motor init failed!");
    return;
  }
  // align sensor and start FOC
  if(!motor.initFOC())
  {
    Serial3.println("FOC init failed!");
    return;
  }

  // set the initial motor target
  motor.target = 0.5;
  // add target command M
  command.add('M', doMotor, "Motor");
  motor.useMonitoring(Serial3);

  Serial3.println(F("Motor ready."));
  Serial3.println(F("Set the target using serial terminal and command M:"));
  _delay(1000);
}

void loop()
{
  motor.loopFOC(); // main FOC algorithm function
  motor.move(); // Motion control function
  motor.monitor(); // display the currents
  command.run(); // user communication
}

 Все казалось бы замечательно и все вращается. Но какие-то призвуки при торможении и разгоне. Надо настраивать ПИД регуляторы. Когда я по диагонали прочитал описание этой библиотеки, я почему-то решил, что где-то есть магическая кнопочка и стоить ее нажать — и все, ПИД регуляторы настроятся сами по себе. А почему нет?

Ишь, размечтался… Знатный зверюга! Мех! Мясо! Шкварок нажарю… Боярин дубовый.

Теперь о той программе, от которой я ожидал магической кнопочки — она называется SimpleFOCStudio и лежит тут, инструкция тут .

В инструкции написано, что ее установить очень просто — а дальше какие-то страшные слова. Я просто скопировал файлы, запустил на выполнение

python3 simpleFOCStudio.py

из терминала и… ничего не произошло, вывались сообщение об ошибке. Видимо, разрабатывали под Windows, а что будет под Linux — не проверили.

В строке 27 файла graphicWidget.py меняем

self.plotWidget.showGrid(x=True, y=True, alpha=0.5)

на

self.plotWidget.showGrid(x=True, y=True, alpha=1)

и все начинает дышать.

Подключаем нужный последовательный порт (лучше делать на вкладке Cmd Line, иначе там какой-то глюк и не всегда работает. Пишем букву M в поле рядом с кнопкой Pull Param и жамкаем эту кнопку. Студия считывает параметры и показываем какие-то меняющие циферки. Запускаем графики и видим, что призвуки были не зря, скорость действительно скачет.

А все назначение этой программы — вы должны вручную ввести значения разных коэффициентов, а программа передаст их в контроллер. Когда все коэффициенты подобраны, нажатие кнопочки с символом Ардуино сгенерирует текст, который вы должны добавить в свой код, чтобы коэффициенты остались там уже навсегда.

Но этот «закат солнца вручную» мне толком провернуть не удалось, обороты так и плавали. Легко удавалось сделать хуже. Будем надеяться, что авторы simpleFOCStudio хоть что-то добавят для автоматизации подбора коэффициентов, тем более, что не очень-то понятно, какие коэффициенты за что отвечают. Часть из них работает в одном режиме, часть — в другом.

А коэффициентов очень много. Народ в интернете обсуждает настройку фильтров и ПИД регуляторов, после долгих теоретизирований их все-таки подбирают вручную. А я так надеялся на магическую кнопочку...

Впрочем, я, наверное, привередничаю. У этого же мотора шума примерно столько же, когда я использую программное обеспечение для гироскутерных плат от Emanuel Feru, которое я использую для всех своих самоделок — наверное, оно вообще самое популярное для этих колесиков. И все ездит без проблем, на ходу этого шума вообще не слышно и на скорость он никак не влияет.

Кстати, помните я писал, что на Таобао продают очень дешевые контроллеры моторов для гироскутера? Так вот, я их там нашел — 12 юаней пара. К сожалению, все посредники, которые доставляют товары с Таобао задирают такие цены, что покупка теряет всякий смысл.

Есть у меня знакомый, который очень долго жил в Китае, и супруга у него китаянка. Но он ничего не смог посоветовать. Разве что поделился наблюдением — когда они ездят в гости к родственникам, его жена оттягивается по полной — цены на Али намного выше, чем на Таобао.

Ну а пока, после консультаций с жабой, я закупился на Али, так что ожидайте в скором  времени реверса контроллеров на MM32SPIN05, если есть интерес.