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

Вы наверняка работали с сервоприводами из под 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);                                    // отправляем на серво
}

БИБЛИОТЕКА SERVOSMOOTH

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

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

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

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


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

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

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


Объект создаётся точно так же, как в Servo.h, без параметров

#include "ServoSmooth.h"  // подключили либу
ServoSmooth servo;        // создали объект

По инициализации ( 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 градусов. Мин. и макс. время импульса отличаются у разных производителей и моделей серво.

Управление


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

  • setTarget(длина) – устанавливает целевую позицию для серво в величине длина импульса, мкс (~500-2400)
  • setTargetDeg(угол) – устанавливает целевую позицию для серво в градусах (0-180)
  • setSpeed(скорость) – установка максимальной скорости (условные единицы, 0 – 200)
  • setAccel(ускорение) – установка ускорения (0.05 – 1). При значении 1 ускорение максимальное
  • start() – автоматический attach + разрешает работу tick – серво движется к заданной позиции
  • stop() – detach + запрещает работу tick – серво останавливается

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

  • setCurrent(длина) – установка текущей позиции в мкс (500 – 2400). Может пригодиться в ситуации, когда мы знаем реальный угол серво и хотим сообщить о нём программе, чтобы алгоритм не дёргал привод.
  • setCurrentDeg(угол) – установка текущей позиции в градусах (0-180). Зависит от min и max.
  • getCurrent() – получение текущей позиции в мкс (500 – 2400)
  • getCurrentDeg() – получение текущей позиции в градусах (0-180). Зависит от min и max
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);                   // установка максимальной скорости (условные единицы, 0 - 200)
void setAccel(float accel);                 // установка ускорения (0.05 - 1). При значении 1 ускорение максимальное
void setTarget(int target);                 // установка целевой позиции в мкс (500 - 2400)
void setTargetDeg(int target);              // установка целевой позиции в градусах (0-180). Зависит от min и max
void setAutoDetach(boolean set);            // вкл/выкл автоматического отключения (detach) при достижении угла. По умолч. вкл

ПРИМЕРЫ


/*
 * Данный скетч крутит 4 сервопривода с разными скоростями и ускорением
 */

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

#include 
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);

  // настраиваем макс. скорости и ускорения
  // скор. по умолч. 50
  // ускорение по умолч. 0.1
  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) {
    servoTimer = millis();
    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).
*/

#include "ServoSmooth.h"
ServoSmooth servo;

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

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

  int newPos = map(analogRead(0), 0, 1023, 0, 180); // берём с потенцометра значение 0-180
  servo.setTargetDeg(newPos);     					// и отправляем на серво
}
/*
   Данный код плавно управляет одной сервой (на пине 2)
   при помощи потенциометра (на пине А0).
   Откройте порт по последовательному соединению для наблюдения за положением серво
*/

#include "ServoSmooth.h"
ServoSmooth servo;

uint32_t myTimer;

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

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

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


  if (millis() - myTimer >= 40) {
    myTimer = millis();
    int newPos = map(analogRead(0), 0, 1023, 500, 2400); // берём с потенцометра значение 0-180
    servo.setTarget(newPos);               // и отправляем на серво
    Serial.println(String(newPos) + " " + String(servo._servoCurrentPos)/* + " " + String(state)*/);
  }
}

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

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