├── .gitattributes ├── .github └── workflows │ └── tg-send.yml ├── LICENSE ├── README.md ├── README_EN.md ├── examples ├── SmoothSweep │ └── SmoothSweep.ino ├── SmoothSweepDriver │ └── SmoothSweepDriver.ino ├── SmoothSweepTest │ └── SmoothSweepTest.ino ├── manyServos │ └── manyServos.ino ├── potControl │ └── potControl.ino ├── potControlPlot │ └── potControlPlot.ino └── potControlPlotDriver │ └── potControlPlotDriver.ino ├── keywords.txt ├── library.properties └── src ├── Adafruit_PWMServoDriver.cpp ├── Adafruit_PWMServoDriver.h ├── ServoDriverSmooth.cpp ├── ServoDriverSmooth.h ├── ServoSmooth.cpp ├── ServoSmooth.h ├── smoothUtil.cpp └── smoothUtil.h /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.github/workflows/tg-send.yml: -------------------------------------------------------------------------------- 1 | 2 | name: Telegram Message 3 | on: 4 | release: 5 | types: [published] 6 | jobs: 7 | build: 8 | name: Send Message 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: send telegram message on push 12 | uses: appleboy/telegram-action@master 13 | with: 14 | to: ${{ secrets.TELEGRAM_TO }} 15 | token: ${{ secrets.TELEGRAM_TOKEN }} 16 | disable_web_page_preview: true 17 | message: | 18 | ${{ github.event.repository.name }} v${{ github.event.release.tag_name }} 19 | ${{ github.event.release.body }} 20 | https://github.com/${{ github.repository }} 21 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Alex 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![latest](https://img.shields.io/github/v/release/GyverLibs/ServoSmooth.svg?color=brightgreen)](https://github.com/GyverLibs/ServoSmooth/releases/latest/download/ServoSmooth.zip) 2 | [![PIO](https://badges.registry.platformio.org/packages/gyverlibs/library/ServoSmooth.svg)](https://registry.platformio.org/libraries/gyverlibs/ServoSmooth) 3 | [![Foo](https://img.shields.io/badge/Website-AlexGyver.ru-blue.svg?style=flat-square)](https://alexgyver.ru/) 4 | [![Foo](https://img.shields.io/badge/%E2%82%BD%24%E2%82%AC%20%D0%9F%D0%BE%D0%B4%D0%B4%D0%B5%D1%80%D0%B6%D0%B0%D1%82%D1%8C-%D0%B0%D0%B2%D1%82%D0%BE%D1%80%D0%B0-orange.svg?style=flat-square)](https://alexgyver.ru/support_alex/) 5 | [![Foo](https://img.shields.io/badge/README-ENGLISH-blueviolet.svg?style=flat-square)](https://github-com.translate.goog/GyverLibs/ServoSmooth?_x_tr_sl=ru&_x_tr_tl=en) 6 | 7 | [![Foo](https://img.shields.io/badge/ПОДПИСАТЬСЯ-НА%20ОБНОВЛЕНИЯ-brightgreen.svg?style=social&logo=telegram&color=blue)](https://t.me/GyverLibs) 8 | 9 | # ServoSmooth 10 | Библиотека для плавного управления сервоприводами 11 | - Дополнение к стандартной библиотеке Servo 12 | - Настройка максимальной скорости сервопривода 13 | - Настройка ускорения (разгон и торможение) сервопривода 14 | - Неблокирующая работа 15 | - Трапецеидальный профиль скорости 16 | - При использовании ESC и БК мотора получаем "плавный пуск" мотора 17 | - Установка целевой позиции серво по углу (0-180) и длине импульса (500-2400) 18 | - Автоматическое отключение (detach) при достижении цели 19 | - Плавный пуск при подключении серво 20 | - Поддержка расширителя PCA9685 21 | 22 | ### Совместимость 23 | Совместима со всеми Arduino платформами (используются Arduino-функции) 24 | 25 | ### Документация 26 | К библиотеке есть [расширенная документация](https://alexgyver.ru/ServoSmooth/) 27 | 28 | ## Содержание 29 | - [Установка](#install) 30 | - [Инициализация](#init) 31 | - [Использование](#usage) 32 | - [Пример](#example) 33 | - [Версии](#versions) 34 | - [Баги и обратная связь](#feedback) 35 | 36 | 37 | ## Установка 38 | - Библиотеку можно найти по названию **ServoSmooth** и установить через менеджер библиотек в: 39 | - Arduino IDE 40 | - Arduino IDE v2 41 | - PlatformIO 42 | > После подключения библиотеки, в **platformio.ini** необходимо добавить **arduino-libraries/Servo** и **Wire**. Таким образом, список должен будет состоять как минимум из 3х библиотек: 43 | ``` 44 | lib_deps = 45 | gyverlibs/ServoSmooth 46 | arduino-libraries/Servo 47 | Wire 48 | ``` 49 | - [Скачать библиотеку](https://github.com/GyverLibs/ServoSmooth/archive/refs/heads/main.zip) .zip архивом для ручной установки: 50 | - Распаковать и положить в *C:\Program Files (x86)\Arduino\libraries* (Windows x64) 51 | - Распаковать и положить в *C:\Program Files\Arduino\libraries* (Windows x32) 52 | - Распаковать и положить в *Документы/Arduino/libraries/* 53 | - (Arduino IDE) автоматическая установка из .zip: *Скетч/Подключить библиотеку/Добавить .ZIP библиотеку…* и указать скачанный архив 54 | - Читай более подробную инструкцию по установке библиотек [здесь](https://alexgyver.ru/arduino-first/#%D0%A3%D1%81%D1%82%D0%B0%D0%BD%D0%BE%D0%B2%D0%BA%D0%B0_%D0%B1%D0%B8%D0%B1%D0%BB%D0%B8%D0%BE%D1%82%D0%B5%D0%BA) 55 | ### Обновление 56 | - Рекомендую всегда обновлять библиотеку: в новых версиях исправляются ошибки и баги, а также проводится оптимизация и добавляются новые фичи 57 | - Через менеджер библиотек IDE: найти библиотеку как при установке и нажать "Обновить" 58 | - Вручную: **удалить папку со старой версией**, а затем положить на её место новую. "Замену" делать нельзя: иногда в новых версиях удаляются файлы, которые останутся при замене и могут привести к ошибкам! 59 | 60 | 61 | 62 | ## Инициализация 63 | ```cpp 64 | ServoSmooth servo; 65 | ServoDriverSmooth servo; // для PCA9685 66 | ``` 67 | 68 | 69 | ## Использование 70 | ```cpp 71 | void write(uint16_t angle); // аналог метода из библиотеки Servo 72 | void writeMicroseconds(uint16_t angle); // аналог метода из библиотеки Servo 73 | void attach(uint8_t pin); // аналог метода из библиотеки Servo 74 | void attach(uint8_t pin, int min, int max); // аналог метода из библиотеки Servo. min по умолч. 500, max 2400 75 | void detach(); // аналог метода detach из библиотеки Servo 76 | void start(); // attach + разрешает работу tick 77 | void stop(); // detach + запрещает работу tick 78 | 79 | boolean tick(); // метод, управляющий сервой, должен опрашиваться как можно чаще. 80 | // Возвращает true, когда целевая позиция достигнута. 81 | // Имеет встроенный таймер с периодом SERVO_PERIOD 82 | boolean tickManual(); // метод, управляющий сервой, без встроенного таймера. 83 | // Возвращает true, когда целевая позиция достигнута 84 | void setSpeed(int speed); // установка максимальной скорости (градусы в секунду) 85 | void setAccel(float accel); // установка ускорения (0.05 - 1.0). При значении 1 ускорение максимальное. 0 - отключено 86 | void setAccel(int accel); // установка ускорения в градусах/сек/сек (рабочее от 1 до ~1500). 0 - отключено 87 | void setTarget(int target); // установка целевой позиции в мкс (500 - 2400) 88 | void setTargetDeg(int target); // установка целевой позиции в градусах (0-макс. угол). Зависит от min и max 89 | void setAutoDetach(boolean set); // вкл/выкл автоматического отключения (detach) при достижении угла. По умолч. вкл 90 | void setCurrent(int target); // установка текущей позиции в мкс (500 - 2400) 91 | void setCurrentDeg(int target); // установка текущей позиции в градусах (0-макс. угол). Зависит от min и max 92 | void setMaxAngle(int maxAngle); // установка макс. угла привода 93 | int getCurrent(); // получение текущей позиции в мкс (500 - 2400) 94 | int getCurrentDeg(); // получение текущей позиции в градусах (0-макс. угол). Зависит от min и max 95 | int getTarget(); // получение целевой позиции в мкс (500 - 2400) 96 | int getTargetDeg(); // получение целевой позиции в градусах (0-макс. угол). Зависит от min и max 97 | void smoothStart(); // вызывать сразу после attach(пин, таргет). Смягчает движение серво из неизвестной позиции к стартовой. БЛОКИРУЮЩАЯ НА 1 СЕК! 98 | ``` 99 | 100 | 101 | ## Пример 102 | Остальные примеры смотри в **examples**! 103 | ```cpp 104 | /* 105 | Данный код плавно управляет одной сервой (на пине 2) 106 | при помощи потенциометра (на пине А0) 107 | Документация: https://alexgyver.ru/servosmooth/ 108 | */ 109 | 110 | #include 111 | ServoSmooth servo; 112 | 113 | void setup() { 114 | Serial.begin(9600); 115 | servo.attach(2, 600, 2400); // 600 и 2400 - длины импульсов, при которых 116 | // серво поворачивается максимально в одну и другую сторону, зависят от самой серво 117 | // и обычно даже указываются продавцом. Мы их тут указываем для того, чтобы 118 | // метод setTargetDeg() корректно отрабатывал полный диапазон поворота сервы 119 | 120 | servo.setSpeed(50); // ограничить скорость 121 | servo.setAccel(0.3); // установить ускорение (разгон и торможение) 122 | 123 | servo.setAutoDetach(false); // отключить автоотключение (detach) при достижении целевого угла (по умолчанию включено) 124 | } 125 | 126 | void loop() { 127 | // желаемая позиция задаётся методом setTarget (импульс) или setTargetDeg (угол), далее 128 | // при вызове tick() производится автоматическое движение сервы 129 | // с заданным ускорением и ограничением скорости 130 | servo.tick(); // здесь происходит движение серво по встроенному таймеру! 131 | 132 | int newPos = map(analogRead(0), 0, 1023, 0, 180); // берём с потенцометра значение 0-180 133 | servo.setTargetDeg(newPos); // и отправляем на серво 134 | } 135 | ``` 136 | 137 | 138 | ## Версии 139 | - v1.1 - автоматическое отключение (detach) при достижении цели 140 | - v1.2 - вкл/выкл автоотключения серво 141 | - v1.3 - отдельный метод для установки и чтения текущего положения. Добавлен вариант метода attach 142 | - v1.4 - улучшена совместимость 143 | - v1.5 - исправлены getCurrent и getCurrentDeg 144 | - v1.6 - чуть оптимизирована инициализация 145 | - v1.7 - исправлен баг с низкой скоростью/ускорением, код оптимизирован 146 | - v1.8 - улучшена стабильность 147 | - v1.9 - добавлена настройка макс. угла серво 148 | - v1.10 - исправлен баг когда текущая позиция совпадает с позицией таргета 149 | 150 | - v2.0 - упрощён алгоритм 151 | - v2.1 - добавлена смена направления 152 | - v2.2 - фикс движения в инверсии (спасибо VICLER) и функций write (спасибо CheDima) 153 | 154 | - v3.0 155 | - Добавлен полностью новый, более плавный алгоритм 156 | - Почищен мусор 157 | - Добавлена поддержка PCA9685 158 | - "Плавность" вынесена в базовый класс для упрощения добавления поддержки новых библиотек серво 159 | 160 | - v3.1 - оптимизирован и облегчён алгоритм, скорость задаётся в градусах/сек 161 | - v3.2 - исправлен баг с резким поворотом при первом tick, добавлена smoothStart 162 | - v3.3 - исправлен баг, возникающий если не вызывать tick 163 | - v3.4 - при установке ускорения в 0 активируется профиль постоянной скорости 164 | 165 | - v3.5 166 | - Улучшена/исправлена работа stop 167 | - Поправлены ошибки с округлением 168 | - Исправлена проблема с медленным изменением target 169 | 170 | - v3.6 - Исправлены мелкие баги, вырезан дебаг с 3.5 171 | - v3.7 - Добавлено задание ускорения в градусах/сек/сек 172 | - v3.8 - Исправлен невозврат тика при autoDetach(false) 173 | - v3.9 - поддержка ESP32 174 | 175 | 176 | ## Баги и обратная связь 177 | При нахождении багов создавайте **Issue**, а лучше сразу пишите на почту [alex@alexgyver.ru](mailto:alex@alexgyver.ru) 178 | Библиотека открыта для доработки и ваших **Pull Request**'ов! 179 | 180 | 181 | При сообщении о багах или некорректной работе библиотеки нужно обязательно указывать: 182 | - Версия библиотеки 183 | - Какой используется МК 184 | - Версия SDK (для ESP) 185 | - Версия Arduino IDE 186 | - Корректно ли работают ли встроенные примеры, в которых используются функции и конструкции, приводящие к багу в вашем коде 187 | - Какой код загружался, какая работа от него ожидалась и как он работает в реальности 188 | - В идеале приложить минимальный код, в котором наблюдается баг. Не полотно из тысячи строк, а минимальный код 189 | -------------------------------------------------------------------------------- /README_EN.md: -------------------------------------------------------------------------------- 1 | This is an automatic translation, may be incorrect in some places. See sources and examples! 2 | 3 | # SERVOSMOOTH 4 | Library for smooth control of servo drives 5 | - Addition to the Standard Library Servo 6 | - Setting the maximum velocity of servo drive 7 | - setting acceleration (acceleration and braking) of servo drive 8 | - Nebenching work 9 | - trapezoidal speed profile 10 | - when using ESC and BC motor, we get a "smooth start" motor 11 | -Installation of the target position of the Servo in the corner (0-180) and pulse length (500-2400) 12 | - automatic shutdown (Detach) when reaching the goal 13 | - smooth start when connecting servo 14 | - Support for the exterator PCA9685 15 | 16 | ## compatibility 17 | Compatible with all arduino platforms (used arduino functions) 18 | 19 | ### Documentation 20 | There is [extended documentation] to the library (https://alexgyver.ru/servosmooth/) 21 | 22 | ## Content 23 | - [installation] (# Install) 24 | - [initialization] (#init) 25 | - [use] (#usage) 26 | - [Example] (# Example) 27 | - [versions] (#varsions) 28 | - [bugs and feedback] (#fedback) 29 | 30 | 31 | ## Installation 32 | - The library can be found by the title ** Servosmooth ** and installed through the library manager in: 33 | - Arduino ide 34 | - Arduino ide v2 35 | - Platformio 36 | - [download the library] (https://github.com/gyverlibs/servosmooth/archive/refs/heads/main.zip). Zip archive for manual installation: 37 | - unpack and put in * C: \ Program Files (X86) \ Arduino \ Libraries * (Windows X64) 38 | - unpack and put in * C: \ Program Files \ Arduino \ Libraries * (Windows X32) 39 | - unpack and put in *documents/arduino/libraries/ * 40 | - (Arduino id) Automatic installation from. Zip: * sketch/connect the library/add .Zip library ... * and specify downloaded archive 41 | - Read more detailed instructions for installing libraries [here] (https://alexgyver.ru/arduino-first/#%D0%A3%D1%81%D1%82%D0%B0%BD%D0%BE%BE%BE%BED0%B2%D0%BA%D0%B0_%D0%B1%D0%B8%D0%B1%D0%BB%D0%B8%D0%BE%D1%82%D0%B5%D0%BA) 42 | ### Update 43 | - I recommend always updating the library: errors and bugs are corrected in the new versions, as well as optimization and new features are added 44 | - through the IDE library manager: find the library how to install and click "update" 45 | - Manually: ** remove the folder with the old version **, and then put a new one in its place.“Replacement” cannot be done: sometimes in new versions, files that remain when replacing are deleted and can lead to errors! 46 | 47 | 48 | 49 | ## initialization 50 | `` `CPP 51 | Servosmooth Servo; 52 | Servodriversmooth Servo;// for PCA9685 53 | `` ` 54 | 55 | 56 | ## Usage 57 | `` `CPP 58 | VOID Write (Uint16_T Angle);// analogue of the method from the Servo library 59 | VOID Writemicroseconds (Uint16_T Angle);// analogue of the method from the Servo library 60 | Void attach (uint8_t pin);// analogue of the method from the Servo library 61 | VOID Attach (Uint8_T PIN, Int min, int max);// Analogue of the method from the library Servo.min.500, max 2400 62 | VOID Detach ();// analogue of the Detach method from the Servo library 63 | VOID Start ();// attach + allows the work to work 64 | VOID Stop ();// DETACH + prohibits the operation of tick 65 | 66 | Boolean Tick ();// A method that controls the cervical should be interpreted as often as possible. 67 | // Returns True when targetsposition has been achieved. 68 | // has a built -in timer with the period Servo_period 69 | Boolean Tickmanual ();// Method that controls the cervical, without a built -in timer. 70 | // Returns True when the target position is reached 71 | VOID SetSpeed (Int Speed);// Installation of maximum speed (degrees per second) 72 | VOID setaccelle (Float Accel);// Acceleration installation (0.05 - 1.0).With a value of 1 acceleration maximum.0 - disabled 73 | VOID setaccel (Intscel);// Installation of acceleration in degrees/s/s (worker from 1 to ~ 1500).0 - disabled 74 | VOID settarget (int Target);// Installation of the target position in the ISS (500 - 2400) 75 | VOID settargetdeg (Int Target);// Installation of the target position in degrees (0-Max. angle).Depends on Min and Max 76 | VOID Setautodetach (Boolean Set);// VKL/Off of automatic shutdown (DETACH) when the angle is reached.By the silence.ON 77 | VOID setcurrent (Int Target);// Installation of the current position in the ISS (500 - 2400) 78 | VOID setcurrentdeg (Int Target);// Installation of the current position in degrees (0-Max. angle).Depends on Min and Max 79 | VOID setmaxangle (int maxangle);// Installation Max.The angle of the drive 80 | Intcurrent ();// Obtaining the current position in the ISS (500 - 2400) 81 | Intcurrentdeg ();// Obtaining the current position in degrees (0-Max. angle).Depends on Min and Max 82 | Inttarget ();// Obtaining the target position in the ISS (500 - 2400) 83 | int Gettargetdeg ();// Obtaining a target position in degrees (0-Max. Corner).Depends on Min and Max 84 | VOID Smoothstart ();// Call immediately after Attach (pin, target).Surveys the movement of the Servo from an unknown position to the starting.Blocking for 1 second! 85 | `` ` 86 | 87 | 88 | ## Example 89 | The rest of the examples look at ** Examples **! 90 | `` `CPP 91 | /* 92 | This code smoothly controls one servant (on pin 2) 93 | Using a potentiometer (on pin A0) 94 | Documentation: https://alexgyver.ru/servosmooth/ 95 | */ 96 | 97 | #include 125 | ## versions 126 | - V1.1 - automatic shutdown (DETACH) when reaching the goal 127 | - V1.2 - ON/Off Auto Circulation SERVO 128 | - V1.3 - A separate method for installing and reading the current position.Added version of the Attach method 129 | - V1.4 - Additional compatibility 130 | - V1.5 - Fixed Getcurrent and Getcurrentdeg 131 | - V1.6 - initialization is slightly optimized 132 | - V1.7 - Fixed a bug with low speed/acceleration, the code is optimized 133 | - V1.8 - Improved stability 134 | - V1.9 - Added Max Settings.corner of the servo 135 | - V1.10 - Bag is fixed when the current position coincides with the position of Target 136 | 137 | - v2.0 - algorithm is simplified 138 | - v2.1 - added a change 139 | - V2.2 - Fix of movement in inversion (thanks Vicler) and Write functions (thanks to cheedima) 140 | 141 | - V3.0 142 | - Added the floorCranberries new, more smooth algorithm 143 | - The garbage is cleaned 144 | - Added support PCA9685 145 | - "smoothness" is taken to the basic class to simplify the addition of support for new libraries servo 146 | 147 | - V3.1 - the algorithm is optimized and lightweight, the speed is set in degrees/s 148 | - V3.2 - A bug with a sharp turn at the first Tick was fixed, added SMOOTHSTART 149 | - v3.3 - a bug that arises if not caused tick is fixed 150 | - v3.4 - When setting acceleration in 0, a profile of constant speed is activated 151 | 152 | - V3.5 153 | - Improved/fixed work stop 154 | - corrected errors with rounding 155 | - Fixed a problem with a slow change in Target 156 | 157 | - v3.6 - small bugs are fixed, debag was cut out with 3.5 158 | - v3.7 - Added the task of acceleration in degrees/s/s/s 159 | - V3.8 - Fixed TIKA non -return at Autodetach (FALSE) 160 | - V3.9 - ESP32 support 161 | 162 | 163 | ## bugs and feedback 164 | Create ** Issue ** when you find the bugs, and better immediately write to the mail [alex@alexgyver.ru] (mailto: alex@alexgyver.ru) 165 | The library is open for refinement and your ** pull Request ** 'ow! 166 | 167 | 168 | When reporting about bugs or incorrect work of the library, it is necessary to indicate: 169 | - The version of the library 170 | - What is MK used 171 | - SDK version (for ESP) 172 | - version of Arduino ide 173 | - whether the built -in examples work correctly, in which the functions and designs are used, leading to a bug in your code 174 | - what code has been loaded, what work was expected from it and how it works in reality 175 | - Ideally, attach the minimum code in which the bug is observed.Not a canvas of a thousand lines, but a minimum code -------------------------------------------------------------------------------- /examples/SmoothSweep/SmoothSweep.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный код плавно двигает туда-сюда одной сервой (на пине 5) 3 | Документация: https://alexgyver.ru/servosmooth/ 4 | */ 5 | 6 | #include 7 | ServoSmooth servo; 8 | 9 | uint32_t tmr; 10 | boolean flag; 11 | 12 | void setup() { 13 | Serial.begin(9600); 14 | servo.attach(5); // подключить 15 | servo.setSpeed(130); // ограничить скорость 16 | servo.setAccel(0.1); // установить ускорение (разгон и торможение) 17 | } 18 | 19 | void loop() { 20 | servo.tick(); 21 | 22 | if (millis() - tmr >= 3000) { // каждые 3 сек 23 | tmr = millis(); 24 | flag = !flag; 25 | servo.setTargetDeg(flag ? 50 : 120); // двигаем на углы 50 и 120 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /examples/SmoothSweepDriver/SmoothSweepDriver.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный код плавно двигает туда-сюда одной сервой 3 | Используется драйвер PCA9685 4 | Документация: https://alexgyver.ru/servosmooth/ 5 | */ 6 | 7 | #include 8 | ServoDriverSmooth servo; 9 | //ServoDriverSmooth servo(0x40); // с указанием адреса драйвера 10 | //ServoDriverSmooth servo(0x40, 270); // с указанием адреса и макс. угла 11 | 12 | uint32_t tmr; 13 | boolean flag; 14 | 15 | void setup() { 16 | Serial.begin(9600); 17 | servo.begin(); 18 | Serial.println("setup"); 19 | servo.attach(0); // подключить 20 | servo.setSpeed(130); // ограничить скорость 21 | servo.setAccel(0.3); // установить ускорение (разгон и торможение) 22 | Serial.println("end setup"); 23 | } 24 | 25 | void loop() { 26 | servo.tick(); 27 | 28 | if (millis() - tmr >= 3000) { // каждые 3 сек 29 | tmr = millis(); 30 | flag = !flag; 31 | servo.setTargetDeg(flag ? 50 : 120); // двигаем на углы 50 и 120 32 | } 33 | } -------------------------------------------------------------------------------- /examples/SmoothSweepTest/SmoothSweepTest.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный код плавно двигает туда-сюда одной сервой (на пине 5) 3 | Наблюдаем за графиком угла в плоттере (Инструменты/Плоттер по последовательному соединения) 4 | Документация: https://alexgyver.ru/servosmooth/ 5 | */ 6 | 7 | #include 8 | ServoSmooth servo; 9 | 10 | uint32_t tmr; 11 | boolean flag; 12 | 13 | void setup() { 14 | Serial.begin(9600); 15 | servo.attach(5); // подключить 16 | servo.setSpeed(130); // ограничить скорость 17 | servo.setAccel(0.3); // установить ускорение (разгон и торможение) 18 | servo.setTargetDeg(100); 19 | //servo.setDirection(REVERSE); 20 | servo.write(180); 21 | } 22 | 23 | void loop() { 24 | Serial.print(servo.getCurrentDeg()); 25 | Serial.print(' '); 26 | Serial.print(servo.getTargetDeg()); 27 | Serial.print(' '); 28 | Serial.println(servo.tick() * 10); // состояние серво третьим графиком 29 | delay(10); 30 | 31 | 32 | static uint32_t timer; 33 | if (millis() - timer > 3000) { 34 | static bool kek = false; 35 | kek = !kek; 36 | timer = millis(); 37 | servo.setTargetDeg(kek ? 10 : 50); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /examples/manyServos/manyServos.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный скетч крутит 4 сервопривода с разными скоростями и ускорениями 3 | Документация: https://alexgyver.ru/servosmooth/ 4 | */ 5 | 6 | #define AMOUNT 4 // кол-во серво 7 | 8 | #include 9 | ServoSmooth servos[AMOUNT]; 10 | 11 | uint32_t servoTimer; 12 | uint32_t turnTimer; 13 | 14 | int position1 = 10; // первое положение серв 15 | int position2 = 160; // второе положение серв 16 | boolean flag; 17 | 18 | void setup() { 19 | Serial.begin(9600); 20 | // подключаем 21 | servos[0].attach(2); 22 | servos[1].attach(3); 23 | servos[2].attach(4); 24 | servos[3].attach(5); 25 | 26 | // настраиваем макс. скорости и ускорения 27 | // скор. по умолч. 100 град/с 28 | // ускорение по умолч. 0.2 29 | servos[0].setSpeed(180); 30 | servos[1].setAccel(0.1); 31 | servos[2].setSpeed(90); 32 | servos[3].setAccel(0.5); 33 | } 34 | 35 | void loop() { 36 | // каждые 20 мс 37 | if (millis() - servoTimer >= 20) { // взводим таймер на 20 мс (как в библиотеке) 38 | servoTimer += 20; 39 | for (byte i = 0; i < AMOUNT; i++) { 40 | servos[i].tickManual(); // двигаем все сервы. Такой вариант эффективнее отдельных тиков 41 | } 42 | } 43 | 44 | // каждые 2 секунды меняем положение 45 | if (millis() - turnTimer >= 2000) { 46 | turnTimer = millis(); 47 | flag = !flag; 48 | for (byte i = 0; i < AMOUNT; i++) { 49 | if (flag) servos[i].setTargetDeg(position1); 50 | else servos[i].setTargetDeg(position2); 51 | } 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /examples/potControl/potControl.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный код плавно управляет одной сервой (на пине 2) 3 | при помощи потенциометра (на пине А0) 4 | Документация: https://alexgyver.ru/servosmooth/ 5 | */ 6 | 7 | #include 8 | ServoSmooth servo; 9 | 10 | void setup() { 11 | Serial.begin(9600); 12 | servo.attach(2, 600, 2400); // 600 и 2400 - длины импульсов, при которых 13 | // серво поворачивается максимально в одну и другую сторону, зависят от самой серво 14 | // и обычно даже указываются продавцом. Мы их тут указываем для того, чтобы 15 | // метод setTargetDeg() корректно отрабатывал полный диапазон поворота сервы 16 | 17 | servo.setSpeed(50); // ограничить скорость 18 | servo.setAccel(0.3); // установить ускорение (разгон и торможение) 19 | 20 | servo.setAutoDetach(false); // отключить автоотключение (detach) при достижении целевого угла (по умолчанию включено) 21 | } 22 | 23 | void loop() { 24 | // желаемая позиция задаётся методом setTarget (импульс) или setTargetDeg (угол), далее 25 | // при вызове tick() производится автоматическое движение сервы 26 | // с заданным ускорением и ограничением скорости 27 | servo.tick(); // здесь происходит движение серво по встроенному таймеру! 28 | 29 | int newPos = map(analogRead(0), 0, 1023, 0, 180); // берём с потенцометра значение 0-180 30 | servo.setTargetDeg(newPos); // и отправляем на серво 31 | } 32 | -------------------------------------------------------------------------------- /examples/potControlPlot/potControlPlot.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный код плавно управляет одной сервой (на пине 2) 3 | при помощи потенциометра (на пине А0). 4 | Откройте порт по последовательному соединению для наблюдения за положением серво 5 | Документация: https://alexgyver.ru/servosmooth/ 6 | */ 7 | 8 | #include 9 | ServoSmooth servo; 10 | 11 | uint32_t myTimer; 12 | 13 | void setup() { 14 | Serial.begin(9600); 15 | servo.attach(A1, 600, 2400); // 600 и 2400 - длины импульсов, при которых 16 | // серво поворачивается максимально в одну и другую сторону, зависят от самой серво 17 | // и обычно даже указываются продавцом. Мы их тут указываем для того, чтобы 18 | // метод setTargetDeg() корректно отрабатывал диапазон поворота сервы 19 | 20 | servo.setSpeed(90); // ограничить скорость 21 | servo.setAccel(0.2); // установить ускорение (разгон и торможение) 22 | } 23 | 24 | void loop() { 25 | // желаемая позиция задаётся методом setTarget (импульс) или setTargetDeg (угол), далее 26 | // при вызове tick() производится автоматическое движение сервы 27 | // с заданным ускорением и ограничением скорости 28 | boolean state = servo.tick(); // здесь происходит движение серво по встроенному таймеру! 29 | 30 | 31 | if (millis() - myTimer >= 40) { 32 | myTimer = millis(); 33 | int newPos = map(analogRead(A2), 0, 1023, 500, 2400); // берём с потенцометра значение 500-2400 (импульс) 34 | servo.setTarget(newPos); // и отправляем на серво 35 | Serial.println(String(newPos) + " " + String(servo.getCurrent())/* + " " + String(state)*/); 36 | // state показывает сотояние сервы (0 - движется, 1 - приехали и отключились) 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /examples/potControlPlotDriver/potControlPlotDriver.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Данный код плавно управляет одной сервой 3 | при помощи потенциометра (на пине А0). 4 | Используется драйвер PCA9685 5 | Откройте порт по последовательному соединению для наблюдения за положением серво 6 | Документация: https://alexgyver.ru/servosmooth/ 7 | */ 8 | 9 | #include 10 | ServoDriverSmooth servo; 11 | //ServoDriverSmooth servo(0x40); // с указанием адреса драйвера 12 | //ServoDriverSmooth servo(0x40, 270); // с указанием адреса и макс. угла 13 | 14 | uint32_t myTimer; 15 | 16 | void setup() { 17 | Serial.begin(9600); 18 | servo.attach(0, 150, 550); // 150 и 600 - длины импульсов, при которых 19 | // серво поворачивается максимально в одну и другую сторону, зависят от самой серво 20 | // и обычно даже указываются продавцом. Мы их тут указываем для того, чтобы 21 | // метод setTargetDeg() корректно отрабатывал диапазон поворота сервы 22 | // для драйвера диапазон в районе 150-600! Не как у обычной серво 23 | 24 | servo.setSpeed(120); // ограничить скорость 25 | servo.setAccel(0.1); // установить ускорение (разгон и торможение) 26 | } 27 | 28 | void loop() { 29 | // желаемая позиция задаётся методом setTarget (импульс) или setTargetDeg (угол), далее 30 | // при вызове tick() производится автоматическое движение сервы 31 | // с заданным ускорением и ограничением скорости 32 | boolean state = servo.tick(); // здесь происходит движение серво по встроенному таймеру! 33 | 34 | 35 | if (millis() - myTimer >= 40) { 36 | myTimer = millis(); 37 | int newPos = map(analogRead(0), 0, 1023, 0, 180); // берём с потенцометра значение 0-180 38 | servo.setTargetDeg(newPos); // и отправляем на серво 39 | Serial.println(String(newPos) + " " + String(servo.getCurrentDeg())/* + " " + String(state)*/); 40 | // state показывает сотояние сервы (0 - движется, 1 - приехали и отключились) 41 | } 42 | } -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map ServoSmooth 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ServoSmooth KEYWORD1 10 | ServoDriverSmooth KEYWORD1 11 | 12 | ####################################### 13 | # Methods and Functions (KEYWORD2) 14 | ####################################### 15 | write KEYWORD2 16 | writeMicroseconds KEYWORD2 17 | attach KEYWORD2 18 | detach KEYWORD2 19 | start KEYWORD2 20 | stop KEYWORD2 21 | tick KEYWORD2 22 | tickManual KEYWORD2 23 | setSpeed KEYWORD2 24 | setAccel KEYWORD2 25 | setTarget KEYWORD2 26 | setTargetDeg KEYWORD2 27 | setCurrent KEYWORD2 28 | setCurrentDeg KEYWORD2 29 | getCurrent KEYWORD2 30 | getCurrentDeg KEYWORD2 31 | getTarget KEYWORD2 32 | getTargetDeg KEYWORD2 33 | setAutoDetach KEYWORD2 34 | setMaxAngle KEYWORD2 35 | setDirection KEYWORD2 36 | smoothStart KEYWORD2 37 | 38 | ####################################### 39 | # Constants (LITERAL1) 40 | ####################################### 41 | NORMAL LITERAL1 42 | REVERSE LITERAL1 43 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=ServoSmooth 2 | version=3.9 3 | author=AlexGyver 4 | maintainer=AlexGyver 5 | sentence=Library for smooth control of servo motor 6 | paragraph=Library for smooth control of servo motor 7 | category=Device Control 8 | url=https://github.com/GyverLibs/ServoSmooth 9 | architectures=* -------------------------------------------------------------------------------- /src/Adafruit_PWMServoDriver.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************** 2 | This is a library for our Adafruit 16-channel PWM & Servo driver 3 | 4 | Pick one up today in the adafruit shop! 5 | ------> http://www.adafruit.com/products/815 6 | 7 | These displays use I2C to communicate, 2 pins are required to 8 | interface. 9 | 10 | Adafruit invests time and resources providing this open source code, 11 | please support Adafruit and open-source hardware by purchasing 12 | products from Adafruit! 13 | 14 | Written by Limor Fried/Ladyada for Adafruit Industries. 15 | BSD license, all text above must be included in any redistribution 16 | ****************************************************/ 17 | 18 | #include "Adafruit_PWMServoDriver.h" 19 | #include 20 | 21 | // Set to true to print some debug messages, or false to disable them. 22 | //#define ENABLE_DEBUG_OUTPUT 23 | 24 | 25 | /**************************************************************************/ 26 | /*! 27 | @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on the Wire interface. On Due we use Wire1 since its the interface on the 'default' I2C pins. 28 | @param addr The 7-bit I2C address to locate this chip, default is 0x40 29 | */ 30 | /**************************************************************************/ 31 | Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(uint8_t addr) { 32 | _i2caddr = addr; 33 | 34 | #if defined(ARDUINO_SAM_DUE) 35 | _i2c = &Wire1; 36 | #else 37 | _i2c = &Wire; 38 | #endif 39 | } 40 | 41 | /**************************************************************************/ 42 | /*! 43 | @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on a TwoWire interface 44 | @param i2c A pointer to a 'Wire' compatible object that we'll use to communicate with 45 | @param addr The 7-bit I2C address to locate this chip, default is 0x40 46 | */ 47 | /**************************************************************************/ 48 | Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(TwoWire *i2c, uint8_t addr) { 49 | _i2c = i2c; 50 | _i2caddr = addr; 51 | } 52 | 53 | /**************************************************************************/ 54 | /*! 55 | @brief Setups the I2C interface and hardware 56 | */ 57 | /**************************************************************************/ 58 | void Adafruit_PWMServoDriver::begin(void) { 59 | _i2c->begin(); 60 | reset(); 61 | // set a default frequency 62 | setPWMFreq(1000); 63 | } 64 | 65 | 66 | /**************************************************************************/ 67 | /*! 68 | @brief Sends a reset command to the PCA9685 chip over I2C 69 | */ 70 | /**************************************************************************/ 71 | void Adafruit_PWMServoDriver::reset(void) { 72 | write8(PCA9685_MODE1, 0x80); 73 | delay(10); 74 | } 75 | 76 | /**************************************************************************/ 77 | /*! 78 | @brief Sets the PWM frequency for the entire chip, up to ~1.6 KHz 79 | @param freq Floating point frequency that we will attempt to match 80 | */ 81 | /**************************************************************************/ 82 | void Adafruit_PWMServoDriver::setPWMFreq(float freq) { 83 | #ifdef ENABLE_DEBUG_OUTPUT 84 | Serial.print("Attempting to set freq "); 85 | Serial.println(freq); 86 | #endif 87 | 88 | freq *= 0.9; // Correct for overshoot in the frequency setting (see issue #11). 89 | float prescaleval = 25000000; 90 | prescaleval /= 4096; 91 | prescaleval /= freq; 92 | prescaleval -= 1; 93 | 94 | #ifdef ENABLE_DEBUG_OUTPUT 95 | Serial.print("Estimated pre-scale: "); Serial.println(prescaleval); 96 | #endif 97 | 98 | uint8_t prescale = floor(prescaleval + 0.5); 99 | #ifdef ENABLE_DEBUG_OUTPUT 100 | Serial.print("Final pre-scale: "); Serial.println(prescale); 101 | #endif 102 | 103 | uint8_t oldmode = read8(PCA9685_MODE1); 104 | uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep 105 | write8(PCA9685_MODE1, newmode); // go to sleep 106 | write8(PCA9685_PRESCALE, prescale); // set the prescaler 107 | write8(PCA9685_MODE1, oldmode); 108 | delay(5); 109 | write8(PCA9685_MODE1, oldmode | 0xa0); // This sets the MODE1 register to turn on auto increment. 110 | 111 | #ifdef ENABLE_DEBUG_OUTPUT 112 | Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX); 113 | #endif 114 | } 115 | 116 | /**************************************************************************/ 117 | /*! 118 | @brief Sets the PWM output of one of the PCA9685 pins 119 | @param num One of the PWM output pins, from 0 to 15 120 | @param on At what point in the 4096-part cycle to turn the PWM output ON 121 | @param off At what point in the 4096-part cycle to turn the PWM output OFF 122 | */ 123 | /**************************************************************************/ 124 | void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) { 125 | #ifdef ENABLE_DEBUG_OUTPUT 126 | Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); Serial.print(on); Serial.print("->"); Serial.println(off); 127 | #endif 128 | 129 | _i2c->beginTransmission(_i2caddr); 130 | _i2c->write(LED0_ON_L+4*num); 131 | _i2c->write(on); 132 | _i2c->write(on>>8); 133 | _i2c->write(off); 134 | _i2c->write(off>>8); 135 | _i2c->endTransmission(); 136 | } 137 | 138 | /**************************************************************************/ 139 | /*! 140 | @brief Helper to set pin PWM output. Sets pin without having to deal with on/off tick placement and properly handles a zero value as completely off and 4095 as completely on. Optional invert parameter supports inverting the pulse for sinking to ground. 141 | @param num One of the PWM output pins, from 0 to 15 142 | @param val The number of ticks out of 4096 to be active, should be a value from 0 to 4095 inclusive. 143 | @param invert If true, inverts the output, defaults to 'false' 144 | */ 145 | /**************************************************************************/ 146 | void Adafruit_PWMServoDriver::setPin(uint8_t num, uint16_t val, bool invert) 147 | { 148 | // Clamp value between 0 and 4095 inclusive. 149 | val = min(val, (uint16_t)4095); 150 | if (invert) { 151 | if (val == 0) { 152 | // Special value for signal fully on. 153 | setPWM(num, 4096, 0); 154 | } 155 | else if (val == 4095) { 156 | // Special value for signal fully off. 157 | setPWM(num, 0, 4096); 158 | } 159 | else { 160 | setPWM(num, 0, 4095-val); 161 | } 162 | } 163 | else { 164 | if (val == 4095) { 165 | // Special value for signal fully on. 166 | setPWM(num, 4096, 0); 167 | } 168 | else if (val == 0) { 169 | // Special value for signal fully off. 170 | setPWM(num, 0, 4096); 171 | } 172 | else { 173 | setPWM(num, 0, val); 174 | } 175 | } 176 | } 177 | 178 | /*******************************************************************************************/ 179 | 180 | uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) { 181 | _i2c->beginTransmission(_i2caddr); 182 | _i2c->write(addr); 183 | _i2c->endTransmission(); 184 | 185 | _i2c->requestFrom((uint8_t)_i2caddr, (uint8_t)1); 186 | return _i2c->read(); 187 | } 188 | 189 | void Adafruit_PWMServoDriver::write8(uint8_t addr, uint8_t d) { 190 | _i2c->beginTransmission(_i2caddr); 191 | _i2c->write(addr); 192 | _i2c->write(d); 193 | _i2c->endTransmission(); 194 | } 195 | -------------------------------------------------------------------------------- /src/Adafruit_PWMServoDriver.h: -------------------------------------------------------------------------------- 1 | /*************************************************** 2 | This is a library for our Adafruit 16-channel PWM & Servo driver 3 | 4 | Pick one up today in the adafruit shop! 5 | ------> http://www.adafruit.com/products/815 6 | 7 | These displays use I2C to communicate, 2 pins are required to 8 | interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4 9 | 10 | Adafruit invests time and resources providing this open source code, 11 | please support Adafruit and open-source hardware by purchasing 12 | products from Adafruit! 13 | 14 | Written by Limor Fried/Ladyada for Adafruit Industries. 15 | BSD license, all text above must be included in any redistribution 16 | ****************************************************/ 17 | 18 | #ifndef _ADAFRUIT_PWMServoDriver_H 19 | #define _ADAFRUIT_PWMServoDriver_H 20 | 21 | #if ARDUINO >= 100 22 | #include 23 | #else 24 | #include 25 | #endif 26 | #include 27 | 28 | #define PCA9685_SUBADR1 0x2 29 | #define PCA9685_SUBADR2 0x3 30 | #define PCA9685_SUBADR3 0x4 31 | 32 | #define PCA9685_MODE1 0x0 33 | #define PCA9685_PRESCALE 0xFE 34 | 35 | #define LED0_ON_L 0x6 36 | #define LED0_ON_H 0x7 37 | #define LED0_OFF_L 0x8 38 | #define LED0_OFF_H 0x9 39 | 40 | #define ALLLED_ON_L 0xFA 41 | #define ALLLED_ON_H 0xFB 42 | #define ALLLED_OFF_L 0xFC 43 | #define ALLLED_OFF_H 0xFD 44 | 45 | /**************************************************************************/ 46 | /*! 47 | @brief Class that stores state and functions for interacting with PCA9685 PWM chip 48 | */ 49 | /**************************************************************************/ 50 | class Adafruit_PWMServoDriver { 51 | public: 52 | Adafruit_PWMServoDriver(uint8_t addr = 0x40); 53 | Adafruit_PWMServoDriver(TwoWire *I2C, uint8_t addr = 0x40); 54 | void begin(void); 55 | void reset(void); 56 | void setPWMFreq(float freq); 57 | void setPWM(uint8_t num, uint16_t on, uint16_t off); 58 | void setPin(uint8_t num, uint16_t val, bool invert=false); 59 | uint8_t _i2caddr; 60 | 61 | private: 62 | TwoWire *_i2c; 63 | 64 | uint8_t read8(uint8_t addr); 65 | void write8(uint8_t addr, uint8_t d); 66 | }; 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /src/ServoDriverSmooth.cpp: -------------------------------------------------------------------------------- 1 | #include "ServoDriverSmooth.h" 2 | 3 | bool ServoDriverSmooth::_startFlag = false; 4 | 5 | ServoDriverSmooth::ServoDriverSmooth(uint8_t addr, int maxAngle) { 6 | _maxAngle = maxAngle; 7 | _servo._i2caddr = addr; 8 | _min = 150; 9 | _max = 600; 10 | SS_DEADZONE = _DRIVER_DEADZONE; 11 | SS_DEADZONE_SP = _DRIVER_DEADZONE_SP; 12 | } 13 | 14 | void ServoDriverSmooth::sendToDriver(uint16_t val) { 15 | _servo.setPWM(_pin, 0, val); 16 | } 17 | 18 | void ServoDriverSmooth::attach(int pin) { 19 | _pin = pin; 20 | if (!_startFlag) { 21 | _startFlag = true; 22 | _servo.begin(); 23 | _servo.setPWMFreq(60); 24 | } 25 | } 26 | 27 | void ServoDriverSmooth::detach() { 28 | _servo.setPWM(_pin, 0, 4096); 29 | } -------------------------------------------------------------------------------- /src/ServoDriverSmooth.h: -------------------------------------------------------------------------------- 1 | #ifndef ServoDriverSmooth_h 2 | #define ServoDriverSmooth_h 3 | #include 4 | #include "Adafruit_PWMServoDriver.h" 5 | #include "smoothUtil.h" 6 | #include 7 | 8 | #define _DRIVER_DEADZONE 5 // мёртвая зона (по микросекундам) 9 | #define _DRIVER_DEADZONE_SP 1 // мёртвая зона (по скорости) 10 | 11 | class ServoDriverSmooth : public Smooth { 12 | public: 13 | ServoDriverSmooth(uint8_t addr = 0x40, int maxAngle = 180); 14 | using Smooth::attach; 15 | void attach(int pin); 16 | void detach(); 17 | void sendToDriver(uint16_t val); 18 | Adafruit_PWMServoDriver _servo = Adafruit_PWMServoDriver(); 19 | 20 | private: 21 | static bool _startFlag; // общий флаг для всех членов 22 | }; 23 | #endif -------------------------------------------------------------------------------- /src/ServoSmooth.cpp: -------------------------------------------------------------------------------- 1 | #include "ServoSmooth.h" 2 | 3 | ServoSmooth::ServoSmooth(int maxAngle) { 4 | _maxAngle = maxAngle; 5 | SS_DEADZONE = _SERVO_DEADZONE; 6 | SS_DEADZONE_SP = _SERVO_DEADZONE_SP; 7 | } 8 | 9 | void ServoSmooth::sendToDriver(uint16_t val) { 10 | _servo.writeMicroseconds(val); 11 | } 12 | 13 | void ServoSmooth::attach(int pin) { 14 | _pin = pin; 15 | _servo.attach(_pin); 16 | } 17 | 18 | void ServoSmooth::detach() { 19 | _servo.detach(); 20 | } -------------------------------------------------------------------------------- /src/ServoSmooth.h: -------------------------------------------------------------------------------- 1 | /* 2 | ServoSmooth - библиотека для плавного управления сервоприводами 3 | Документация: https://alexgyver.ru/servosmooth/ 4 | GitHub: https://github.com/GyverLibs/ServoSmooth 5 | Возможности: 6 | - Дополнение к стандартной библиотеке Servo 7 | - Настройка максимальной скорости сервопривода 8 | - Настройка ускорения (разгон и торможение) сервопривода 9 | - Неблокирующая работа 10 | - Трапецеидальный профиль скорости 11 | - При использовании ESC и БК мотора получаем "плавный пуск" мотора 12 | - Устанвока целевой позиции серво по углу (0-180) и длине импульса (500-2400) 13 | - Автоматическое отключение (detach) при достижении цели 14 | - Плавный пуск при подключении серво 15 | - Поддержка расширителя PCA9685 16 | 17 | AlexGyver, alex@alexgyver.ru 18 | https://alexgyver.ru/ 19 | MIT License 20 | 21 | Версии: 22 | v1.1 - автоматическое отключение (detach) при достижении цели 23 | v1.2 - вкл/выкл автоотключения серво 24 | v1.3 - отдельный метод для установки и чтения текущего положения. Добавлен вариант метода attach 25 | v1.4 - улучшена совместимость 26 | v1.5 - исправлены getCurrent и getCurrentDeg 27 | v1.6 - чуть оптимизирована инициализация 28 | v1.7 - исправлен баг с низкой скоростью/ускорением, код оптимизирован 29 | v1.8 - улучшена стабильность 30 | v1.9 - добавлена настройка макс. угла серво 31 | v1.10 - исправлен баг когда текущая позиция совпадает с позицией таргета 32 | 33 | v2.0 - упрощён алгоритм 34 | v2.1 - добавлена смена направления 35 | v2.2 - фикс движения в инверсии (спасибо VICLER) и функций write (спасибо CheDima) 36 | 37 | v3.0 38 | - Добавлен полностью новый, более плавный алгоритм 39 | - Почищен мусор 40 | - Добавлена поддержка PCA9685 41 | - "Плавность" вынесена в базовый класс для упрощения добавления поддержки новых библиотек серво 42 | 43 | v3.1 - оптимизирован и облегчён алгоритм, скорость задаётся в градусах/сек 44 | v3.2 - исправлен баг с резким поворотом при первом tick, добавлена smoothStart 45 | v3.3 - исправлен баг, возникающий если не вызывать tick 46 | v3.4 - при установке ускорения в 0 активируется профиль постоянной скорости 47 | 48 | v3.5 49 | - Улучшена/исправлена работа stop 50 | - Поправлены ошибки с округлением 51 | - Исправлена проблема с медленным изменением target 52 | 53 | v3.6 - Исправлены мелкие баги, вырезан дебаг с 3.5 54 | v3.7 - Добавлено задание ускорения в градусах/сек/сек 55 | v3.8 - Исправлен невозврат тика при autoDetach(false) 56 | v3.9 - поддержка ESP32 57 | */ 58 | 59 | #ifndef _ServoSmooth_h 60 | #define _ServoSmooth_h 61 | #include 62 | #if defined(ESP32) 63 | #include 64 | #else 65 | #include 66 | #endif 67 | #include "smoothUtil.h" 68 | 69 | #define _SERVO_DEADZONE 10 // мёртвая зона (по микросекундам) 70 | #define _SERVO_DEADZONE_SP 3 // мёртвая зона (по скорости) 71 | 72 | class ServoSmooth : public Smooth { 73 | public: 74 | ServoSmooth(int maxAngle = 180); 75 | using Smooth::attach; 76 | void attach(int pin); 77 | void detach(); 78 | void sendToDriver(uint16_t val); 79 | Servo _servo; 80 | private: 81 | }; 82 | #endif -------------------------------------------------------------------------------- /src/smoothUtil.cpp: -------------------------------------------------------------------------------- 1 | #include "smoothUtil.h" 2 | 3 | // знак числа 4 | static int _sign(int x) {return ((x) > 0 ? 1 : -1);} 5 | 6 | // ====== WRITE ====== 7 | // отправить на драйвер с учётом направления 8 | void Smooth::writeUs(int val) { 9 | sendToDriver(_dir ? (_max + _min - val) : val); 10 | } 11 | 12 | // жёстко повернуть на угол (опираясь на min и max) 13 | void Smooth::write(uint16_t angle) { 14 | angle = map(angle, 0, _maxAngle, _min, _max); 15 | writeMicroseconds(angle); 16 | } 17 | 18 | // жёстко повернуть на импульс (без ограничений) 19 | void Smooth::writeMicroseconds(uint16_t val) { 20 | writeUs(val); 21 | _servoCurrentPos = val; 22 | _servoTargetPos = val; 23 | } 24 | 25 | // отправить значение на драйвер 26 | void Smooth::sendToDriver(uint16_t val) { 27 | // пустышка, заменить производным 28 | } 29 | 30 | // ====== ATTACH ====== 31 | void Smooth::attach(int pin) { 32 | // пустышка, заменить производным 33 | } 34 | 35 | void Smooth::detach() { 36 | // пустышка, заменить производным 37 | } 38 | 39 | void Smooth::attach() { 40 | attach(_pin); 41 | } 42 | 43 | void Smooth::attach(int pin, int target) { 44 | _pin = pin; 45 | attach(_pin); 46 | if (target <= _maxAngle) write(target); // если в градусах 47 | else writeMicroseconds(target); 48 | } 49 | 50 | void Smooth::attach(int pin, int min, int max, int target) { 51 | _min = min; 52 | _max = max; 53 | attach(pin, target); 54 | } 55 | 56 | void Smooth::start() { 57 | attach(_pin); 58 | _tickFlag = true; 59 | } 60 | 61 | void Smooth::stop() { 62 | detach(); 63 | _tickFlag = false; 64 | _speed = 0; 65 | _lastSpeed = 0; 66 | } 67 | 68 | void Smooth::smoothStart() { 69 | for (byte i = 0; i < 10; i++) { 70 | detach(); 71 | delay(75); 72 | attach(); 73 | writeUs(_servoCurrentPos); 74 | delay(25); 75 | } 76 | } 77 | 78 | // ====== SET ====== 79 | void Smooth::setSpeed(int speed) { 80 | _servoMaxSpeed = (long)speed * _max / _maxAngle; // ~ перевод из градусов в секунду в тики 81 | } 82 | 83 | void Smooth::setAccel(double accel) { 84 | _acceleration = (float)accel * _max * 3; // для совместимости со старыми скетчами (уск. 0.1-1) 85 | } 86 | 87 | void Smooth::setAccel(int accel) { 88 | _acceleration = (long)accel * (_max - _min) / _maxAngle; // напрямую в градусах/сек/сек (перевод в тики) 89 | } 90 | 91 | void Smooth::setTarget(int target) { 92 | _servoTargetPos = target; 93 | } 94 | 95 | void Smooth::setTargetDeg(int target) { 96 | target = constrain(target, 0, _maxAngle); 97 | _servoTargetPos = map(target, 0, _maxAngle, _min, _max); 98 | } 99 | 100 | void Smooth::setCurrent(int target) { 101 | _servoCurrentPos = target; 102 | } 103 | 104 | void Smooth::setCurrentDeg(int target) { 105 | target = constrain(target, 0, _maxAngle); 106 | _servoCurrentPos = map(target, 0, _maxAngle, _min, _max); 107 | } 108 | 109 | int Smooth::getCurrent() { 110 | return round(_servoCurrentPos); 111 | } 112 | int Smooth::getCurrentDeg() { 113 | return (map(round(_servoCurrentPos), _min, _max, 0, _maxAngle)); 114 | } 115 | 116 | int Smooth::getTarget() { 117 | return _servoTargetPos; 118 | } 119 | int Smooth::getTargetDeg() { 120 | return (map(_servoTargetPos, _min, _max, 0, _maxAngle)); 121 | } 122 | 123 | void Smooth::setAutoDetach(boolean set) { 124 | _autoDetach = set; 125 | } 126 | 127 | void Smooth::setMaxAngle(int maxAngle) { 128 | _maxAngle = maxAngle; 129 | } 130 | 131 | void Smooth::setDirection(bool dir) { 132 | _dir = dir; 133 | } 134 | 135 | 136 | // ====== TICK ====== 137 | boolean Smooth::tick() { 138 | if (millis() - _prevServoTime >= SS_SERVO_PERIOD) { 139 | _prevServoTime = millis(); 140 | Smooth::tickManual(); 141 | } 142 | return !_servoState; 143 | } 144 | 145 | boolean Smooth::tickManual() { 146 | if (_tickFlag) { 147 | int err = _servoTargetPos - _servoCurrentPos; 148 | if (abs(err) > SS_DEADZONE && abs(_lastSpeed - _speed) < SS_DEADZONE_SP) { // условие остановки 149 | if (_acceleration != 0) { 150 | bool thisDir = ((float)_speed * _speed / _acceleration / 2.0 >= abs(err)); // пора тормозить 151 | _speed += (float)_acceleration * _delta * (thisDir ? -_sign(_speed) : _sign(err)); 152 | } else { 153 | _speed = err/_delta; 154 | } 155 | _speed = constrain(_speed, -_servoMaxSpeed, _servoMaxSpeed); 156 | _servoCurrentPos += _speed * _delta; 157 | if (!_servoState) { 158 | _servoState = true; 159 | timeoutCounter = 0; 160 | if (_autoDetach) attach(_pin); 161 | } 162 | writeUs(_servoCurrentPos); 163 | } else { 164 | //_servoCurrentPos = _servoTargetPos; 165 | _speed = 0; 166 | if (_servoState) { 167 | writeUs(_servoCurrentPos); 168 | timeoutCounter++; 169 | } 170 | if (timeoutCounter > SS_DEADTIME && _servoState) { 171 | _servoState = false; 172 | if (_autoDetach) detach(); 173 | } 174 | } 175 | _lastSpeed = _speed; 176 | } 177 | return !_servoState; 178 | } 179 | 180 | /* 181 | // алгоритм версии 3.0 182 | if (err != 0) { 183 | float accel = _k; 184 | static float lastSpeed; 185 | if (abs(err) < SS_DEADZONE && abs(lastSpeed - _speed) < 1) { 186 | writeUs(_servoTargetPos); 187 | _servoCurrentPos = _servoTargetPos; 188 | _speed = 0; 189 | lastSpeed = 0; 190 | if (_autoDetach && _servoState) { 191 | _servoState = false; 192 | detach(); 193 | } 194 | return true; 195 | } else { 196 | if (_autoDetach && !_servoState) { 197 | _servoState = true; 198 | attach(_pin); 199 | } 200 | } 201 | if (_servoState) { 202 | if (abs(err) < (float)(_speed * _speed) / (2.0 * _k)) { 203 | err = -err; 204 | accel = (float)(_speed * _speed) / (2.0 * abs(err)); 205 | if (_sign(_speed) == _sign(err)) err = -err; 206 | } 207 | _speed += (float)accel * _sign(err); 208 | _speed = constrain(_speed, -_servoMaxSpeed, _servoMaxSpeed); 209 | _servoCurrentPos += _speed; 210 | lastSpeed = _speed; 211 | writeUs(_servoCurrentPos); 212 | } 213 | return false; 214 | } 215 | */ 216 | /* 217 | // алгоритм до версии 3.0 218 | if (_tickFlag) { 219 | int _newSpeed = _servoTargetPos - _servoCurrentPos; // расчёт скорости 220 | if (_servoState) { 221 | _newSpeed = constrain(_newSpeed, -_servoMaxSpeed, _servoMaxSpeed); // ограничиваем по макс. 222 | _servoCurrentPos += _newSpeed; // получаем новую позицию 223 | _newPos += (float)(_servoCurrentPos - _newPos) * _k; // и фильтруем её 224 | _newPos = constrain(_newPos, _min, _max); // ограничиваем 225 | writeUs((int)_newPos); // отправляем на серво 226 | } 227 | } 228 | if (abs(_servoTargetPos - (int)_newPos) < SS_DEADZONE) { 229 | if (_autoDetach && _servoState) { 230 | _servoCurrentPos = _servoTargetPos; 231 | _servoState = false; 232 | _servo.detach(); 233 | } 234 | return !_servoState; // приехали 235 | } else { 236 | if (_autoDetach && !_servoState) { 237 | _servoState = true; 238 | _servo.attach(_pin); 239 | } 240 | } 241 | return false; 242 | */ -------------------------------------------------------------------------------- /src/smoothUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef _smoothUtil_h 2 | #define _smoothUtil_h 3 | // базовый класс для плавного управления серво 4 | // в дочернем нужно объявить sendToDriver(int), attach(int) и detach() 5 | 6 | #include 7 | 8 | #define SS_SERVO_PERIOD 20 // период работы tick(), мс 9 | #define SS_DEADTIME 10 // количество тиков до detach 10 | #define NORMAL 0 11 | #define REVERSE 1 12 | 13 | class Smooth { 14 | public: 15 | void write(uint16_t angle); // повернуть на угол. Аналог метода из библиотеки Servo 16 | virtual void sendToDriver(uint16_t val); // отправить на серво 17 | void writeMicroseconds(uint16_t angle); // повернуть на импульс. Аналог метода из библиотеки Servo 18 | void attach(); // аттач при известном пине 19 | virtual void attach(int pin); // аналог метода из библиотеки Servo 20 | void attach(int pin, int target); // аттач + установка позиции (в градусах ИЛИ в микросекундах, программа сама поймёт) 21 | void attach(int pin, int min, int max, int target = 0); // аналог метода из библиотеки Servo. min по умолч. 500, max 2400. target - положение (в углах или мкс, на которые серво повернётся при подключении) 22 | virtual void detach(); // аналог метода из библиотеки Servo 23 | void start(); // attach + разрешает работу tick 24 | void stop(); // detach + запрещает работу tick 25 | 26 | bool tick(); // метод, управляющий сервой, должен опрашиваться как можно чаще. 27 | // Возвращает true, когда целевая позиция достигнута. 28 | // Имеет встроенный таймер с периодом SS_SERVO_PERIOD 29 | 30 | bool tickManual(); // метод, управляющий сервой, без встроенного таймера. 31 | // Возвращает true, когда целевая позиция достигнута 32 | 33 | void setSpeed(int speed); // установка максимальной скорости (больше 0), градусов / с 34 | void setAccel(double accel); // установка ускорения (0.1-1), условные величины 35 | void setAccel(int accel); // установка ускорения в градусах/сек/сек (рабочее от 0 до ~1500) 36 | void setTarget(int target); // установка целевой позиции в мкс (~500 - 2400 серво, ~150-600 драйвер PCA9685) 37 | void setTargetDeg(int target); // установка целевой позиции в градусах (0-макс. угол). Зависит от min и max 38 | void setAutoDetach(boolean set); // вкл/выкл автоматического отключения (detach) при достижении угла. По умолч. вкл 39 | void setCurrent(int target); // установка текущей позиции в мкс (~500 - 2400 серво, ~150-600 драйвер PCA9685) 40 | void setCurrentDeg(int target); // установка текущей позиции в градусах (0-макс. угол). Зависит от min и max 41 | void setMaxAngle(int maxAngle); // установка макс. угла привода 42 | int getCurrent(); // получение текущей позиции в мкс (~500 - 2400 серво, ~150-600 драйвер PCA9685) 43 | int getCurrentDeg(); // получение текущей позиции в градусах (0-макс. угол). Зависит от min и max 44 | int getTarget(); // получение целевой позиции в мкс (~500 - 2400 серво, ~150-600 драйвер PCA9685) 45 | int getTargetDeg(); // получение целевой позиции в градусах (0-макс. угол). Зависит от min и max 46 | 47 | void setDirection(bool dir); // смена направления поворота 48 | void smoothStart(); // вызывать сразу после attach(пин, таргет). Смягчает движение серво из неизвестной позиции к стартовой. БЛОКИРУЮЩАЯ НА 1С! 49 | 50 | protected: 51 | void writeUs(int val); 52 | float _speed = 0, _lastSpeed = 0; 53 | byte timeoutCounter = 0; 54 | int _maxAngle = 180; 55 | int _servoCurrentPos = 600; // если не вызван аттач(пин, таргет) 56 | int _servoTargetPos = 600; 57 | int _min = 500; 58 | int _max = 2400; 59 | float _delta = SS_SERVO_PERIOD / 1000.0; 60 | uint32_t _prevServoTime = 0; 61 | int8_t _pin; 62 | int16_t _servoMaxSpeed = 1400; 63 | uint16_t _acceleration = 1000; 64 | bool _tickFlag = true; 65 | bool _servoState = true; 66 | bool _autoDetach = true; 67 | bool _dir = 0; 68 | byte SS_DEADZONE = 10; 69 | byte SS_DEADZONE_SP = 3; 70 | }; 71 | #endif --------------------------------------------------------------------------------