В этом уроке мы научимся делать линейный сервопривод. В отличие от обычных линейных приводов, которые движутся в определенном направлении при подаче напряжения, этот изготовленный на заказ линейный сервопривод обеспечивает точные и повторяемые движения, которыми можно легко управлять.
Он называется сервоприводом, поскольку имеет систему обратной связи, с помощью которой мы можем точно контролировать выходное движение привода.
Вы можете посмотреть следующее видео или прочитать письменное руководство ниже.
Обзор проекта
Вход для управления этим линейным сервоприводом может быть как аналоговым, так и цифровым. В случае аналогового входа это может быть любой тип потенциометра, как показано здесь. Линейный потенциометр, обычный поворотный потенциометр или, например, джойстик, который также является поворотным потенциометром и т. д.
В случае цифрового входа мы можем управлять приводом с помощью RC-передатчика. Конечно, для этой установки нам также понадобится RC-приемник, который будет использоваться в качестве входа для привода.
Для обоих режимов аналогового и цифрового ввода нам нужно всего 3 провода для подключения, 2 из которых предназначены для питания входного устройства, а 3-й — для входного сигнала.
Интересной особенностью этого изготовленного на заказ линейного сервопривода является то, что мы можем задать начальное и конечное положение для выходного штока, а также настроить чувствительность или скорость реакции привода на наш ввод.
Хотя моя любимая функция — это возможность управлять этим приводом с ПК или ноутбука через последовательный порт. Мы можем ввести значения в миллиметрах через последовательный монитор Arduino IDE, и привод переместится в это положение.
Что еще круче, мы можем выполнять повторяющиеся движения или сохранять положения, набирая «сохранить» на последовательном мониторе в каждой нужной нам позиции, а затем сообщать приводу о необходимости повторять движения в цикле, набирая «запустить» на последовательном мониторе.
Теперь позвольте мне объяснить все, что вам нужно знать об этом изготовленном на заказ линейном сервоприводе, как он работает и как я его спроектировал, чтобы вы также могли собрать его самостоятельно.
Как это работает
Таким образом, замкнутая система управления основана на магнитном датчике положения вращения AS5600 и реализованном ПИД-регуляторе для управления двигателем постоянного тока.
На самом деле я использую ту же самую плату контроллера серводвигателя, которую я сделал в своем предыдущем проекте, которая включает в себя собственный микроконтроллер и все остальное, чтобы легко превратить любой двигатель постоянного тока в автономный серводвигатель.
Если говорить совсем коротко, серводвигатель — это замкнутая система управления, в которой входной сигнал или желаемое положение сравнивается с фактическим положением двигателя, которое мы получаем от датчика обратной связи по положению.
Возникающая разница, называемая ошибкой, затем обрабатывается в контроллере, который дает команду двигателю двигаться до тех пор, пока он не достигнет желаемого положения.
Таким образом, этот линейный сервопривод имеет тот же принцип работы, что и серводвигатель, но с одним дополнительным шагом преобразования вращательного движения двигателя в линейное движение с помощью механизма ходового винта.
Конструкция линейного сервопривода
3D-модель нашего линейного сервопривода, на которой мы можем увидеть, как все работает, показана на следующем рисунке.
Магнитный датчик положения вращения AS5600 расположен на задней стороне привода и отслеживает вращение ходового винта. Ходовой винт, который я использую, имеет шаг 8 мм, что означает, что при каждом полном обороте гайка ходового винта совершает линейное движение на 8 мм.
AS5600 — это 12-битный энкодер, что означает, что он может выводить 4096 позиций за оборот. Если мы разделим 8 на 4096, то получим разрешение 0,001953 мм. Это наименьшее изменение позиции, которое может обнаружить энкодер AS5600. Я думаю, это весьма впечатляет.
Двигатель постоянного тока, который я использую, — это двигатель на 12 В с включенным редуктором, который выдает 480 об/мин. Если мы разделим 480 на 60, то получим значение 8 оборотов в секунду, а если мы умножим это число на 8, то, поскольку ходовой винт имеет шаг 8 мм, мы получим линейную скорость привода 64 мм/с.
Я обнаружил, что это было на месте, потому что максимальный ход штока этого привода составляет 150 мм, поэтому на максимальной скорости потребуется около 2,5 с от начального до конечного положения или около 3 с, если включить ускорение и замедление. Поэтому я использовал передаточное отношение 1:1 для привода ходового винта.
Конструкция всего линейного привода основана на размере платы контроллера серводвигателя, изготовленной на заказ, и, конечно же, ходового винта и гайки ходового винта. Печатная плата имела размер 40x40 мм, так что это был минимальный размер блока цилиндров.
Гайка ходового винта 8 мм имела внешний размер 22 мм, поэтому в соответствии с ней я спроектировал стержень. Гайка и стержень соединены четырьмя болтами М3 и резьбовыми вставками. В верхней части стержня находится подшипник, который скользит по блоку цилиндров, и он используется для направления стержня и предотвращения его вращения.
На выходной крышке цилиндра имеются 4 небольших подшипника, которые направляют шток из блока цилиндров.
В целом, я считаю, что линейный привод получился достаточно компактным, учитывая все использованные компоненты.
Мне также удалось установить внутри блока цилиндров микроконцевой выключатель, который используется для возврата в исходное положение и установки начального положения привода.
Загрузка 3D-модели
Вы можете просматривать и изучать 3D-модель этого изготовленного на заказ линейного сервопривода непосредственно в своем веб-браузере с помощью Onshape (для этого вам понадобится учетная запись Onshape).
Необходимые для 3D-печати STL-файлы, а также STEP-файл этой 3D-модели вы можете получить на сайте Cults3D.
Еще одна вещь, которую следует здесь упомянуть, заключается в том, что вы можете легко увеличить максимальную длину хода этого линейного привода, просто увеличив длину блока цилиндров и штока. Я выбрал эти размеры, потому что хотел, чтобы все детали поместились на 3D-принтере с меньшим печатным столом 220x220 мм. Самая большая часть здесь — шток, длина которого составляет 215 мм.
3D-печать компонентов сервопривода
Мой новый Creality Ender-3 V3 SE отлично справился с печатью в горизонтальной ориентации по оси Y. Хотя нам нужно немного изменить настройки, печать стержня в этой ориентации будет способствовать более плавной работе и более прочному стержню.
При 3D-печати важно использовать функцию горизонтального расширения в программном обеспечении для нарезки, чтобы компенсировать расширение нити и получать более точные по размерам детали.
Я использовал значение –0,1 мм, но вам следует сделать несколько тестовых отпечатков, чтобы увидеть, какое значение подойдет вашему 3D-принтеру.
Я напечатал блок цилиндров по оси Z, чтобы избежать печати большого количества поддерживающего материала. Creality Ender-3 V3 SE также отлично справился с этой печатью.
Я был приятно удивлен качеством печати, которое предлагает этот 3D-принтер, учитывая его цену. 3D-принтер очень легко настроить, у него есть автоматическое выравнивание платформы, прямой экструдер, отличное качество печати и увеличенная скорость печати до 250 мм/с. Все это за чуть менее 200 долларов делает его одним из лучших 3D-принтеров для тех, кто ограничен в средствах.
Сборка линейного сервопривода
Итак, вот все детали, напечатанные на 3D-принтере, и теперь мы можем приступить к сборке линейного привода.
Список деталей
Компоненты, необходимые для этого проекта линейного сервопривода, представлены ниже.
Механика:
- Ходовой винт 8 мм Tr8x8
- Шариковый подшипник 8x22x7мм
- Шариковый подшипник 4x9x4мм x 4
- Шариковый подшипник 6x13x5 мм 686-2RS x1
- Резьбовые вставки M3x5 мм и M4x5 мм
- Болты и гайки M3 и M4. Болты: M3x8 – 10 шт.; M3x10 – 2 шт.; M3x10 зенковка – 1 шт.; M4x6 – 2 шт.; M4x25 – 2 шт.; M4x30 – 2 шт.; M2x8 – 6 шт.; Винт M4x5 – 5 шт.
Электроника:
- Микроконтроллер Atmega328p-AU (купить на AliExpress).
- Драйвер двигателя постоянного тока DRV8871 (купить на AliExpress).
- Магнитный энкодер AS5600 (купить на AliExpress).
- Кварцевый генератор 16 МГц (купить на AliExpress).
- Регулятор напряжения AMS1117 5 В (купить на AliExpress).
- Квадратный потенциометр 3386P.
- Конденсаторы 0805 комплект.
- Двигатель постоянного тока 12 В – ~ 50 об/мин.
Реклама: ООО "АЛИБАБА.КОМ (РУ)" ИНН: 7703380158
Сборка механизма ходового винта
Сначала нам нужно установить ходовой винт на место в блоке основания цилиндра. Для этого сначала нам нужно вставить эту 3D-печатную гайку, которая имеет ту же резьбу, что и ходовой винт.
Немного сложно навинтить гайку на ходовой винт, так как она плотно прилегает, но это то, что нам нужно. Эта гайка удерживает всю силу, когда стержень толкает, поэтому чем плотнее посадка, тем больше силы она сможет удерживать. Кроме того, в гайке также есть отверстие для вставки резьбовой вставки для ее крепления к валу с помощью установочного винта.
Ходовой винт удерживается на месте в блоке цилиндров с помощью двух шарикоподшипников с наружным диаметром 22 мм.
Сзади расположена шестерня, которая приводит в движение ходовой винт. Эта шестерня также имеет соответствующую резьбу и два отверстия для резьбовых вставок для ее крепления к ходовому винту с помощью винтов-штифтов.
Это соединение также имеет решающее значение, поскольку оно передает весь крутящий момент двигателя на ходовой винт, поэтому оно не должно проскальзывать.
Чтобы изготовить этот узел, сначала нам нужно установить резьбовые вставки на шестерню и гайку, а также некоторые на блок цилиндров.
Мы закручиваем шестерню и гайку в противоположном направлении, но не слишком туго, так как это добавляет осевые силы подшипникам. Затем, используя несколько винтов, мы можем закрепить гайку и шестерню на ходовом винте.
После закрепления мы можем заметить, что ходовой винт еще не зафиксирован на месте. Нам нужно добавить эту пластину к блоку цилиндров, которая обеспечит, чтобы подшипники оставались на месте в блоке цилиндров.
На этом сборка завершена: ходовой винт теперь надежно закреплен на месте и может свободно вращаться.
Далее мы можем подготовить стержень. Стержень полый по всей длине для размещения ходового винта. Для соединения гайки ходового винта и стержня нам сначала нужно установить несколько резьбовых вставок.
На верхнюю часть штока необходимо установить направляющий подшипник с наружным диаметром 13 мм и внутренним 6 мм.
Мы помещаем подшипник на 3D-печатный полый вал диаметром 6 мм и закрепляем его на стержне с помощью потайного винта M3 длиной 10 мм. Теперь стержень готов, и мы можем увидеть, как он будет скользить по направляющим рельсам цилиндра.
Далее нам нужно прикрепить цилиндр к блоку основания цилиндра. Но перед этим нам следует установить микроконцевой выключатель на место.
Сначала нам нужно припаять к нему провода, в NC-подключении. Провода должны быть длиной около 15 см. Провода пропускаются через отверстие в верхней части цилиндра, а затем мы можем закрепить микроконцевой выключатель на цилиндре с помощью двух болтов M2 длиной 8 мм.
Вам понадобится именно этот микроконцевой выключатель, чтобы направляющий подшипник срабатывал именно в нужный момент, не задев ничего другого.
Если вы не можете найти точную модель концевого выключателя, вы, конечно, можете модифицировать отверстия и механизм.
Чтобы прикрепить цилиндр к базовому блоку, нам нужно установить здесь несколько резьбовых вставок. Затем мы можем закрепить его на месте с помощью двух болтов М4 длиной 25 мм. На данном этапе нам следует вставить только два верхних болта.
Нижние два понадобятся немного позже при установке крышки редуктора и печатной платы, поскольку для крепления крышки используются те же отверстия.
Далее мы можем ввинтить стержень в ходовой винт. Направляющий подшипник должен войти между направляющими на цилиндре.
При вращении шестерни на задней стороне шток будет двигаться назад до тех пор, пока не достигнет микроконцевого выключателя.
Затем мы можем прикрепить крышку цилиндра на место. Крышка цилиндра вмещает четыре небольших подшипника с внешним диаметром 9 мм. Валы для этих подшипников можно напечатать на 3D-принтере.
Мы должны быть осторожны, вставляя их на место, так как часть, куда входят эти валы, довольно мала и может легко сломаться. Это случалось со мной несколько раз, поэтому убедитесь, что они легко устанавливаются. Эти подшипники будут поддерживать и направлять стержень для более плавной работы.
Крышка цилиндра закреплена на месте четырьмя болтами М4.
Установка двигателя постоянного тока
Хорошо, теперь мы можем вставить двигатель постоянного тока на место. Мы закрепляем двигатель постоянного тока шестью болтами М3. Затем мы можем установить шестерню на вал двигателя.
Для фиксации шестерни на месте мы используем две резьбовые вставки и установочные винты.
После того, как шестерни будут правильно спарены, мы можем перейти к креплению шестеренок и крышки печатной платы на задней стороне линейного привода. Для этого сначала нам нужно установить еще несколько резьбовых вставок в блок основания цилиндра.
Затем мы можем подключить провода к двигателю постоянного тока. В моем случае я напрямую припаял их к двигателю постоянного тока.
Длина проводов должна быть около 20 см. В блоке основания цилиндра есть отверстие, через которое следует пропустить провода как двигателя постоянного тока, так и концевого выключателя.
Затем нам следует также пропустить их через два держателя на крышке, что позволит им держаться подальше от шестерни.
На этом этапе мы можем закрепить крышку на базовом блоке. Для этого сначала нам нужно вставить два болта М4 в нижнюю часть, но не до конца.
Нам следует оставить около 2 мм или 3 мм, чтобы мы могли разместить держатель крышки между ними, а затем скрепить эти болты вместе с крышкой.
Вся эта операция немного грязная, но так и должно было быть, потому что я хотел, чтобы крышка была как можно меньше и представляла собой единый принт, а держатели печатных плат преграждали путь болтам.
Установка специальной платы контроллера серводвигателя
В любом случае, как только мы закончим с крышкой, мы можем установить на место плату контроллера серводвигателя. Как я уже сказал, это тот же контроллер из моего предыдущего видео, где я показал вам, как можно превратить любой двигатель постоянного тока в серводвигатель.
Основным компонентом здесь является магнитный вращающийся энкодер положения AS5600, который отслеживает угловое положение магнита, прикрепленного к выходному валу. В этом случае мы прикрепим магнит к выходной шестерне на ходовом винте. Магнит идеально выравнивается с датчиком AS5600, когда печатная плата встает на место.
Для закрепления печатной платы сначала необходимо вставить гайки М2 в пазы держателей, а затем затянуть печатную плату четырьмя болтами М2.
Теперь осталось только подключить провода на место. Провода двигателя постоянного тока идут к клеммной колодке двигателя, и полярность следует дополнительно проверить, чтобы она совпадала с программой контроллера.
На самом деле, перед подключением двигателя к печатной плате мы можем подать на него некоторое напряжение, чтобы проверить правильность работы механизма ходового винта.
Что касается проводов концевого выключателя, то, поскольку у меня нет специальных контактов для этой цели, я припаял провод заземления к контакту заземления на электролитическом конденсаторе, а провод соединения NC — к контакту SCK, который является цифровым контактом номер 13 на микроконтроллере ATMEGA328.
Клеммная колодка для питания находится прямо рядом с боковой частью крышки, поэтому там есть отверстие, через которое я подключил разъем питания 5,5 мм.
Я также добавил радиатор к драйверу двигателя постоянного тока. Наконец, мы можем установить защелкивающуюся крышку на заднюю сторону и все, мы закончили этот проект.
Теперь мы можем подключить любой тип потенциометра или RC-приемника к соответствующим входным контактам и с его помощью управлять положением линейного привода.
Как я уже упоминал, в предыдущей статье я подробно рассказал, как работает этот контроллер, его принципиальную схему и как я сделал печатную плату.
Схема проекта
Если говорить совсем быстро, то основным компонентом нашего проекта сервопривода является магнитный датчик AS5600, который отслеживает положение выходного сигнала привода. Данные датчика поступают в мозг этой платы контроллера сервопривода, микроконтроллер Atmega328, который выполняет вычисления и сообщает драйверу двигателя постоянного тока DRV8871, как управлять двигателем постоянного тока.
Драйвер двигателя постоянного тока DRV8871 может выдерживать пиковый ток до 3,6 А. Для питания платы мы можем использовать 12 В, которые затем понижаются до 5 В с помощью регулятора напряжения ASM1117 для Atmega328 и других 5-вольтовых компонентов. Имеется двухканальный DIP-переключатель, с помощью которого мы выбираем режим ввода привода: аналоговый или цифровой, или через последовательный порт связи.
Один из подстроечных потенциометров используется для регулировки чувствительности привода, а кнопка SDM — для установки начального и конечного положений.
Заказать изготовление печатной платы данного контроллера вы можете в любом сервисе, в котором вы привыкли это делать.
Скачать Gerber файлы для этого проекта можно по следующей ссылке (однако доступ к ней из России запрещен, поэтому придется использовать специальные средства чтобы получить к ней доступ если вы находитесь в России).
В любом случае, вы, конечно, можете реализовать этот проект линейного привода даже без этого специально изготовленного сервоконтроллера.
Вы можете использовать датчик AS5600 на коммутационной плате в сочетании с платой Arduino для управления двигателем постоянного тока.
Код программы
Теперь давайте взглянем на код этого линейного сервопривода:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 |
/* Linear Servo Actuator - Arduino Code by Dejan, www.HowToMechatronics.com Libraries: AS5600 encoder: https://github.com/RobTillaart/AS5600 PID conroller: https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h */ #include "AS5600.h" #include "Wire.h" #include <PID_v1.h> AS5600 as5600; // use default Wire double Pk1 = 0.65; //speed it gets there double Ik1 = 0; double Dk1 = 0.1; //Define Variables we'll be connecting to double Setpoint, Input, Output; PID myPID(&Input, &Output, &Setpoint, Pk1, Ik1, Dk1, DIRECT); #define motor_IN1 5 #define motor_IN2 6 #define ch1 2 #define setButton 7 #define inputSwitch 3 #define modeSwitch 4 #define limitSwitch 13 int ch1Value; long encoderValue, desiredValue, pwmValue; String serialInput = ""; // string to hold input int serialIntInput; double totalDistance = 0; long startPosition = 358; long endPosition = 6750; long rangeAdjustment = 0; float sensitivityAdjustment = 0; float angle = 0; float angleValue = 0; float rodPosition; float positionsArray[100]; int positionsCounter = 0; long quadrantNumber = 2; long previousQuadrantNumber = 3; long numberOfTurns = 0; float totalAngle = 0; char incomingByte = 0; int intInput = 0; void setup() { Serial.begin(115200); Serial.println(__FILE__); Serial.print("AS5600_LIB_VERSION: "); Serial.println(AS5600_LIB_VERSION); Wire.begin(); pinMode(motor_IN1, OUTPUT); pinMode(motor_IN2, OUTPUT); // Activate the Arduino internal pull-up resistors pinMode(setButton, INPUT_PULLUP); pinMode(inputSwitch, INPUT_PULLUP); pinMode(modeSwitch, INPUT_PULLUP); pinMode(limitSwitch, INPUT_PULLUP); // PID Setup myPID.SetMode(AUTOMATIC); myPID.SetOutputLimits(-255, 255); myPID.SetSampleTime(20); // --- HOMING ---- // Move backward until you ... while (digitalRead(limitSwitch) != 1) { digitalWrite(motor_IN1, LOW); analogWrite(motor_IN2, 70); encoderValue = as5600.readAngle(); } while (digitalRead(limitSwitch) != 0) { analogWrite(motor_IN1, 50); digitalWrite(motor_IN2, LOW); } digitalWrite(motor_IN1, LOW); digitalWrite(motor_IN2, LOW); startPosition = as5600.readAngle() * 0.087890625; endPosition = 6000; Setpoint = startPosition; // --- HOMING End --- } void loop() { // Read encoder value - current position rodPosition = as5600.readAngle() / 0.001953125; // in millimters - The lead screw is 8mm per rotation, and the encoder RAW value is 4096 per roration, so 8/4096 to get the value in milimiters // Serial communication mode - Read data from the serial monitor if (digitalRead(modeSwitch) == 0) { while (Serial.available() > 0) { serialInput = Serial.readString(); serialInput.trim(); // If "save" string is sent through the serial monitor, save the current rodPosition into the array if (serialInput == "save") { positionsArray[positionsCounter] = totalDistance; delay(1000); positionsCounter++; } // Clear the saved positions if (serialInput == "clear") { // Clear the array data to 0 memset(positionsArray, 0, sizeof(positionsArray)); positionsCounter = 0; } // Convert the String to Integer and use it as a Setpoint for the PID control serialIntInput = serialInput.toInt(); if (serialIntInput != 0) { if (serialIntInput < 0) { serialIntInput = 0; } if (serialIntInput > 150) { serialIntInput = 150; } Setpoint = serialIntInput * 45; // convert mm into degrees (1mm linear movement equals 45 degrees rotational movement) } } // Run stored positions if (serialInput == "run") { while (serialInput != "stop") { for (int i = 0; i <= positionsCounter - 1; i++) { if (serialInput == "stop") { break; } while (positionsArray[i] > totalDistance + 25 || positionsArray[i] < totalDistance - 25 || pwmValue != 0) { // Desired position / setpoint for the PID contorller Setpoint = positionsArray[i]; // Read encoder - use that value as an Input for the PID control readEncoder(); // Run motor - PID controller inside runMotor(); } delay(2000); // Delay between steps // Check the serial monitor for a stop command while (Serial.available() > 0) { serialInput = Serial.readString(); serialInput.trim(); } } } } } // Potentiometer and RC Receiver control mode else if (digitalRead(modeSwitch) == 1) { if (digitalRead(inputSwitch) == 0) { // Analog input - Potentiometer // Get value from potentiometer desiredValue = analogRead(A0); if (desiredValue < 15) { desiredValue = 15; } if (desiredValue > 1008) { desiredValue = 1008; } Setpoint = map(desiredValue, 15, 1008, startPosition, endPosition); } else if (digitalRead(inputSwitch) == 1) { // Digital input - RC transmitter desiredValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input if (desiredValue < 1000 || desiredValue > 2000) { desiredValue = 1000; } Setpoint = map(desiredValue, 1000, 2000, startPosition, endPosition); } } // Confine the minimum and maximum values of the setpoint if (Setpoint > endPosition) { Setpoint = endPosition; } if (Setpoint < startPosition) { Setpoint = startPosition; } // Adjusting sensitivity //Pk1 = analogRead(A2) * 0.002; // Adjust the PID gain term //myPID.SetTunings(Pk1, Ik1, Dk1); // Read encoder - use that value as an input for the PID control readEncoder(); // Run motor runMotor(); // Set start and end positions by pressing the "set" button if (digitalRead(setButton) == LOW) { delay(3000); if (digitalRead(setButton) == LOW) { endPosition = totalDistance; while (digitalRead(setButton) != HIGH) ; } else { startPosition = totalDistance; } } } void readEncoder() { // Convert encoder RAW values into angle values for keeping track of the angular position of the shaft encoderValue = as5600.readAngle() * 0.087890625; // Quadrant 1 if (encoderValue >= 0 && encoderValue <= 90) { quadrantNumber = 1; } // Quadrant 2 if (encoderValue >= 90 && encoderValue <= 180) { quadrantNumber = 2; } // Quadrant 3 if (encoderValue >= 180 && encoderValue <= 270) { quadrantNumber = 3; } // Quadrant 4 if (encoderValue >= 270 && encoderValue <= 360) { quadrantNumber = 4; } if (quadrantNumber != previousQuadrantNumber) { // Transition from 4th to 1st quadrant if (quadrantNumber == 1 && previousQuadrantNumber == 4) { numberOfTurns++; } // Transition from 1st to 4th quadrant if (quadrantNumber == 4 && previousQuadrantNumber == 1) { numberOfTurns--; } previousQuadrantNumber = quadrantNumber; } if (totalDistance >= 0) { totalDistance = (numberOfTurns * 360) + encoderValue; } else { totalDistance = (numberOfTurns * 360) + encoderValue; } // Establish Input value for PID Input = totalDistance; // current value of the position } void runMotor() { // Run PID process to get Output value myPID.Compute(); // Move right if (Output > 1) { pwmValue = Output; if (pwmValue < 50 && pwmValue > 15) { pwmValue = pwmValue + 40; } if (pwmValue <= 15) { pwmValue = 0; } analogWrite(motor_IN1, pwmValue); digitalWrite(motor_IN2, LOW); } // Move left else if (Output < 1) { pwmValue = abs(Output); if (pwmValue < 50 && pwmValue > 15) { pwmValue = pwmValue + 40; } if (pwmValue <= 15) { pwmValue = 0; } digitalWrite(motor_IN1, LOW); analogWrite(motor_IN2, pwmValue); } // Do not move else if (Output > -1 && Output < 1) { pwmValue = 0; digitalWrite(motor_IN1, LOW); digitalWrite(motor_IN2, LOW); } } |
Описание кода
Итак, мы начинаем цикл, считывая значение энкодера или текущее положение привода и преобразуя его в миллиметры.
|
1 2 |
// Read encoder value - current position rodPosition = as5600.readAngle() / 0.001953125; // in millimters - The lead screw is 8mm per rotation, and the encoder RAW value is 4096 per roration, so 8/4096 to get the value in milimiters |
Затем, если мы находимся в «режиме последовательной связи», мы считываем входящие данные, которые вводим на последовательном мониторе. Если вход — «сохранить», мы сохраняем текущее положение привода, или если «очистить», мы очищаем все уже сохраненные положения.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
// Serial communication mode - Read data from the serial monitor if (digitalRead(modeSwitch) == 0) { while (Serial.available() > 0) { serialInput = Serial.readString(); serialInput.trim(); // If "save" string is sent through the serial monitor, save the current rodPosition into the array if (serialInput == "save") { positionsArray[positionsCounter] = totalDistance; delay(1000); positionsCounter++; } // Clear the saved positions if (serialInput == "clear") { // Clear the array data to 0 memset(positionsArray, 0, sizeof(positionsArray)); positionsCounter = 0; } |
Если входное значение представляет собой целое число или число от 0 до 150, мы используем это значение в качестве установленного значения.
|
1 2 3 4 5 6 7 8 9 10 11 12 |
// Convert the String to Integer and use it as a Setpoint for the PID control serialIntInput = serialInput.toInt(); if (serialIntInput != 0) { if (serialIntInput < 0) { serialIntInput = 0; } if (serialIntInput > 150) { serialIntInput = 150; } Setpoint = serialIntInput * 45; // convert mm into degrees (1mm linear movement equals 45 degrees rotational movement) } } |
Мы вводим значения в миллиметрах, но для отслеживания вращающегося вала мы используем градусы, поэтому мы преобразуем значения миллиметров в значения градусов, умножая на 45. Это так, потому что для линейного перемещения на 1 мм ходовой винт должен повернуться на 45 градусов. Если у вас другой шаг на вашем ходовом винте, это число должно быть другим.
Если мы наберем «run» в окне последовательного монитора, то с помощью циклов while и for программа будет многократно проходить по сохраненным позициям.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 |
// Run stored positions if (serialInput == "run") { while (serialInput != "stop") { for (int i = 0; i <= positionsCounter - 1; i++) { if (serialInput == "stop") { break; } while (positionsArray[i] > totalDistance + 25 || positionsArray[i] < totalDistance - 25 || pwmValue != 0) { // Desired position / setpoint for the PID contorller Setpoint = positionsArray[i]; // Read encoder - use that value as an Input for the PID control readEncoder(); // Run motor - PID controller inside runMotor(); } delay(2000); // Delay between steps // Check the serial monitor for a stop command while (Serial.available() > 0) { serialInput = Serial.readString(); serialInput.trim(); } } } } |
С другой стороны, если мы находимся в режиме управления потенциометром и RC-приемником, мы проверяем, есть ли у нас аналоговый или цифровой вход.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
// Potentiometer and RC Receiver control mode else if (digitalRead(modeSwitch) == 1) { if (digitalRead(inputSwitch) == 0) { // Analog input - Potentiometer // Get value from potentiometer desiredValue = analogRead(A0); if (desiredValue < 15) { desiredValue = 15; } if (desiredValue > 1008) { desiredValue = 1008; } Setpoint = map(desiredValue, 15, 1008, startPosition, endPosition); } else if (digitalRead(inputSwitch) == 1) { // Digital input - RC transmitter desiredValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input if (desiredValue < 1000 || desiredValue > 2000) { desiredValue = 1000; } Setpoint = map(desiredValue, 1000, 2000, startPosition, endPosition); } } |
Если аналоговый, мы считываем аналоговый вход с потенциометра и используем это значение как установленное значение (желаемое положение для привода). Аналогично, если вход цифровой, мы считываем входящие данные с RC-приемника и используем это значение как установленное значение.
Затем мы вызываем пользовательские функции readEncoder() и runMotor() для считывания текущего положения привода и выполнения ПИД-регулирования. С помощью функции readEncoder() мы считываем текущее значение датчика в угловых значениях, а с помощью этих операторов if мы отслеживаем, в каком квадранте находится текущее положение вала.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 |
void readEncoder() { // Convert encoder RAW values into angle values for keeping track of the angular position of the shaft encoderValue = as5600.readAngle() * 0.087890625; // Quadrant 1 if (encoderValue >= 0 && encoderValue <= 90) { quadrantNumber = 1; } // Quadrant 2 if (encoderValue >= 90 && encoderValue <= 180) { quadrantNumber = 2; } // Quadrant 3 if (encoderValue >= 180 && encoderValue <= 270) { quadrantNumber = 3; } // Quadrant 4 if (encoderValue >= 270 && encoderValue <= 360) { quadrantNumber = 4; } if (quadrantNumber != previousQuadrantNumber) { // Transition from 4th to 1st quadrant if (quadrantNumber == 1 && previousQuadrantNumber == 4) { numberOfTurns++; } // Transition from 1st to 4th quadrant if (quadrantNumber == 4 && previousQuadrantNumber == 1) { numberOfTurns--; } previousQuadrantNumber = quadrantNumber; } if (totalDistance >= 0) { totalDistance = (numberOfTurns * 360) + encoderValue; } else { totalDistance = (numberOfTurns * 360) + encoderValue; } // Establish Input value for PID Input = totalDistance; // current value of the position } |
С помощью этой информации мы можем отслеживать, как вращается вал и когда он сделает полный оборот. Полный угол является входным значением для ПИД-регулятора.
Используя аналоговый вход от подстроечного потенциометра, мы можем отрегулировать пропорциональный коэффициент усиления ПИД-регулятора, и, наконец, мы запускаем процесс ПИД, чтобы получить выходное значение.
|
1 2 3 |
// Adjusting sensitivity //Pk1 = analogRead(A2) * 0.002; // Adjust the PID gain term //myPID.SetTunings(Pk1, Ik1, Dk1); |
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 |
void runMotor() { // Run PID process to get Output value myPID.Compute(); // Move right if (Output > 1) { pwmValue = Output; if (pwmValue < 50 && pwmValue > 15) { pwmValue = pwmValue + 40; } if (pwmValue <= 15) { pwmValue = 0; } analogWrite(motor_IN1, pwmValue); digitalWrite(motor_IN2, LOW); } // Move left else if (Output < 1) { pwmValue = abs(Output); if (pwmValue < 50 && pwmValue > 15) { pwmValue = pwmValue + 40; } if (pwmValue <= 15) { pwmValue = 0; } digitalWrite(motor_IN1, LOW); analogWrite(motor_IN2, pwmValue); } // Do not move else if (Output > -1 && Output < 1) { pwmValue = 0; digitalWrite(motor_IN1, LOW); digitalWrite(motor_IN2, LOW); } } |
Мы используем это выходное значение для управления двигателями постоянного тока с помощью ШИМ-сигнала, влево или вправо, или в неподвижном положении в зависимости от выходного значения ПИД-регулятора или в зависимости от ошибки между желаемым и фактическим положением, считываемым энкодером.
Три члена ПИД-регулятора — пропорциональный, интегральный и производный — определены в верхней части, и, регулируя их, мы можем получать различные выходные отклики.
|
1 2 3 |
double Pk1 = 0.65; //speed it gets there double Ik1 = 0; double Dk1 = 0.1; |
Качество, то есть насколько хорошо привод будет работать или реагировать на наши воздействия, напрямую зависит от этих значений.
Тестирование работы сервопривода
Здесь я проверяю точность привода. Он возвращается на место прилично. Затем я начал перемещать шток по одному миллиметру за раз. Первое движение было около 0,8 мм вместо 1 мм, но следующие 4 были достаточно близки к 1 мм. Затем движение на 4 мм было примерно на 0,15 мм меньше.
Мы должны заметить, что стержень имеет люфт около 0,25 мм. Этот люфт находится между ходовым винтом и гайкой ходового винта. В дополнение к этому у нас, вероятно, есть некоторый люфт в 3D-печатных шестернях, а также в шестернях самого двигателя постоянного тока.
Если мы приложим силу к стержню и проверим точность сейчас, то, конечно, получим еще большую погрешность, но ее можно улучшить, настроив ПИД-регулятор.
Тем не менее, это все для этого урока. Надеюсь, вам понравилось и вы узнали что-то новое.
Похожие статьи
(Проголосуй первым!)
Загрузка...