ПЛАВНОЕ УПРАВЛЕНИЕ СЕРВОПРИВОДОМ

ВНИМАНИЕ! В версии 3.0 ускорение задавалось целым числом от 0 до 10. В версии 3.1 я вернул ускорение к дробным числам, как было раньше (для совместимости со старыми скетчами и проектами). Ускорение задаётся числом 0.01-1.0 (на самом деле можно чуть больше 1.0)

Вы наверняка работали с сервоприводами из под Arduino и знаете, как это выглядит: сервоприводу можно приказать повернуться на угол, и он с максимальной скоростью начнёт поворачиваться на этот угол. Это очень неправильно применять в реальных устройствах, потому что создаются лишние нагрузки и растёт потребление тока (большой стартовый ток). Можно ли крутить серво плавно? Можно! Я сделал библиотеку ServoSmooth, которая в этом поможет.

Зачем это нужно? В реальных устройствах, где нужно сервой повернуть/подвинуть тяжёлый объект, стандартный подход (дать сигнал и ждать поворота) работает на уничтожение редуктора привода, потому что объекты инерционные и быстро их разогнать и остановить невозможно! Ограничив максимальную скорость серво, разгон и торможение мы продлеваем ресурс редуктора в десятки раз, а также потребляем меньший ток за счёт плавности прикладывания момента. И очевидно получаем приятный визуальный эффект – нет резких рывков всей конструкции при разгоне-остановке.

Так как ESC контроллеры используют такой же протокол связи, мы автоматически получаем плавный разгон и торможение для бесколлекторных моторов (в этом случае за ускорение мотора отвечает максимальная скорость, метод setSpeed. Подумайте, это уже производная). И это круто!

[УСТАРЕЛО] Алгоритм работы для любопытных: работает всё на экспоненциальном бегущем среднем, именно оно обеспечивает плавный разгон и торможение. Ограничение скорости делается “дроблением” поворота серво по времени: серво поворачивается на несколько градусов по таймеру.

//по таймеру:
_newSpeed = _servoTargetPos - _servoCurrentPos;              // расчёт скорости
if (_servoState) {
 _newSpeed = constrain(_newSpeed, -_servoMaxSpeed, _servoMaxSpeed);    // ограничиваем по макс.
 _servoCurrentPos += _newSpeed;                                        // получаем новую позицию
 _newPos += (float)(_servoCurrentPos - _newPos) * _k;                  // и фильтруем её
 _newPos = constrain(_newPos, _min, _max);                             // ограничиваем
 _servo.writeMicroseconds(_newPos);                                    // отправляем на серво
}

Новый алгоритм работает по другому, обеспечивая более плавный разгон. Ускорение осуществляется двойным интегрированием позиции: к ней прибавляется скорость, к которой прибавляется ускорение. Торможение начинается с момента, полученного из школьной формулы S=V*V/(2*a). Для любознательных прикреплю алгоритм ниже.

float err = _targetPos - _currentPos;
if (abs(err) > 0.1) {
  bool thisDir = (_speed * _speed / _maxAcceleration / 2.0 >= abs(err));  // пора тормозить
  _speed += _maxAcceleration * delta * (thisDir ? -_sign(_speed) : _sign(err));
  _speed = constrain(_speed, -_maxSpeed, _maxSpeed);
  _currentPos += _speed * delta;
}

БИБЛИОТЕКА SERVOSMOOTH

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

  • Является дополнением к стандартной библиотеке Servo
  • Поддерживает работу с расширителем серво PCA9685 (начиная с v3.0)
  • Настройка максимальной скорости сервопривода
  • Настройка ускорения (разгон и торможение) сервопривода
  • Плавный пуск из любого положения при запуске программы (начиная с v3.2)
  • При использовании ESC и БК мотора получаем “плавный пуск” мотора
  • Установка целевой позиции серво по углу (0-180) и длине импульса (500-2400)
  • Автоматическое отключение привода по таймауту неактивности и включение при изменении позиции (настраивается)
  • Нативная поддержка серво с любым диапазоном по углу (180, 270, 360), см. документацию

Поддерживаемые платформы: все Arduino-совместимые платы (библиотека является дополнением к стандартной библиотеке Servo и PCA9685)

v1.1 – автоматическое отключение (detach) при достижении цели
v1.2 – вкл/выкл автоотключения серво
v1.3 – отдельный метод для установки и чтения текущего положения. Добавлен вариант метода attach
v1.4 – улучшена совместимость
v1.5 – исправлены getCurrent и getCurrentDeg
v1.6 – чуть оптимизирована инициализация
v1.7 – исправлен баг с низкой скоростью/ускорением, код оптимизирован
v1.8 – улучшена стабильность
v1.9 – добавлена настройка макс. угла серво
v1.10 – исправлен баг когда текущая позиция совпадает с позицией таргета
v2.0 – упрощён алгоритм
v2.1 – добавлена смена направления
v2.2 – фикс движения в инверсии (спасибо VICLER) и функций write (спасибо CheDima)
v3.0
– Добавлен полностью новый, более плавный алгоритм
– Почищен мусор
– Добавлена поддержка PCA9685
– “Плавность” вынесена в базовый класс для упрощения добавления поддержки новых библиотек серво
v3.1 – оптимизирован и облегчён алгоритм, скорость задаётся в градусах/сек
v3.2 – исправлен баг с резким поворотом при первом tick, добавлена smoothStart
v3.3 – исправлен баг, возникающий если не вызывать tick
v3.4 – при установке ускорения в 0 активируется профиль постоянной скорости
v3.5
– Улучшена/исправлена работа stop
– Поправлены ошибки с округлением
– Исправлена проблема с медленным изменением target
v3.6 – исправлены баги
v3.6 – добавлена установка ускорения в градусах/сек/сек

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


Данная библиотека является “дополнением” к стандартной библиотеке Servo.h и позволяет плавно управлять сервоприводом. Суть работы кроется в методе tick(), который нужно вызывать постоянно в loop (или прерывании таймера), внутри тика находится алгоритм с собственным таймером, который по чуть чуть поворачивает серво к нужному положению. Библиотека дублирует несколько методов из Servo.h (attach имеет расширенную инициализацию):

  • write() и writeMicroseconds() – повернут вал серво с максимальной скоростью
  • attach() и detach() – подключить и отключить серво от управления

Инициализация


Объект создаётся точно так же, как в Servo.h, без параметров. Также можно передать рабочий угол серво (если не передавать, будет равен стандартному 180 град.)

#include "ServoSmooth.h"  // подключили либу
ServoSmooth servo;        // создали объект
ServoSmooth servo(270);   // создали с указанием макс. угла серво

По инициализации  attach()  есть несколько вариантов:

  • attach(pin); – подключит серво на указанный pin, угол поворота будет установлен на 0 градусов. Длина импульса* мин-макс будет стандартная, 500-2400 мкс
  • attach(pin, target); – подключит серво на указанный pin, угол поворота** будет установлен на target градусов. Длина импульса* мин-макс будет стандартная, 500-2400 мкс
  • attach(pin, min, max); – подключит серво на указанный pin, угол поворота будет установлен на 0 градусов. Длина импульса* будет установлена min и max соответственно.
  • attach(pin, min, max, target); – подключит серво на указанный pin, угол поворота будет установлен на target градусов. Длина импульса* будет установлена min и max соответственно.

*Длина импульса – сервопривод управляется ШИМ сигналом, в котором длина импульса прямо управляет углом поворота, то есть подавая минимальную и максимальную длину мы получаем рабочий угол 180 градусов. По умолчанию мин. и макс. длина установлены 500 и 2400 соответственно, что подходит большинству сервоприводов, но желательно посмотреть и “откалибровать” свой привод так, чтобы он работал на все 180 градусов. Мин. и макс. время импульса отличаются у разных производителей и моделей серво.

**Указание угла поворота при инициализации устанавливает серво на нужный угол сразу при подаче сигнала, а также выставляет текущую и целевую позицию равными этой.

Плавный пуск (new!)


Сервопривод не имеет обратной связи по углу (для программы), поэтому при запуске будет “резко” повёрнут на стартовый угол (“в ноль” по умолчанию или на указанный в attach(pin, target). Есть два варианта избежать резких рывков в механизме при запуске программы:

  • Заранее знать, на какой угол физически повёрнут привод при запуске и передать его в attach(pin, target). Как узнать? Зависит от конкретной задачи и логики работы программы. Можно запоминать положение сервы в ЕЕПРОМ и восстанавливать при запуске, можно устанавливать серво в один и тот же угол перед выключением/перезагрузкой системы, и т.д.
  • Воспользоваться фичей smoothStart(), которая появилась в версии 3.2 данной библиотеки. Работает она очень просто: аттачит и детачит сервопривод с периодом в пару десятков миллисекунд, таким образом привод плавно движется до заданного угла из любого начального положения. Вызывать smoothStart() нужно однократно (при старте программы) сразу после attach(pin, target) в блоке setup(). Внимание! Функция блокирующая, выполнение занимает 900 миллисекунд. Период “рывка” сервопривода выбран минимальный, при котором серво начинает понимать, чего от неё хотят. Период довольно большой, поэтому движение к заданной позиции происходит рывками, но в целом гораздо плавнее, чем без smoothStart(). В массивном   механизме рывки практически незаметны!
servo.attach(2, 35);  // стартовый угол 35 градусов
servo.smoothStart();  // "плавно" движемся к нему

Управление


Движение серво происходит автоматически в методе tick(), нам нужно всего лишь вызывать его как можно чаще в loop() (tick() имеет встроенный таймер на 20 миллисекунд). Также есть метод tickManual(), который поворачивает серву на следующий “шаг” при каждом вызове (тот же tick(), но не имеет своего таймера). Оба метода tick() возвращают false, пока серво движется, и true, когда серво достигла установленного угла, это можно использовать. Также серво автоматически отключается от управления при достижении заданного угла поворота (это уменьшает жужжание серво в простое). Эту функцию можно отключить, вызвав setAutoDetach(false). Инструменты для управления движением привода:

  • setTarget(длина); – устанавливает целевую позицию для серво в величине длина импульса, мкс (~500-2400)
  • setTargetDeg(угол); – устанавливает целевую позицию для серво в градусах (0-180)
  • setSpeed(скорость); – установка максимальной скорости (больше нуля) в градусах в секунду
  • setAccel(ускорение); – установка ускорения (float числа 0.01 – 1.0). Можно больше 1, будет ещё резче. Если установить ускорение 0 – оно будет отключено и серво будет двигаться по профилю постоянной скорости (с бесконечным ускорением)
    • Если передавать ускорение в целых числах (с версии 3.7 библиотеки) – ускорение будет установлено в градусах/сек/сек. Рабочий диапазон ускорений 1 - 1500, чем больше – тем резче. При значении 0 ускорение будет отключено.
  • start(); – автоматический attach + разрешает работу tick – серво движется к заданной позиции
  • stop(); – detach + запрещает работу tick – серво останавливается

Полезные вспомогательные методы для различных ситуаций:

  • setDirection(напр); – принимает NORMAL (false) или REVERSE (true), меняет направление серво
  • setCurrent(длина); – установка текущей позиции в мкс (500 – 2400). Может пригодиться в ситуации, когда мы знаем реальный угол серво и хотим сообщить о нём программе, чтобы алгоритм не дёргал привод.
  • setCurrentDeg(угол); – установка текущей позиции в градусах (0-180). Зависит от min и max.
  • getCurrent(); – получение текущей позиции в мкс (500 – 2400)
  • getCurrentDeg(); – получение текущей позиции в градусах (0-180). Зависит от min и max
  • getTarget(); – получение целевой позиции в мкс (500 – 2400)
  • getTargetDeg(); – получение целевой позиции в градусах (0-180). Зависит от min и max
  • setMaxAngle(); – установка макс. угла серво, по умолчанию 180. Позволяет удобно работать с разными сервами (на 270 и 360 градусов)

Расширитель серво PCA9685


В версии библиотеки 3 и выше добавлена поддержка драйвера PCA9685, подключать нужно файл #include "ServoDriverSmooth.h"

  • ServoDriverSmooth servo;  // по умолчанию (адрес 0x40, угол 180)
  • ServoDriverSmooth servo(0x40); // с указанием адреса драйвера
  • ServoDriverSmooth servo(0x40, 270); // с указанием адреса и макс. угла

Метод attach(pin) принимает номер вывода на драйвере. В остальном всё работает точно так же, как с обычной сервой.

void write(uint16_t angle);                 // аналог метода из библиотеки Servo
void writeMicroseconds(uint16_t angle);     // аналог метода из библиотеки Servo
void attach(uint8_t pin);                   // аналог метода из библиотеки Servo
void attach(uint8_t pin, int min, int max); // аналог метода из библиотеки Servo. min по умолч. 500, max 2400
void detach();                              // аналог метода detach из библиотеки Servo
void start();                               // attach + разрешает работу tick
void stop();                                // detach + запрещает работу tick
  
boolean tick();                             // метод, управляющий сервой, должен опрашиваться как можно чаще.
                                            // Возвращает true, когда целевая позиция достигнута.
                                            // Имеет встроенный таймер с периодом SERVO_PERIOD

boolean tickManual();                       // метод, управляющий сервой, без встроенного таймера.
                                            // Возвращает true, когда целевая позиция достигнута

void setSpeed(int speed);                   // установка максимальной скорости (градусы в секунду)
void setAccel(float accel);                 // установка ускорения (0.05 - 1.0). При значении 1 ускорение максимальное. 0 - отключено
void setAccel(int accel);                   // установка ускорения в градусах/сек/сек (рабочее от 1 до ~1500). 0 - отключено
void setTarget(int target);                 // установка целевой позиции в мкс (500 - 2400)
void setTargetDeg(int target);              // установка целевой позиции в градусах (0-макс. угол). Зависит от min и max
void setAutoDetach(boolean set);            // вкл/выкл автоматического отключения (detach) при достижении угла. По умолч. вкл
void setCurrent(int target);                // установка текущей позиции в мкс (500 - 2400)
void setCurrentDeg(int target);             // установка текущей позиции в градусах (0-макс. угол). Зависит от min и max
void setMaxAngle(int maxAngle);             // установка макс. угла привода
int getCurrent();                           // получение текущей позиции в мкс (500 - 2400)
int getCurrentDeg();                        // получение текущей позиции в градусах (0-макс. угол). Зависит от min и max
int getTarget();                            // получение целевой позиции в мкс (500 - 2400)
int getTargetDeg();                         // получение целевой позиции в градусах (0-макс. угол). Зависит от min и max
void smoothStart();							// вызывать сразу после attach(пин, таргет). Смягчает движение серво из неизвестной позиции к стартовой. БЛОКИРУЮЩАЯ НА 1  СЕК!

ПРИМЕРЫ


/*
  Данный скетч крутит 4 сервопривода с разными скоростями и ускорениями
  Документация: https://alexgyver.ru/servosmooth/
 */

#define AMOUNT 4  // кол-во серво

#include "ServoSmooth.h"
ServoSmooth servos[AMOUNT];

uint32_t servoTimer;
uint32_t turnTimer;

int position1 = 10;   // первое положение серв
int position2 = 160;  // второе положение серв
boolean flag;

void setup() {
  Serial.begin(9600);
  // подключаем
  servos[0].attach(2);
  servos[1].attach(3);
  servos[2].attach(4);
  servos[3].attach(5);

  // настраиваем макс. скорости и ускорения
  // скор. по умолч. 100
  // ускорение по умолч. 0.5
  servos[0].setSpeed(20);
  servos[1].setAccel(0.2);
  servos[2].setSpeed(100);
  servos[3].setAccel(0.05);
}

void loop() {
  // каждые 20 мс
  if (millis() - servoTimer >= 20) {	// взводим таймер на 20 мс (как в библиотеке)
    servoTimer += 20;
    for (byte i = 0; i < AMOUNT; i++) { servos[i].tickManual(); // двигаем все сервы. Такой вариант эффективнее отдельных тиков } } // каждые 2 секунды меняем положение if (millis() - turnTimer >= 2000) {
    turnTimer = millis();
    flag = !flag;
    for (byte i = 0; i < AMOUNT; i++) {
      if (flag) servos[i].setTargetDeg(position1);
      else servos[i].setTargetDeg(position2);
    }
  }
}
/*
   Данный код плавно управляет одной сервой (на пине 2)
   при помощи потенциометра (на пине А0).
   Откройте порт по последовательному соединению для наблюдения за положением серво
   Документация: https://alexgyver.ru/servosmooth/
*/

#include "ServoSmooth.h"
ServoSmooth servo;

uint32_t myTimer;

void setup() {
  Serial.begin(9600);
  servo.attach(A1, 600, 2400);  // 600 и 2400 - длины импульсов, при которых
  // серво поворачивается максимально в одну и другую сторону, зависят от самой серво
  // и обычно даже указываются продавцом. Мы их тут указываем для того, чтобы
  // метод setTargetDeg() корректно отрабатывал диапазон поворота сервы

  servo.setSpeed(90);   // ограничить скорость
  servo.setAccel(0.1);  // установить ускорение (разгон и торможение)
}

void loop() {
  // желаемая позиция задаётся методом setTarget (импульс) или setTargetDeg (угол), далее
  // при вызове tick() производится автоматическое движение сервы
  // с заданным ускорением и ограничением скорости
  boolean state = servo.tick();   // здесь происходит движение серво по встроенному таймеру!


  if (millis() - myTimer >= 40) {
    myTimer = millis();
    int newPos = map(analogRead(A2), 0, 1023, 500, 2400); // берём с потенцометра значение 500-2400 (импульс)
    servo.setTarget(newPos);               // и отправляем на серво
    Serial.println(String(newPos) + " " + String(servo.getCurrent())/* + " " + String(state)*/);
  // state показывает сотояние сервы (0 - движется, 1 - приехали и отключились)
  }
}
/*
   Данный код плавно управляет одной сервой   
   при помощи потенциометра (на пине А0).
   Используется драйвер PCA9685
   Откройте порт по последовательному соединению для наблюдения за положением серво
   Документация: https://alexgyver.ru/servosmooth/
*/

#include "ServoDriverSmooth.h"
ServoDriverSmooth servo;
//ServoDriverSmooth servo(0x40);      // с указанием адреса драйвера
//ServoDriverSmooth servo(0x40, 270); // с указанием адреса и макс. угла

uint32_t myTimer;

void setup() {
  Serial.begin(9600);
  servo.attach(0, 150, 550);  // 150 и 600 - длины импульсов, при которых
  // серво поворачивается максимально в одну и другую сторону, зависят от самой серво
  // и обычно даже указываются продавцом. Мы их тут указываем для того, чтобы
  // метод setTargetDeg() корректно отрабатывал диапазон поворота сервы
  // для драйвера диапазон в районе 150-600! Не как у обычной серво

  servo.setSpeed(120);    // ограничить скорость
  servo.setAccel(0.5);    // установить ускорение (разгон и торможение)
}

void loop() {
  // желаемая позиция задаётся методом setTarget (импульс) или setTargetDeg (угол), далее
  // при вызове tick() производится автоматическое движение сервы
  // с заданным ускорением и ограничением скорости
  boolean state = servo.tick();   // здесь происходит движение серво по встроенному таймеру!


  if (millis() - myTimer >= 40) {
    myTimer = millis();
    int newPos = map(analogRead(0), 0, 1023, 0, 180); // берём с потенцометра значение 0-180
    servo.setTargetDeg(newPos);                       // и отправляем на серво
    Serial.println(String(newPos) + " " + String(servo.getCurrentDeg())/* + " " + String(state)*/);
  // state показывает сотояние сервы (0 - движется, 1 - приехали и отключились)
  }
}

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

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

БАГИ И ОШИБКИ

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

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

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