ПЛАВНОЕ УПРАВЛЕНИЕ ЩЁТОЧНЫМ МОТОРОМ

Решил я сделать библиотеку для управления обычным щёточным мотором с энкодером, которая позволила бы управлять им как шаговым (с либой AccelStepper) с поддержкой ускорения и ограничения максимальной скорости. Библиотека полностью наследует все инструменты по работе с мотором из GyverMotor. Основной метод библиотеки tick() принимает текущее положение вала мотора (сигнал с потенциометра или энкодера) и позволяет:

  • Сделать полноценный сервопривод из моторчика с редуктором и резистором на валу
  • Поддерживать скорость вращения моторчика под переменной нагрузкой (шпиндель станка)
  • Из моторчика с энкодером позволяет получить аналог шагового мотора с возможностью повернуть вал на нужное количество оборотов (градусов) с плавным ускорением и ограничением максимальной скорости

БИБЛИОТЕКА ACCELMOTOR

Библиотека для расширенного управления и стабилизации мотора с энкодером

  • Наследует все фишки из библиотеки GyverMotor (поддержка разных драйверов и режимов)
  • Режим поддержания скорости с обратной связью
  • Режим поворота на заданный угол с обратной связью
  • Настраиваемые коэффициенты PID регулятора
  • Ограничение ускорения и скорости
  • Библиотека принимает любой тип обратной связи: энкодер, потенциометр, и т.д.
  • Поддержка мотор-редукторов, настройка передаточного отношения энкодера
  • Регулятор учитывает “мёртвую зону” мотора
  • Все функции работают в градусах и “тиках” энкодера

Поддерживаемые платформы: все Arduino (используются стандартные Wiring-функции)

ДОКУМЕНТАЦИЯ


// управляет мотором. Вызывать как можно чаще (внутри таймер с периодом dt)
// принимает текущее положение вала мотора (по счёту энкодера)
// возвращает true если мотор всё ещё движется к цели
bool tick(long pos);

// установка передаточного отношения редуктора и энкодера
// пример: если редуктор 1:30 - передаём в функцию 30
// пример: если редуктор 1:30 и энкодер на 12 тиков - передаём 30*12
void setRatio(float ratio);

// установка периода регулятора (рекомендуется 2-50 миллисекунд)
void setDt(int dt);

// установка и получение текущей позиции в тиках энкодера и градусах.
// setCurrent(pos) равносильна вызову tick(pos) и в принципе не нужна!
void setCurrent(long pos);
long getCurrent();
long getCurrentDeg();

// установка и получение целевой позиции в тиках энкодера и градусах
void setTarget(long pos);
void setTargetDeg(long pos);
long getTarget();
long getTargetDeg();

// установка максимальной скорости в тиках энкодера/секунду и градусах/секунду
void setMaxSpeed(int speed);
void setMaxSpeedDeg(int speed);

// установка ускорения тиках энкодера и градусах в секунду
void setAcceleration(float accel);
void setAccelerationDeg(float accel);

// установка и получение целевой скорости в тиках энкодера/секунду и градусах/секунду
void setTargetSpeed(int speed);
void setTargetSpeedDeg(int speed);
int getTargetSpeed();
int getTargetSpeedDeg();

// получить текущую скорость в тиках энкодера/секунду и градусах/секунду
int getSpeed();
int getSpeedDeg();

// получить текущий ШИМ сигнал (float из ПИД регулятора)
float getDuty();

// ручная установка режима работы
// IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки
// ACCEL_POS - tick() работает в режиме плавного следования к целевому углу
// PID_POS - tick() работает в режиме резкого следования к целевому углу
// ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением)
// PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору
void setRunMode(runMode mode);

// возвращает true, если вал мотора заблокирован, а сигнал подаётся
bool isBlocked();

// коэффициенты ПИД регулятора
// пропорциональный - от него зависит агрессивность управления, нужно увеличивать kp
// при увеличении нагрузки на вал, чтобы регулятор подавал больший управляющий ШИМ сигнал
float kp = 2.0;// (знач. по умолчанию)

// интегральный - позволяет нивелировать ошибку со временем, имеет накопительный эффект
float ki = 0.9;// (знач. по умолчанию)

// дифференциальный. Позволяет чуть сгладить рывки, но при большом значении
// сам становится причиной рывков и раскачки системы!
float kd = 0.1;// (знач. по умолчанию)

// установить зону остановки мотора для режима стабилизации позиции (по умолч. 8)
void setStopZone(int zone);

ПРИМЕРЫ


/*
  Пример управления мотором при помощи драйвера полного моста и потенциометра
  Для режимов следования к позиции и удержания скорости
*/
#include "AccelMotor.h"
AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH);

// инициализация наследуется от GyverMotor
// варианты инициализации в зависимости от типа драйвера:
// AccelMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, level)
// AccelMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, level)
// AccelMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, level)
/*
  DRIVER2WIRE - двухпроводной драйвер (направление + ШИМ)
  DRIVER3WIRE - трёхпроводной драйвер (два пина направления + ШИМ)
  RELAY2WIRE - реле в качестве драйвера (два пина направления)

  dig_pin, dig_pin_A, dig_pin_B - любой цифровой пин МК
  PWM_pin - любой ШИМ пин МК
  level - LOW / HIGH - уровень драйвера. Если при увеличении скорости мотор наоборот тормозит - смени уровень
*/

void setup() {
  Serial.begin(9600);
  // использую мотор JGA25
  // редуктор 1:21.3
  // энкодер 12 тиков на оборот
  motor.setRatio(21.3 * 12);

  // период интегрирования (по умолч. 20)
  motor.setDt(30);  // миллисекунды

  // установка максимальной скорости для режима ACCEL_POS
  motor.setMaxSpeedDeg(600);  // в градусах/сек
  //motor.setMaxSpeed(400); // в тиках/сек

  // установка ускорения для режима ACCEL_POS
  motor.setAccelerationDeg(10);  // в градусах/сек/сек
  //motor.setAcceleration(10);  // в тиках

  // минимальный (по модулю) ШИМ сигнал (при котором мотор трогается)
  motor.setMinDuty(50);

  // коэффициенты ПИД регулятора
  motor.kp = 3;   // отвечает за резкость регулирования.
  // При малых значениях сигнала вообще не будет, при слишком больших – будет трясти

  motor.ki = 0.2; // отвечает за коррекцию ошибки в течение времени
  motor.kd = 0.1; // отвечает за компенсацию резких изменений
  
  // установить зону остановки мотора для режима стабилизации позиции в тиках (по умолч. 8)
  motor.setStopZone(10);

  motor.setRunMode(ACCEL_POS);
  // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки
  // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу
  // PID_POS - tick() работает в режиме резкого следования к целевому углу
  // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением)
  // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору
}

void loop() {
  // потенциометр на А0
  // преобразуем значение в -255.. 255
  static float val;
  val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор

  // для режима PID_SPEED/ACCEL_SPEED
  //motor.setTargetSpeedDeg(val*3);  // задаём целевую скорость в градусах/сек

  // для режима PID_POS/ACCEL_POS
  motor.setTargetDeg(val * 2);  // задаём новый целевой угол в градусах

  // обязательная функция. Делает все вычисления
  // принимает текущее значение с энкодера или потенциометра
  motor.tick(encTick(4));

  static uint32_t tmr = 0;
  if (millis() - tmr > 100) {   // таймер на 100мс для графиков
    tmr += 100;
    // отладка позиции (открой плоттер)
    Serial.print(motor.getTargetDeg());
    Serial.print(',');
    Serial.print(motor.getDuty());
    Serial.print(',');
    Serial.println(motor.getCurrentDeg());

    /*
      // отладка скорости (открой плоттер)
      Serial.print(motor.getTargetSpeedDeg());
      Serial.print(',');
      Serial.print(motor.getDuty());
      Serial.print(',');
      Serial.println(motor.getSpeedDeg());
    */
  }
}

// читаем энкодер вручную, через digitalRead()
long encTick(byte pin) {
  static bool lastState;
  static long encCounter = 0;
  bool curState = digitalRead(pin);       // опрос
  if (lastState != curState) {            // словили изменение
    lastState = curState;
    if (curState) {                       // по фронту
      encCounter += motor.getState();     // запомнили поворот
    }
  }
  return encCounter;
}
/*
  Пример опроса энкодера через digitalRead
*/
#include "AccelMotor.h"
AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH);
// подробнее об инициализации смотри в примере motor_demo

void setup() {
  Serial.begin(9600);
  motor.setRunMode(ACCEL_POS);
}

void loop() {
  // потенциометр на А0
  // преобразуем значение в -255.. 255
  static float val;
  val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор

  // для режима PID_SPEED/ACCEL_SPEED
  //motor.setTargetSpeedDeg(val*3);  // задаём целевую скорость в градусах/сек

  // для режима PID_POS/ACCEL_POS
  motor.setTargetDeg(val * 2);  // задаём новый целевой угол в градусах

  // обязательная функция. Делает все вычисления
  // принимает текущее значение с энкодера или потенциометра
  motor.tick(encTick(4));
}

// функция опроса энкодера через digitalRead()
long encTick(byte pin) {
  static bool lastState;
  static long encCounter = 0;
  bool curState = digitalRead(pin);       // опрос
  if (lastState != curState) {            // словили изменение
    lastState = curState;
    if (curState) {                       // по фронту
      encCounter += motor.getState();     // запомнили поворот
      // motor.getState() вернёт 1 или -1 в зависимости от направления
    }
  }
  return encCounter;
}
/*
  Пример опроса энкодера через аппаратные прерывания
*/
#include "AccelMotor.h"
AccelMotor motor(DRIVER2WIRE, 4, 3, HIGH);
// подробнее об инициализации смотри в примере motor_demo

void setup() {
  Serial.begin(9600);
  motor.setRunMode(ACCEL_POS);
  attachInterrupt(0, isr, RISING);
}

volatile long encCounter = 0;
void isr() {
  // опрос энкодера
  encCounter += motor.getState();
}

void loop() {
  // потенциометр на А0
  // преобразуем значение в -255.. 255
  static float val;
  val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор

  // для режима PID_SPEED/ACCEL_SPEED
  //motor.setTargetSpeedDeg(val*3);  // задаём целевую скорость в градусах/сек

  // для режима PID_POS/ACCEL_POS
  motor.setTargetDeg(val * 2);  // задаём новый целевой угол в градусах

  // обязательная функция. Делает все вычисления
  // принимает текущее значение с энкодера или потенциометра
  motor.tick(encCounter);
}

УСТАНОВКА БИБЛИОТЕКИ

Если вы не знаете, как установить библиотеку – читайте отдельный урок по работе с библиотеками!

БАГИ И ОШИБКИ

Если вы нашли баг или ошибку в исходнике или примерах, или у вас есть идеи по доработке библиотеки – пишите пожалуйста на почту alex@alexgyver.ru. В комментарии на страницах я заглядываю очень редко, на форум – ещё реже.

ОСТАЛЬНЫЕ БИБЛИОТЕКИ

У меня есть ещё очень много всего интересного! Смотрите полный список библиотек вот здесь.