├── .gitattributes ├── .github └── workflows │ └── tg-send.yml ├── LICENSE ├── README.md ├── README_EN.md ├── examples ├── enc_digitalRead │ └── enc_digitalRead.ino ├── enc_interrupt │ └── enc_interrupt.ino ├── motor_demo │ └── motor_demo.ino ├── motor_demo_pos │ └── motor_demo_pos.ino └── motor_demo_speed │ └── motor_demo_speed.ino ├── keywords.txt ├── library.properties └── src ├── AccelMotor.cpp └── AccelMotor.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 AlexGyver 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/AccelMotor.svg?color=brightgreen)](https://github.com/GyverLibs/AccelMotor/releases/latest/download/AccelMotor.zip) 2 | [![PIO](https://badges.registry.platformio.org/packages/gyverlibs/library/AccelMotor.svg)](https://registry.platformio.org/libraries/gyverlibs/AccelMotor) 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/AccelMotor?_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 | # AccelMotor 10 | Библиотека для расширенного управления и стабилизации мотора с энкодером для Arduino 11 | - Наследует все фишки из библиотеки GyverMotor (поддержка разных драйверов и режимов) 12 | - Режим поддержания скорости с обратной связью 13 | - Режим поворота на заданный угол с обратной связью 14 | - Настраиваемые коэффициенты PID регулятора 15 | - Ограничение ускорения и скорости 16 | - Библиотека принимает любой тип обратной связи: энкодер, потенциометр, и т.д. 17 | - Поддержка мотор-редукторов, настройка передаточного отношения энкодера 18 | - Регулятор учитывает "мёртвую зону" мотора 19 | - Все функции работают в градусах и "тиках" энкодера 20 | 21 | ### Совместимость 22 | Совместима со всеми Arduino платформами (используются Arduino-функции) 23 | 24 | ### Документация 25 | К библиотеке есть [расширенная документация](https://alexgyver.ru/accelmotor/) 26 | 27 | ## Содержание 28 | - [Установка](#install) 29 | - [Инициализация](#init) 30 | - [Использование](#usage) 31 | - [Пример](#example) 32 | - [Версии](#versions) 33 | - [Баги и обратная связь](#feedback) 34 | 35 | 36 | ## Установка 37 | - Библиотеку можно найти по названию **AccelMotor** и установить через менеджер библиотек в: 38 | - Arduino IDE 39 | - Arduino IDE v2 40 | - PlatformIO 41 | - [Скачать библиотеку](https://github.com/GyverLibs/AccelMotor/archive/refs/heads/main.zip) .zip архивом для ручной установки: 42 | - Распаковать и положить в *C:\Program Files (x86)\Arduino\libraries* (Windows x64) 43 | - Распаковать и положить в *C:\Program Files\Arduino\libraries* (Windows x32) 44 | - Распаковать и положить в *Документы/Arduino/libraries/* 45 | - (Arduino IDE) автоматическая установка из .zip: *Скетч/Подключить библиотеку/Добавить .ZIP библиотеку…* и указать скачанный архив 46 | - Читай более подробную инструкцию по установке библиотек [здесь](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) 47 | ### Обновление 48 | - Рекомендую всегда обновлять библиотеку: в новых версиях исправляются ошибки и баги, а также проводится оптимизация и добавляются новые фичи 49 | - Через менеджер библиотек IDE: найти библиотеку как при установке и нажать "Обновить" 50 | - Вручную: **удалить папку со старой версией**, а затем положить на её место новую. "Замену" делать нельзя: иногда в новых версиях удаляются файлы, которые останутся при замене и могут привести к ошибкам! 51 | 52 | 53 | 54 | ## Инициализация 55 | ```cpp 56 | // инициализация наследуется от GyverMotor 57 | // варианты инициализации в зависимости от типа драйвера: 58 | AccelMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, level); 59 | AccelMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, level); 60 | AccelMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, level); 61 | /* 62 | DRIVER2WIRE - двухпроводной драйвер (направление + ШИМ) 63 | DRIVER3WIRE - трёхпроводной драйвер (два пина направления + ШИМ) 64 | RELAY2WIRE - реле в качестве драйвера (два пина направления) 65 | 66 | dig_pin, dig_pin_A, dig_pin_B - любой цифровой пин МК 67 | PWM_pin - любой ШИМ пин МК 68 | level - LOW / HIGH - уровень драйвера. Если при увеличении скорости мотор наоборот тормозит - смени уровень 69 | */ 70 | ``` 71 | 72 | 73 | ## Использование 74 | ```cpp 75 | // управляет мотором. Вызывать как можно чаще (внутри таймер с периодом dt) 76 | // принимает текущее положение вала мотора (по счёту энкодера) 77 | // возвращает true если мотор всё ещё движется к цели 78 | bool tick(long pos); 79 | 80 | // установка передаточного отношения редуктора и энкодера 81 | // пример: если редуктор 1:30 - передаём в функцию 30 82 | // пример: если редуктор 1:30 и энкодер на 12 тиков - передаём 30*12 83 | void setRatio(float ratio); 84 | 85 | // установка периода регулятора (рекомендуется 2-50 миллисекунд) 86 | void setDt(int dt); 87 | 88 | // установка и получение текущей позиции в тиках энкодера и градусах. 89 | // setCurrent(pos) равносильна вызову tick(pos) и в принципе не нужна! 90 | void setCurrent(long pos); 91 | long getCurrent(); 92 | long getCurrentDeg(); 93 | 94 | // установка и получение целевой позиции в тиках энкодера и градусах 95 | void setTarget(long pos); 96 | void setTargetDeg(long pos); 97 | long getTarget(); 98 | long getTargetDeg(); 99 | 100 | // установка максимальной скорости в тиках энкодера/секунду и градусах/секунду 101 | void setMaxSpeed(int speed); 102 | void setMaxSpeedDeg(int speed); 103 | 104 | // установка ускорения тиках энкодера и градусах в секунду 105 | void setAcceleration(int accel); 106 | void setAccelerationDeg(int accel); 107 | 108 | // установка и получение целевой скорости в тиках энкодера/секунду и градусах/секунду 109 | void setTargetSpeed(int speed); 110 | void setTargetSpeedDeg(int speed); 111 | int getTargetSpeed(); 112 | int getTargetSpeedDeg(); 113 | 114 | // получить текущую скорость в тиках энкодера/секунду и градусах/секунду 115 | int getSpeed(); 116 | int getSpeedDeg(); 117 | 118 | // получить текущий ШИМ сигнал (float из ПИД регулятора) 119 | float getDuty(); 120 | 121 | // ручная установка режима работы 122 | // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки 123 | // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу 124 | // PID_POS - tick() работает в режиме резкого следования к целевому углу 125 | // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением) 126 | // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору 127 | void setRunMode(AM_runMode mode); 128 | 129 | // возвращает true, если вал мотора заблокирован, а сигнал подаётся 130 | bool isBlocked(); 131 | 132 | // коэффициенты ПИД регулятора 133 | // пропорциональный - от него зависит агрессивность управления, нужно увеличивать kp 134 | // при увеличении нагрузки на вал, чтобы регулятор подавал больший управляющий ШИМ сигнал 135 | float kp = 2.0; // (знач. по умолчанию) 136 | 137 | // интегральный - позволяет нивелировать ошибку со временем, имеет накопительный эффект 138 | float ki = 0.9; // (знач. по умолчанию) 139 | 140 | // дифференциальный. Позволяет чуть сгладить рывки, но при большом значении 141 | // сам становится причиной рывков и раскачки системы! 142 | float kd = 0.1; // (знач. по умолчанию) 143 | 144 | // установить зону остановки мотора для режима стабилизации позиции (по умолч. 8) 145 | void setStopZone(int zone); 146 | 147 | // установить пределы шагов/градусов, вне которых мотор будет жёстко отключен для безопасности. Если по нулям, ограничения нет (по умолч.) 148 | void setRange(long min, long max); 149 | void setRangeDeg(long min, long max); 150 | 151 | long controlPos = 0; // для отладки 152 | ``` 153 | 154 | 155 | ## Пример 156 | ```cpp 157 | /* 158 | Пример управления мотором при помощи драйвера полного моста и потенциометра 159 | Для режимов следования к позиции и удержания скорости 160 | */ 161 | #include "AccelMotor.h" 162 | AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH); 163 | 164 | void setup() { 165 | Serial.begin(9600); 166 | // использую мотор JGA25 167 | // редуктор 1:21.3 168 | // энкодер 12 тиков на оборот 169 | motor.setRatio(21.3 * 12); 170 | 171 | // период интегрирования (по умолч. 20) 172 | motor.setDt(30); // миллисекунды 173 | 174 | // установка максимальной скорости для режима ACCEL_POS 175 | motor.setMaxSpeedDeg(600); // в градусах/сек 176 | //motor.setMaxSpeed(400); // в тиках/сек 177 | 178 | // установка ускорения для режима ACCEL_POS 179 | motor.setAccelerationDeg(300); // в градусах/сек/сек 180 | //motor.setAcceleration(300); // в тиках 181 | 182 | // минимальный (по модулю) ШИМ сигнал (при котором мотор трогается) 183 | motor.setMinDuty(50); 184 | 185 | // коэффициенты ПИД регулятора 186 | motor.kp = 3; // отвечает за резкость регулирования. 187 | // При малых значениях сигнала вообще не будет, при слишком больших – будет трясти 188 | 189 | motor.ki = 0.2; // отвечает за коррекцию ошибки в течение времени 190 | motor.kd = 0.1; // отвечает за компенсацию резких изменений 191 | 192 | // установить зону остановки мотора для режима стабилизации позиции в тиках (по умолч. 8) 193 | motor.setStopZone(10); 194 | 195 | motor.setRunMode(ACCEL_POS); 196 | // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки 197 | // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу 198 | // PID_POS - tick() работает в режиме резкого следования к целевому углу 199 | // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением) 200 | // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору 201 | } 202 | 203 | void loop() { 204 | // потенциометр на А0 205 | // преобразуем значение в -255.. 255 206 | static float val; 207 | val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор 208 | 209 | // для режима PID_SPEED/ACCEL_SPEED 210 | //motor.setTargetSpeedDeg(val*3); // задаём целевую скорость в градусах/сек 211 | 212 | // для режима PID_POS/ACCEL_POS 213 | motor.setTargetDeg(val * 2); // задаём новый целевой угол в градусах 214 | 215 | // обязательная функция. Делает все вычисления 216 | // принимает текущее значение с энкодера или потенциометра 217 | motor.tick(encTick(4)); 218 | 219 | static uint32_t tmr = 0; 220 | if (millis() - tmr > 100) { // таймер на 100мс для графиков 221 | tmr += 100; 222 | // отладка позиции (открой плоттер) 223 | Serial.print(motor.getTargetDeg()); 224 | Serial.print(','); 225 | Serial.print(motor.getDuty()); 226 | Serial.print(','); 227 | Serial.println(motor.getCurrentDeg()); 228 | 229 | /* 230 | // отладка скорости (открой плоттер) 231 | Serial.print(motor.getTargetSpeedDeg()); 232 | Serial.print(','); 233 | Serial.print(motor.getDuty()); 234 | Serial.print(','); 235 | Serial.println(motor.getSpeedDeg()); 236 | */ 237 | } 238 | } 239 | 240 | // читаем энкодер вручную, через digitalRead() 241 | long encTick(byte pin) { 242 | static bool lastState; 243 | static long encCounter = 0; 244 | bool curState = digitalRead(pin); // опрос 245 | if (lastState != curState) { // словили изменение 246 | lastState = curState; 247 | if (curState) { // по фронту 248 | encCounter += motor.getState(); // запомнили поворот 249 | } 250 | } 251 | return encCounter; 252 | } 253 | ``` 254 | 255 | 256 | ## Версии 257 | - v1.1 - улучшен алгоритм 258 | - v1.2 - совместимость с esp 259 | - v1.3 - небольшие улучшения и фиксы 260 | 261 | 262 | ## Баги и обратная связь 263 | При нахождении багов создавайте **Issue**, а лучше сразу пишите на почту [alex@alexgyver.ru](mailto:alex@alexgyver.ru) 264 | Библиотека открыта для доработки и ваших **Pull Request**'ов! 265 | 266 | 267 | При сообщении о багах или некорректной работе библиотеки нужно обязательно указывать: 268 | - Версия библиотеки 269 | - Какой используется МК 270 | - Версия SDK (для ESP) 271 | - Версия Arduino IDE 272 | - Корректно ли работают ли встроенные примеры, в которых используются функции и конструкции, приводящие к багу в вашем коде 273 | - Какой код загружался, какая работа от него ожидалась и как он работает в реальности 274 | - В идеале приложить минимальный код, в котором наблюдается баг. Не полотно из тысячи строк, а минимальный код 275 | -------------------------------------------------------------------------------- /README_EN.md: -------------------------------------------------------------------------------- 1 | This is an automatic translation, may be incorrect in some places. See sources and examples! 2 | 3 | # Accelmotor 4 | Library for expanded control and stabilization of the engine with encoder for Arduino 5 | - inherits all the chips from the Gyvermotor library (support for different drivers and modes) 6 | - mode of maintaining feedback speed 7 | - turning mode to a given angle with feedback 8 | - custom -made pid regulator coefficients 9 | - restriction of acceleration and speed 10 | - The library accepts any type of feedback: encoder, potentiometer, etc. 11 | - support for motor-tractors, tuning the transfer of encoder 12 | - the regulator takes into account the "dead zone" of the motor 13 | - all functions work in degrees and Tiki Encoder 14 | 15 | ## compatibility 16 | Compatible with all arduino platforms (used arduino functions) 17 | 18 | ### Documentation 19 | There is [extended documentation] to the library (https://alexgyver.ru/accellemotor/) 20 | 21 | ## Content 22 | - [installation] (# Install) 23 | - [initialization] (#init) 24 | - [use] (#usage) 25 | - [Example] (# Example) 26 | - [versions] (#varsions) 27 | - [bugs and feedback] (#fedback) 28 | 29 | 30 | ## Installation 31 | - The library can be found by the name ** Accelmotor ** and installed through the library manager in: 32 | - Arduino ide 33 | - Arduino ide v2 34 | - Platformio 35 | - [download the library] (https://github.com/gyverlibs/accellmotor/archive/refs/heads/main.zip) .Zip archive for manual installation: 36 | - unpack and put in * C: \ Program Files (X86) \ Arduino \ Libraries * (Windows X64) 37 | - unpack and put in * C: \ Program Files \ Arduino \ Libraries * (Windows X32) 38 | - unpack and put in *documents/arduino/libraries/ * 39 | - (Arduino id) Automatic installation from. Zip: * sketch/connect the library/add .Zip library ... * and specify downloaded archive 40 | - 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) 41 | ### Update 42 | - I recommend always updating the library: errors and bugs are corrected in the new versions, as well as optimization and new features are added 43 | - through the IDE library manager: find the library how to install and click "update" 44 | - 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! 45 | 46 | 47 | 48 | ## initialization 49 | `` `CPP 50 | // initialization inherited from gyvermotor 51 | // Prevention options depending on the type of driver: 52 | Accelmotor Motor (Driver2wire, Dig_pin, Pwm_pin, Level); 53 | Accelmotor Motor (Driver3wire, Dig_pin_a, Dig_pin_b, Pwm_pin, Level); 54 | Accelmotor Motor (Relay2wire, Dig_pin_a, Dig_pin_b, Level); 55 | /* 56 | Driver2wire - two -wire driver (direction + ShIM) 57 | Driver3wire - a three -wire driver (two directions + shim) 58 | Relay2wire - Relay as a driver (two pins of the direction) 59 | 60 | dig_pin, dig_pin_a, dig_pin_b - any digital PIN MK 61 | Pwm_pin - any shim pin mk 62 | Level - Low / High - Driver level.If, with an increase in speed, the motor, on the contrary, slows down, change the level 63 | */ 64 | `` ` 65 | 66 | 67 | ## Usage 68 | `` `CPP 69 | // controls the motor.Call as often as possible (inside the timer with a period dt) 70 | // accepts the current position of VALa Motor (according to the encoder) 71 | // Returns True if the motor still moves towards the goal 72 | Bool Tick (Long Pos); 73 | 74 | // Installation of the gear ratio of the gearbox and encoder 75 | // Example: if the gearbox is 1:30 - we transfer to function 30 76 | // Example: if the gearbox is 1:30 and an encoder for 12 ticks - we transfer 30*12 77 | VOID setratio (Float Ratio); 78 | 79 | // Installation of the regulator period (2-50 milliseconds are recommended) 80 | VOID Setdt (Int DT); 81 | 82 | // Installation and receipt of the current position in the tics of encoder and degrees. 83 | // setcurrent (POS) is equivalent to call Tick (POS) and, in principle, is not needed! 84 | VOID setcurrent (LONG POS); 85 | Long getcurrent (); 86 | Long getcurrentdeg (); 87 | 88 | // Installation and receipt of the target position in the tics of encoder and degrees 89 | VOID settarget (LONG POS); 90 | VOID settargetdeg (Long POS); 91 | Long gettarget (); 92 | Long gettargetdeg (); 93 | 94 | // Installation of maximum speed in ticks of encoder/second and degrees/second 95 | VOID SetmaxSpeed (Intsed); 96 | VOID setmaxSpeedDeg (int spEED); 97 | 98 | // Installation of acceleration ticks of encoder and degrees per second 99 | VOID setaccoleration (Intscel); 100 | VOID setaccolerationdeg (intactel); 101 | 102 | // Installation and receipt of targeted speed in the ticks of encoder/second and degrees/second 103 | VOID settargetSpeed (int sponeD); 104 | VOID settargetSpeeddeg 105 | IntargetSpeed (); 106 | IntargetSpeedDeg (); 107 | 108 | // Get the current speed in ticks of encoder/second and degrees/second 109 | Intspeed (); 110 | Intspeeddeg (); 111 | 112 | // Get the current PWM signal (Float from PID of the regulator) 113 | Float Getduuty (); 114 | 115 | // manual installation of the operating mode 116 | // IDLE_RUN - Tick () does not control the motor.Can be used for debugging 117 | // Accel_pos - Tick () works in a smooth follow -up mode to the target corner 118 | // pid_pos - tick () operates in a sharp follow -up mode to the target corner 119 | // Accel_Speed - Tick () works in a smooth maintenance mode (with a given acceleration) 120 | // pid_Speed - Tick () works in the mode of maintaining speed on PID to the regulator 121 | VOID Setrunmode (am_runmode mode); 122 | 123 | // returns True if the shaft of the motor is blocked and the signal is supplied 124 | Bool ISBLOCKED (); 125 | 126 | // Regulator PID coefficients 127 | // proportional - aggressiveness of management depends on it, you need to increase KP 128 | // with an increase in the load on the shaft, so that the regulator supplies a larger control PWM signal 129 | Float KP = 2.0;// (value by default) 130 | 131 | // Integral - allows you to level the error over time, has a cumulative effect 132 | Float Ki = 0.9;// (value by default) 133 | 134 | // differential.Allows you to slightly smooth out jerks, but with great meaning 135 | // itself becomes the cause of jerks and build -up systems! 136 | Float KD = 0.1;// (value by default) 137 | 138 | // Install the zone of stopping the motor for the stabilization regimen (in silence 8) 139 | VOID SetStopzone (Int Zone); 140 | 141 | // set the limits of steps/degrees, outside which the motor will be hard to disable for safety.If by zero, there is no restriction (by the silence) 142 | VOID SETRANGE (LONG MIN, LONG MAX); 143 | VOID SETRANGEDEG (LONG MIN, LONG MAX); 144 | 145 | Long Controlpos = 0;// for debugging 146 | `` ` 147 | 148 | 149 | ## Example 150 | `` `CPP 151 | /* 152 | An example of a motor management using a driver of a full bridge and a potentiometer 153 | For the regimes for the position and maintenance of speed 154 | */ 155 | #include "Accelmotor.h" 156 | Accelmotor Motor (Driver2wire, 2, 3, High); 157 | 158 | VOID setup () { 159 | Serial.Begin (9600); 160 | // I use the jga25 motor 161 | // Reducer 1: 21.3 162 | // Encoder 12 ticks for circulation 163 | Motor.Setratio (21.3 * 12); 164 | 165 | // Integration period (in silence 20) 166 | Motor.Setdt (30);// milliseconds 167 | 168 | // Installation of maximum speed for the accel_pos mode 169 | Motor.SetmaxSpeedDeg (600);// in degrees/s 170 | //motor.SetmaxSpeed(400);// in ticks/sec 171 | 172 | // Installation of acceleration for the accel_pos mode 173 | Motor.Setaccelerationdeg (300);// in degrees/second 174 | //motor.setacceleration(300);// in ticks 175 | 176 | // Minimum (according to the module) shim signal (at which the motor touches) 177 | Motor.Setminduty (50); 178 | 179 | // Regulator PID coefficients 180 | Motor.kp = 3;// is responsible for the sharpness of regulation. 181 | // with small values of the signal, there will be no signal at all, with too large ones - it will shake 182 | 183 | Motor.ki = 0.2;// is responsible for correction of errors in the flowe 184 | Motor.kd = 0.1;// is responsible for compensation for sharp changes 185 | 186 | // Install the zone of stopping the motor for the stabilization mode of the position in ticks (according to the silence 8) 187 | Motor.SetStopzone (10); 188 | 189 | Motor.Setrunmode (Accel_pos); 190 | // IDLE_RUN - Tick () does not control the motor.Can be used for debugging 191 | // Accel_pos - Tick () works in a smooth follow -up mode to the target corner 192 | // pid_pos - tick () operates in a sharp follow -up mode to the target corner 193 | // Accel_Speed - Tick () works in a smooth maintenance mode (with a given acceleration) 194 | // pid_Speed - Tick () works in the mode of maintaining speed on PID to the regulator 195 | } 196 | 197 | VOID loop () { 198 | // Potentiometer on A0 199 | // transform the value of --255 .. 255 200 | Static Float Val; 201 | val += (255 - analogread (0) / 2 - val) * 0.3;// Filter 202 | 203 | // for pid_Speed/acceel_Speed 204 | //motor.SettargetSpeeddeg(val*3);// set targeted speed in degrees/s 205 | 206 | // for PID_POS/Accel_POS mode 207 | Motor.Settargetdeg (val * 2);// set a new target angle in degrees 208 | 209 | // Mandatory function.Makes all calculations 210 | // accepts the current value from the encoder or potentiometer 211 | Motor.tick (Enctick (4)); 212 | 213 | static uint32_t tmr = 0; 214 | if (millis () - tmr> 100) {// Timer for 100ms for graphs 215 | TMR += 100; 216 | // debugging position (open Plotter) 217 | Serial.print (motor.gettargetdeg ()); 218 | Serial.print (','); 219 | Serial.print (motor.getduut ()); 220 | Serial.print (','); 221 | Serial.println (Motor.getcurrentdeg ()); 222 | 223 | /* 224 | // Speed debugging (open Plotter) 225 | Serial.print (Motor.gettargetSpeeddeg ()); 226 | Serial.print (','); 227 | Serial.print (motor.getduut ()); 228 | Serial.print (','); 229 | Serial.println (Motor.getSpeeddeg ()); 230 | */ 231 | } 232 | } 233 | 234 | // We read the encoder manually through DigitalRead () 235 | LONG ENCTICK (Byte PIN) { 236 | Static Bool Laststate; 237 | Static Long Encounter = 0; 238 | Bool Curstate = DigitalRead (PIN);// survey 239 | IfState! = curstate) {// caught the change 240 | Laststate = Curstate; 241 | if (curstate) {// on the front 242 | ENCCOUNTER += MOTOR.GETSTATE ();// remembered the turn 243 | } 244 | } 245 | Return ENCCOUNTER; 246 | } 247 | `` ` 248 | 249 | 250 | ## versions 251 | - V1.1 - the algorithm has been improved 252 | - V1.2 - Compatibility with ESP 253 | - V1.3 - small improvements and fixes 254 | 255 | 256 | ## bugs and feedback 257 | Create ** Issue ** when you find the bugs, and better immediately write to the mail [alex@alexgyver.ru] (mailto: alex@alexgyver.ru) 258 | The library is open for refinement and your ** pull Request ** 'ow! 259 | 260 | 261 | When reporting about bugs or incorrect work of the library, it is necessary to indicate: 262 | - The version of the library 263 | - What is MK used 264 | - SDK version (for ESP) 265 | - version of Arduino ide 266 | - whether the built -in examples work correctly, in which the functions and designs are used, leading to a bug in your code 267 | - what code has been loaded, what work was expected from it and how it works in reality 268 | - Ideally, attach the minimum code in which the bug is observed.Not a canvas of a thousand lines, but a minimum code -------------------------------------------------------------------------------- /examples/enc_digitalRead/enc_digitalRead.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Пример опроса энкодера через digitalRead 3 | */ 4 | #include "AccelMotor.h" 5 | AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH); 6 | // подробнее об инициализации смотри в примере motor_demo 7 | 8 | void setup() { 9 | Serial.begin(9600); 10 | motor.setRunMode(ACCEL_POS); 11 | } 12 | 13 | void loop() { 14 | // потенциометр на А0 15 | // преобразуем значение в -255.. 255 16 | static float val; 17 | val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор 18 | 19 | // для режима PID_SPEED/ACCEL_SPEED 20 | //motor.setTargetSpeedDeg(val*3); // задаём целевую скорость в градусах/сек 21 | 22 | // для режима PID_POS/ACCEL_POS 23 | motor.setTargetDeg(val * 2); // задаём новый целевой угол в градусах 24 | 25 | // обязательная функция. Делает все вычисления 26 | // принимает текущее значение с энкодера или потенциометра 27 | motor.tick(encTick(4)); 28 | } 29 | 30 | // функция опроса энкодера через digitalRead() 31 | long encTick(byte pin) { 32 | static bool lastState; 33 | static long encCounter = 0; 34 | bool curState = digitalRead(pin); // опрос 35 | if (lastState != curState) { // словили изменение 36 | lastState = curState; 37 | if (curState) { // по фронту 38 | encCounter += motor.getState(); // запомнили поворот 39 | // motor.getState() вернёт 1 или -1 в зависимости от направления 40 | } 41 | } 42 | return encCounter; 43 | } 44 | -------------------------------------------------------------------------------- /examples/enc_interrupt/enc_interrupt.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Пример опроса энкодера через аппаратные прерывания 3 | */ 4 | #include "AccelMotor.h" 5 | AccelMotor motor(DRIVER2WIRE, 4, 3, HIGH); 6 | // подробнее об инициализации смотри в примере motor_demo 7 | 8 | void setup() { 9 | Serial.begin(9600); 10 | motor.setRunMode(ACCEL_POS); 11 | attachInterrupt(0, isr, RISING); 12 | } 13 | 14 | volatile long encCounter = 0; 15 | void isr() { 16 | // опрос энкодера 17 | encCounter += motor.getState(); 18 | } 19 | 20 | void loop() { 21 | // потенциометр на А0 22 | // преобразуем значение в -255.. 255 23 | static float val; 24 | val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор 25 | 26 | // для режима PID_SPEED/ACCEL_SPEED 27 | //motor.setTargetSpeedDeg(val*3); // задаём целевую скорость в градусах/сек 28 | 29 | // для режима PID_POS/ACCEL_POS 30 | motor.setTargetDeg(val * 2); // задаём новый целевой угол в градусах 31 | 32 | // обязательная функция. Делает все вычисления 33 | // принимает текущее значение с энкодера или потенциометра 34 | motor.tick(encCounter); 35 | } 36 | -------------------------------------------------------------------------------- /examples/motor_demo/motor_demo.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Пример управления мотором при помощи драйвера полного моста и потенциометра 3 | Для режимов следования к позиции и удержания скорости 4 | */ 5 | #include "AccelMotor.h" 6 | AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH); 7 | 8 | // инициализация наследуется от GyverMotor 9 | // варианты инициализации в зависимости от типа драйвера: 10 | // AccelMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, level) 11 | // AccelMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, level) 12 | // AccelMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, level) 13 | /* 14 | DRIVER2WIRE - двухпроводной драйвер (направление + ШИМ) 15 | DRIVER3WIRE - трёхпроводной драйвер (два пина направления + ШИМ) 16 | RELAY2WIRE - реле в качестве драйвера (два пина направления) 17 | 18 | dig_pin, dig_pin_A, dig_pin_B - любой цифровой пин МК 19 | PWM_pin - любой ШИМ пин МК 20 | level - LOW / HIGH - уровень драйвера. Если при увеличении скорости мотор наоборот тормозит - смени уровень 21 | */ 22 | 23 | void setup() { 24 | Serial.begin(9600); 25 | // использую мотор JGA25 26 | // редуктор 1:21.3 27 | // энкодер 12 тиков на оборот 28 | motor.setRatio(21.3 * 12); 29 | 30 | // период интегрирования (по умолч. 20) 31 | motor.setDt(30); // миллисекунды 32 | 33 | // установка максимальной скорости для режима ACCEL_POS 34 | motor.setMaxSpeedDeg(600); // в градусах/сек 35 | //motor.setMaxSpeed(400); // в тиках/сек 36 | 37 | // установка ускорения для режима ACCEL_POS 38 | motor.setAccelerationDeg(300); // в градусах/сек/сек 39 | //motor.setAcceleration(300); // в тиках 40 | 41 | // минимальный (по модулю) ШИМ сигнал (при котором мотор трогается) 42 | motor.setMinDuty(50); 43 | 44 | // коэффициенты ПИД регулятора 45 | motor.kp = 3; // отвечает за резкость регулирования. 46 | // При малых значениях сигнала вообще не будет, при слишком больших – будет трясти 47 | 48 | motor.ki = 0.2; // отвечает за коррекцию ошибки в течение времени 49 | motor.kd = 0.1; // отвечает за компенсацию резких изменений 50 | 51 | // установить зону остановки мотора для режима стабилизации позиции в тиках (по умолч. 8) 52 | motor.setStopZone(10); 53 | 54 | motor.setRunMode(ACCEL_POS); 55 | // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки 56 | // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу 57 | // PID_POS - tick() работает в режиме резкого следования к целевому углу 58 | // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением) 59 | // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору 60 | } 61 | 62 | void loop() { 63 | // потенциометр на А0 64 | // преобразуем значение в -255.. 255 65 | static float val; 66 | val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор 67 | 68 | // для режима PID_SPEED/ACCEL_SPEED 69 | //motor.setTargetSpeedDeg(val*3); // задаём целевую скорость в градусах/сек 70 | 71 | // для режима PID_POS/ACCEL_POS 72 | motor.setTargetDeg(val * 2); // задаём новый целевой угол в градусах 73 | 74 | // обязательная функция. Делает все вычисления 75 | // принимает текущее значение с энкодера или потенциометра 76 | motor.tick(encTick(4)); 77 | 78 | static uint32_t tmr = 0; 79 | if (millis() - tmr > 100) { // таймер на 100мс для графиков 80 | tmr += 100; 81 | // отладка позиции (открой плоттер) 82 | Serial.print(motor.getTargetDeg()); 83 | Serial.print(','); 84 | Serial.print(motor.getDuty()); 85 | Serial.print(','); 86 | Serial.println(motor.getCurrentDeg()); 87 | 88 | /* 89 | // отладка скорости (открой плоттер) 90 | Serial.print(motor.getTargetSpeedDeg()); 91 | Serial.print(','); 92 | Serial.print(motor.getDuty()); 93 | Serial.print(','); 94 | Serial.println(motor.getSpeedDeg()); 95 | */ 96 | } 97 | } 98 | 99 | // читаем энкодер вручную, через digitalRead() 100 | long encTick(byte pin) { 101 | static bool lastState; 102 | static long encCounter = 0; 103 | bool curState = digitalRead(pin); // опрос 104 | if (lastState != curState) { // словили изменение 105 | lastState = curState; 106 | if (curState) { // по фронту 107 | encCounter += motor.getState(); // запомнили поворот 108 | } 109 | } 110 | return encCounter; 111 | } 112 | -------------------------------------------------------------------------------- /examples/motor_demo_pos/motor_demo_pos.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Пример управления мотором при помощи драйвера полного моста и потенциометра 3 | Для режимов следования к позиции и удержания скорости 4 | */ 5 | #include "AccelMotor.h" 6 | AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH); 7 | 8 | // инициализация наследуется от GyverMotor 9 | // варианты инициализации в зависимости от типа драйвера: 10 | // AccelMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, level) 11 | // AccelMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, level) 12 | // AccelMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, level) 13 | /* 14 | DRIVER2WIRE - двухпроводной драйвер (направление + ШИМ) 15 | DRIVER3WIRE - трёхпроводной драйвер (два пина направления + ШИМ) 16 | RELAY2WIRE - реле в качестве драйвера (два пина направления) 17 | 18 | dig_pin, dig_pin_A, dig_pin_B - любой цифровой пин МК 19 | PWM_pin - любой ШИМ пин МК 20 | level - LOW / HIGH - уровень драйвера. Если при увеличении скорости мотор наоборот тормозит - смени уровень 21 | */ 22 | 23 | void setup() { 24 | Serial.begin(9600); 25 | // использую мотор JGA25 26 | // редуктор 1:21.3 27 | // энкодер 12 тиков на оборот 28 | motor.setRatio(21.3 * 12); 29 | 30 | // период интегрирования (по умолч. 20) 31 | motor.setDt(30); // миллисекунды 32 | 33 | // установка максимальной скорости для режима ACCEL_POS 34 | motor.setMaxSpeedDeg(600); // в градусах/сек 35 | //motor.setMaxSpeed(400); // в тиках/сек 36 | 37 | // установка ускорения для режима ACCEL_POS 38 | motor.setAccelerationDeg(300); // в градусах/сек/сек 39 | //motor.setAcceleration(300); // в тиках 40 | 41 | // минимальный (по модулю) ШИМ сигнал (при котором мотор трогается) 42 | motor.setMinDuty(50); 43 | 44 | // коэффициенты ПИД регулятора 45 | motor.kp = 2; // отвечает за резкость регулирования. 46 | // При малых значениях сигнала вообще не будет, при слишком больших – будет трясти 47 | 48 | motor.ki = 9; // отвечает за коррекцию ошибки в течение времени 49 | motor.kd = 0.3; // отвечает за компенсацию резких изменений 50 | 51 | // установить зону остановки мотора для режима стабилизации позиции в тиках (по умолч. 8) 52 | motor.setStopZone(10); 53 | 54 | motor.setRunMode(ACCEL_POS); 55 | 56 | // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки 57 | // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу 58 | // PID_POS - tick() работает в режиме резкого следования к целевому углу 59 | // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением) 60 | // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору 61 | } 62 | 63 | void loop() { 64 | // потенциометр на А0 65 | // преобразуем значение в -255.. 255 66 | static float val; 67 | val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор 68 | 69 | // для режима PID_POS/ACCEL_POS 70 | motor.setTargetDeg(val * 2); // задаём новый целевой угол в градусах 71 | 72 | // обязательная функция. Делает все вычисления 73 | // принимает текущее значение с энкодера или потенциометра 74 | motor.tick(encTick(4)); 75 | 76 | static uint32_t tmr = 0; 77 | if (millis() - tmr > 100) { // таймер на 100мс для графиков 78 | tmr += 100; 79 | 80 | // отладка позиции (открой плоттер) 81 | Serial.print(motor.getTarget()); 82 | Serial.print(','); 83 | Serial.print(motor.getDuty()); 84 | Serial.print(','); 85 | Serial.print(motor.controlPos); 86 | Serial.print(','); 87 | Serial.println(motor.getCurrent()); 88 | } 89 | } 90 | 91 | // читаем энкодер вручную, через digitalRead() 92 | long encTick(byte pin) { 93 | static bool lastState; 94 | static long encCounter = 0; 95 | bool curState = digitalRead(pin); // опрос 96 | if (lastState != curState) { // словили изменение 97 | lastState = curState; 98 | if (curState) { // по фронту 99 | encCounter += motor.getState(); // запомнили поворот 100 | } 101 | } 102 | return encCounter; 103 | } 104 | -------------------------------------------------------------------------------- /examples/motor_demo_speed/motor_demo_speed.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Пример управления мотором при помощи драйвера полного моста и потенциометра 3 | Для режимов следования к позиции и удержания скорости 4 | */ 5 | #include "AccelMotor.h" 6 | AccelMotor motor(DRIVER2WIRE, 2, 3, HIGH); 7 | 8 | // инициализация наследуется от GyverMotor 9 | // варианты инициализации в зависимости от типа драйвера: 10 | // AccelMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, level) 11 | // AccelMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, level) 12 | // AccelMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, level) 13 | /* 14 | DRIVER2WIRE - двухпроводной драйвер (направление + ШИМ) 15 | DRIVER3WIRE - трёхпроводной драйвер (два пина направления + ШИМ) 16 | RELAY2WIRE - реле в качестве драйвера (два пина направления) 17 | 18 | dig_pin, dig_pin_A, dig_pin_B - любой цифровой пин МК 19 | PWM_pin - любой ШИМ пин МК 20 | level - LOW / HIGH - уровень драйвера. Если при увеличении скорости мотор наоборот тормозит - смени уровень 21 | */ 22 | 23 | void setup() { 24 | Serial.begin(9600); 25 | // использую мотор JGA25 26 | // редуктор 1:21.3 27 | // энкодер 12 тиков на оборот 28 | motor.setRatio(21.3 * 12); 29 | 30 | // период интегрирования (по умолч. 20) 31 | motor.setDt(30); // миллисекунды 32 | 33 | // установка максимальной скорости для режима ACCEL_POS 34 | motor.setMaxSpeedDeg(600); // в градусах/сек 35 | //motor.setMaxSpeed(400); // в тиках/сек 36 | 37 | // установка ускорения для режима ACCEL_POS 38 | motor.setAccelerationDeg(300); // в градусах/сек/сек 39 | //motor.setAcceleration(300); // в тиках 40 | 41 | // минимальный (по модулю) ШИМ сигнал (при котором мотор трогается) 42 | motor.setMinDuty(50); 43 | 44 | // коэффициенты ПИД регулятора 45 | motor.kp = 2; // отвечает за резкость регулирования. 46 | // При малых значениях сигнала вообще не будет, при слишком больших – будет трясти 47 | 48 | motor.ki = 0.2; // отвечает за коррекцию ошибки в течение времени 49 | motor.kd = 0.1; // отвечает за компенсацию резких изменений 50 | 51 | // установить зону остановки мотора для режима стабилизации позиции в тиках (по умолч. 8) 52 | motor.setStopZone(10); 53 | 54 | motor.setRunMode(PID_SPEED); 55 | 56 | // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки 57 | // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу 58 | // PID_POS - tick() работает в режиме резкого следования к целевому углу 59 | // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением) 60 | // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору 61 | } 62 | 63 | void loop() { 64 | // потенциометр на А0 65 | // преобразуем значение в -255.. 255 66 | static float val; 67 | val += (255 - analogRead(0) / 2 - val) * 0.3; // фильтор 68 | 69 | // для режима PID_SPEED/ACCEL_SPEED 70 | motor.setTargetSpeedDeg(val * 4); // задаём целевую скорость в градусах/сек 71 | 72 | // обязательная функция. Делает все вычисления 73 | // принимает текущее значение с энкодера или потенциометра 74 | motor.tick(encTick(4)); 75 | 76 | static uint32_t tmr = 0; 77 | if (millis() - tmr > 100) { // таймер на 100мс для графиков 78 | tmr += 100; 79 | 80 | // отладка скорости (открой плоттер) 81 | Serial.print(motor.getTargetSpeedDeg()); 82 | Serial.print(','); 83 | Serial.print(motor.getDuty()); 84 | Serial.print(','); 85 | Serial.println(motor.getSpeedDeg()); 86 | } 87 | } 88 | 89 | // читаем энкодер вручную, через digitalRead() 90 | long encTick(byte pin) { 91 | static bool lastState; 92 | static long encCounter = 0; 93 | bool curState = digitalRead(pin); // опрос 94 | if (lastState != curState) { // словили изменение 95 | lastState = curState; 96 | if (curState) { // по фронту 97 | encCounter += motor.getState(); // запомнили поворот 98 | } 99 | } 100 | return encCounter; 101 | } 102 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For AccelMotor 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | AccelMotor KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | tick KEYWORD2 14 | setRatio KEYWORD2 15 | setDt KEYWORD2 16 | setCurrent KEYWORD2 17 | getCurrent KEYWORD2 18 | getCurrentDeg KEYWORD2 19 | setTarget KEYWORD2 20 | setTargetDeg KEYWORD2 21 | getTarget KEYWORD2 22 | getTargetDeg KEYWORD2 23 | setMaxSpeed KEYWORD2 24 | setMaxSpeedDeg KEYWORD2 25 | setAcceleration KEYWORD2 26 | setAccelerationDeg KEYWORD2 27 | setTargetSpeed KEYWORD2 28 | setTargetSpeedDeg KEYWORD2 29 | getTargetSpeed KEYWORD2 30 | getTargetSpeedDeg KEYWORD2 31 | getSpeed KEYWORD2 32 | getSpeedDeg KEYWORD2 33 | getDuty KEYWORD2 34 | setRunMode KEYWORD2 35 | setStopZone KEYWORD2 36 | isBlocked KEYWORD2 37 | setRange KEYWORD2 38 | setRangeDeg KEYWORD2 39 | kp KEYWORD2 40 | ki KEYWORD2 41 | kd KEYWORD2 42 | 43 | ####################################### 44 | # Constants (LITERAL1) 45 | ####################################### 46 | 47 | IDLE_RUN LITERAL1 48 | ACCEL_POS LITERAL1 49 | PID_POS LITERAL1 50 | ACCEL_SPEED LITERAL1 51 | PID_SPEED LITERAL1 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=AccelMotor 2 | version=1.3 3 | author=AlexGyver 4 | maintainer=AlexGyver 5 | sentence=Library for smooth control motor with encoder 6 | paragraph=Library for smooth control motor with encoder 7 | category=Device Control 8 | url=https://github.com/GyverLibs/AccelMotor 9 | architectures=* 10 | depends=GyverMotor -------------------------------------------------------------------------------- /src/AccelMotor.cpp: -------------------------------------------------------------------------------- 1 | #include "AccelMotor.h" 2 | #define _sign(x) ((x) > 0 ? 1 : -1) 3 | 4 | bool AccelMotor::tick(long pos) { 5 | _currentPos = pos; 6 | if (millis() - _tmr2 > _dt) { 7 | _dts = (millis() - _tmr2) / 1000.0; 8 | _tmr2 = millis(); 9 | _curSpeed = (long)(_currentPos - _lastPos) / _dts; // ищем скорость в тиках/секунду 10 | _curSpeed = filter(_curSpeed); // медиана + RA 11 | _lastPos = _currentPos; 12 | switch (_runMode) { 13 | case ACCEL_POS: 14 | { 15 | long err = _targetPos - controlPos; // "ошибка" позиции 16 | if (err != 0) { 17 | if (_accel != 0) { 18 | bool thisDir = (controlSpeed * controlSpeed / _accel / 2.0 >= abs(err)); // пора тормозить 19 | controlSpeed += _accel * _dts * (thisDir ? -_sign(controlSpeed) : _sign(err)); 20 | } else { 21 | controlSpeed = err / _dts; // профиль постоянной скорости 22 | } 23 | controlSpeed = constrain(controlSpeed, -_maxSpeed, _maxSpeed); 24 | controlPos += controlSpeed * _dts; 25 | } 26 | PIDcontrol(controlPos, _currentPos, true); 27 | } 28 | break; 29 | case PID_POS: 30 | PIDcontrol(_targetPos, _currentPos, true); 31 | break; 32 | case ACCEL_SPEED: 33 | { 34 | int err = _targetSpeed - _curSpeed; // ошибка скорости 35 | //float reducer = min(abs(err) / _accel*10.0, 1.0); // уменьшает ускорение, если шаг будет дальше чем установка 36 | _dutyF += (float)_sign(err) * _accel/10 * _dts; // ускоряем/замедляем 37 | _dutyF = constrain(_dutyF, -_maxDuty, _maxDuty); // ограничитель 8/10 бит 38 | setSpeed(_dutyF); 39 | } 40 | break; 41 | case PID_SPEED: 42 | PIDcontrol(_targetSpeed, _curSpeed, false); 43 | break; 44 | } 45 | } 46 | if (_runMode > 1) return (getState() != 0); 47 | else return (getState() != 0 || abs(_targetPos - _currentPos) > _stopzone); 48 | } 49 | 50 | void AccelMotor::PIDcontrol(long target, long current, bool cutoff) { 51 | // cutoff нужен только для стабилизации позиции, обнуляет integral и учитывает мёртвую зону мотора 52 | long err = target - current; // ошибка регулирования 53 | long deltaInput = _prevInput - current; // изменение входного сигнала 54 | _dutyF = 0; 55 | if (!cutoff) _dutyF = err * kp; // P составляющая для режимов скорости 56 | _dutyF += (float)deltaInput * kd / _dts; // D составляющая 57 | _prevInput = current; // запомнили текущий 58 | integral += (float)err * ki * _dts; // интегральная сумма 59 | if (cutoff) integral += deltaInput * kp; // +P по скорости изменения для режимов позиции 60 | integral = constrain(integral, -_maxDuty, _maxDuty); // ограничили 61 | _dutyF += integral; // I составляющая 62 | if (cutoff) { // отсечка (для режимов позиции) 63 | if (abs(err) < _stopzone) {integral = 0; _dutyF = 0;} 64 | } else { // для скорости 65 | if (err == 0 && target == 0) integral = 0; 66 | } 67 | _dutyF = constrain(_dutyF, -_maxDuty, _maxDuty); // ограничиваем по разрешению 68 | if (cutoff && _min != 0 && _max != 0 && (current <= _min || current >= _max)) { 69 | setSpeed(0); // вырубаем, если вышли за диапазон 70 | } else setSpeed(_dutyF); // и поехали 71 | } 72 | 73 | void AccelMotor::setRange(long min, long max) { 74 | _min = min; 75 | _max = max; 76 | } 77 | void AccelMotor::setRangeDeg(long min, long max) { 78 | _min = min * _ratio / 360.0; 79 | _max = max * _ratio / 360.0; 80 | } 81 | 82 | bool AccelMotor::isBlocked() { 83 | return (abs(_dutyF) > _minDuty && _curSpeed == 0); 84 | } 85 | 86 | // ===== settings ===== 87 | void AccelMotor::setRatio(float ratio) { 88 | _ratio = ratio; 89 | } 90 | void AccelMotor::setDt(int dt) { 91 | _dt = dt; 92 | _dts = dt / 1000.0; 93 | } 94 | void AccelMotor::setCurrent(long pos) { 95 | _currentPos = pos; 96 | } 97 | void AccelMotor::setRunMode(AM_runMode mode) { 98 | _runMode = mode; 99 | if (mode == ACCEL_POS) controlPos = _currentPos; // костыль! 100 | } 101 | 102 | // ===== current position ===== 103 | long AccelMotor::getCurrent() { 104 | return _currentPos; 105 | } 106 | long AccelMotor::getCurrentDeg() { 107 | return (long)_currentPos * 360.0 / _ratio; 108 | } 109 | 110 | // ===== current speed ===== 111 | int AccelMotor::getSpeed() { 112 | return _curSpeed; 113 | } 114 | int AccelMotor::getSpeedDeg() { 115 | return (long)_curSpeed * 360.0 / _ratio; 116 | } 117 | float AccelMotor::getDuty() { 118 | return _dutyF; 119 | } 120 | 121 | // ===== target position mode ===== 122 | void AccelMotor::setTarget(long pos) { 123 | _targetPos = pos; 124 | _mode = AUTO; 125 | } 126 | void AccelMotor::setTargetDeg(long pos) { 127 | _targetPos = (long)pos * _ratio / 360.0; 128 | _mode = AUTO; 129 | } 130 | long AccelMotor::getTarget() { 131 | return _targetPos; 132 | } 133 | long AccelMotor::getTargetDeg() { 134 | return (long)_targetPos * 360 / _ratio; 135 | } 136 | 137 | void AccelMotor::setStopZone(int zone) { 138 | _stopzone = zone; 139 | } 140 | 141 | // ===== target speed mode ===== 142 | void AccelMotor::setTargetSpeed(int speed) { 143 | _targetSpeed = speed; 144 | _mode = AUTO; 145 | } 146 | void AccelMotor::setTargetSpeedDeg(int speed) { 147 | _targetSpeed = (long)speed * _ratio / 360; 148 | _mode = AUTO; 149 | } 150 | int AccelMotor::getTargetSpeed() { 151 | return _targetSpeed; 152 | } 153 | int AccelMotor::getTargetSpeedDeg() { 154 | return (long)_targetSpeed * 360 / _ratio; 155 | } 156 | 157 | // ===== max speed / acceleration ===== 158 | void AccelMotor::setMaxSpeed(int speed) { 159 | _maxSpeed = speed; 160 | } 161 | void AccelMotor::setMaxSpeedDeg(int speed) { 162 | _maxSpeed = (long)speed * _ratio / 360; 163 | } 164 | void AccelMotor::setAcceleration(int accel) { 165 | _accel = accel; 166 | } 167 | void AccelMotor::setAccelerationDeg(int accel) { 168 | _accel = accel * _ratio / 360.0; 169 | } 170 | 171 | // ==== filter ==== 172 | int AccelMotor::filter(int newVal) { 173 | _buf[_count] = newVal; 174 | if (++_count >= 3) _count = 0; 175 | int middle = 0; 176 | if ((_buf[0] <= _buf[1]) && (_buf[0] <= _buf[2])) { 177 | middle = (_buf[1] <= _buf[2]) ? _buf[1] : _buf[2]; 178 | } else { 179 | if ((_buf[1] <= _buf[0]) && (_buf[1] <= _buf[2])) { 180 | middle = (_buf[0] <= _buf[2]) ? _buf[0] : _buf[2]; 181 | } 182 | else { 183 | middle = (_buf[0] <= _buf[1]) ? _buf[0] : _buf[1]; 184 | } 185 | } 186 | _middle_f += (middle-_middle_f) * 0.7; 187 | return _middle_f; 188 | } 189 | -------------------------------------------------------------------------------- /src/AccelMotor.h: -------------------------------------------------------------------------------- 1 | /* 2 | Библиотека для расширенного управления и стабилизации мотора с энкодером 3 | Документация: https://alexgyver.ru/accelmotor/ 4 | GitHub: https://github.com/GyverLibs/AccelMotor 5 | Возможности: 6 | - Наследует все фишки из библиотеки GyverMotor (поддержка разных драйверов и режимов) 7 | - Режим поддержания скорости с обратной связью 8 | - Режим поворота на заданный угол с обратной связью 9 | - Настраиваемые коэффициенты PID регулятора 10 | - Ограничение ускорения и скорости 11 | - Библиотека принимает любой тип обратной связи: энкодер, потенциометр, и т.д. 12 | - Поддержка мотор-редукторов, настройка передаточного отношения энкодера 13 | - Регулятор учитывает "мёртвую зону" мотора 14 | - Все функции работают в градусах и "тиках" энкодера 15 | 16 | AlexGyver, alex@alexgyver.ru 17 | https://alexgyver.ru/ 18 | MIT License 19 | 20 | Версии: 21 | v1.1 - улучшен алгоритм 22 | v1.2 - совместимость с esp 23 | v1.3 - небольшие улучшения и фиксы 24 | */ 25 | 26 | #ifndef AccelMotor_h 27 | #define AccelMotor_h 28 | #include 29 | #include 30 | 31 | enum AM_runMode { 32 | ACCEL_POS, 33 | PID_POS, 34 | ACCEL_SPEED, 35 | PID_SPEED, 36 | IDLE_RUN, 37 | }; 38 | 39 | class AccelMotor : public GMotor { 40 | public: 41 | using GMotor::GMotor; 42 | 43 | // управляет мотором. Вызывать как можно чаще (внутри таймер с периодом dt) 44 | // принимает текущее положение вала мотора (по счёту энкодера) 45 | // возвращает true если мотор всё ещё движется к цели 46 | bool tick(long pos); 47 | 48 | // установка передаточного отношения редуктора и энкодера 49 | // пример: если редуктор 1:30 - передаём в функцию 30 50 | // пример: если редуктор 1:30 и энкодер на 12 тиков - передаём 30*12 51 | void setRatio(float ratio); 52 | 53 | // установка периода регулятора (рекомендуется 2-50 миллисекунд) 54 | void setDt(int dt); 55 | 56 | // установка и получение текущей позиции в тиках энкодера и градусах. 57 | // setCurrent(pos) равносильна вызову tick(pos) и в принципе не нужна! 58 | void setCurrent(long pos); 59 | long getCurrent(); 60 | long getCurrentDeg(); 61 | 62 | // установка и получение целевой позиции в тиках энкодера и градусах 63 | void setTarget(long pos); 64 | void setTargetDeg(long pos); 65 | long getTarget(); 66 | long getTargetDeg(); 67 | 68 | // установка максимальной скорости в тиках энкодера/секунду и градусах/секунду 69 | void setMaxSpeed(int speed); 70 | void setMaxSpeedDeg(int speed); 71 | 72 | // установка ускорения тиках энкодера и градусах в секунду 73 | void setAcceleration(int accel); 74 | void setAccelerationDeg(int accel); 75 | 76 | // установка и получение целевой скорости в тиках энкодера/секунду и градусах/секунду 77 | void setTargetSpeed(int speed); 78 | void setTargetSpeedDeg(int speed); 79 | int getTargetSpeed(); 80 | int getTargetSpeedDeg(); 81 | 82 | // получить текущую скорость в тиках энкодера/секунду и градусах/секунду 83 | int getSpeed(); 84 | int getSpeedDeg(); 85 | 86 | // получить текущий ШИМ сигнал (float из ПИД регулятора) 87 | float getDuty(); 88 | 89 | // ручная установка режима работы 90 | // IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки 91 | // ACCEL_POS - tick() работает в режиме плавного следования к целевому углу 92 | // PID_POS - tick() работает в режиме резкого следования к целевому углу 93 | // ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением) 94 | // PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору 95 | void setRunMode(AM_runMode mode); 96 | 97 | // возвращает true, если вал мотора заблокирован, а сигнал подаётся 98 | bool isBlocked(); 99 | 100 | // коэффициенты ПИД регулятора 101 | // пропорциональный - от него зависит агрессивность управления, нужно увеличивать kp 102 | // при увеличении нагрузки на вал, чтобы регулятор подавал больший управляющий ШИМ сигнал 103 | float kp = 2.0; // (знач. по умолчанию) 104 | 105 | // интегральный - позволяет нивелировать ошибку со временем, имеет накопительный эффект 106 | float ki = 0.9; // (знач. по умолчанию) 107 | 108 | // дифференциальный. Позволяет чуть сгладить рывки, но при большом значении 109 | // сам становится причиной рывков и раскачки системы! 110 | float kd = 0.1; // (знач. по умолчанию) 111 | 112 | // установить зону остановки мотора для режима стабилизации позиции (по умолч. 8) 113 | void setStopZone(int zone); 114 | 115 | // установить пределы шагов/градусов, вне которых мотор будет жёстко отключен для безопасности. Если по нулям, ограничения нет (по умолч.) 116 | void setRange(long min, long max); 117 | void setRangeDeg(long min, long max); 118 | 119 | long controlPos = 0; // для отладки 120 | private: 121 | int filter(int newVal); 122 | int _buf[3]; 123 | byte _count = 0; 124 | float _middle_f = 0; 125 | long _min = 0, _max = 0; 126 | float _lastSpeed = 0; 127 | void PIDcontrol(long target, long current, bool cutoff); 128 | float integral = 0; 129 | int _dt = 20; 130 | float _dts = 0.02; 131 | long _lastPos = 0, _currentPos = 0, _targetPos = 0; 132 | int _curSpeed = 0; 133 | int _maxSpeed = 300, _targetSpeed = 0; 134 | float _ratio = 1; 135 | uint32_t _tmr2 = 0; 136 | int _accel = 300; 137 | float _dutyF = 0; 138 | float controlSpeed = 0; 139 | int _stopzone = 8; 140 | long _prevInput = 0; 141 | AM_runMode _runMode = IDLE_RUN; 142 | }; 143 | 144 | /* 145 | ======= НАСЛЕДУЕТСЯ ИЗ GYVERMOTOR ======= 146 | GMotor(driverType type, int8_t param1 = NC, int8_t param2 = NC, int8_t param3 = NC, int8_t param4 = NC); 147 | // три варианта создания объекта в зависимости от драйвера: 148 | // GMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, (LOW/HIGH) ) 149 | // GMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, (LOW/HIGH) ) 150 | // GMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, (LOW/HIGH) ) 151 | 152 | // установка скорости 0-255 (8 бит) и 0-1023 (10 бит) 153 | void setSpeed(int16_t duty); 154 | 155 | // сменить режим работы мотора: 156 | // FORWARD - вперёд 157 | // BACKWARD - назад 158 | // STOP - остановить 159 | // BRAKE - активное торможение 160 | void setMode(workMode mode); 161 | 162 | // направление вращения 163 | // NORM - обычное 164 | // REVERSE - обратное 165 | void setDirection(dir direction); 166 | 167 | // установить минимальную скважность (при которой мотор начинает крутиться) 168 | void setMinDuty(int duty); 169 | 170 | // установить выход в 8 бит 171 | void set8bitMode(); 172 | 173 | // установить выход в 10 бит 174 | void set10bitMode(); 175 | 176 | // установить deadtime (в микросекундах). По умолч 0 177 | void setDeadtime(uint16_t deadtime); 178 | 179 | // установить уровень драйвера (по умолч. HIGH) 180 | void setLevel(int8_t level); 181 | 182 | // плавное изменение к указанной скорости (к величине ШИМ) 183 | void smoothTick(int16_t duty); 184 | 185 | // скорость изменения скорости 186 | void setSmoothSpeed(uint8_t speed); 187 | 188 | // дать прямую команду мотору (без смены режима) 189 | void run(workMode mode, int16_t duty); 190 | 191 | // возвращает -1 при вращении BACKWARD, 1 при FORWARD и 0 при остановке и торможении 192 | int getState(); 193 | */ 194 | #endif --------------------------------------------------------------------------------