Создайте прецизионный линейный сервопривод с обратной связью по положению — пошаговое руководство

В этом уроке мы научимся делать линейный сервопривод. В отличие от обычных линейных приводов, которые перемещаются в определенном направлении при подаче напряжения, этот линейный сервопривод, изготовленный по индивидуальному заказу, обеспечивает точные и повторяемые движения, которыми можно легко управлять.

Он называется сервоприводом, потому что имеет систему обратной связи, с помощью которой мы можем точно контролировать выходное движение привода.

Вы можете посмотреть следующее видео или прочитать письменное руководство ниже.

Обзор

Вход для управления этим линейным сервоприводом может быть аналоговым или цифровым. В случае аналогового входа это может быть потенциометр любого типа, как показано здесь. Линейный потенциометр, обычный поворотный потенциометр или, например, джойстик, который снова является поворотным потенциометром, и так далее.

В случае цифрового входа мы можем управлять приводом с помощью RC-передатчика. Конечно, для этой установки нам также понадобится RC-приемник, который будет служить входом для привода.

Как для аналогового, так и для цифрового режима ввода нам нужно всего 3 провода для подключения, 2 из которых предназначены для питания устройства ввода, а третий — для входного сигнала.

Интересная особенность этого изготовленного по индивидуальному заказу линейного сервопривода заключается в том, что мы можем установить произвольное начальное и конечное положение выходного стержня, а также настроить чувствительность или скорость реакции привода на наши входные данные. 

Однако моя любимая функция — это возможность управлять этим приводом с ПК или ноутбука через последовательный порт. Мы можем ввести значения в миллиметрах через последовательный монитор Arduino IDE, и привод переместится в это положение.

Что еще круче, мы можем выполнять повторяемые движения или сохранять позиции, набирая «сохранить» на последовательном мониторе в каждой желаемой позиции, а затем приказывать приводу повторять движения в цикле, набирая «run» на последовательном мониторе.

Теперь позвольте мне объяснить все, что вам нужно знать об этом изготовленном по индивидуальному заказу линейном сервоприводе, о том, как он работает и как я его спроектировал, чтобы вы также могли создать его самостоятельно. 

Как это работает

Таким образом, система управления с обратной связью основана на магнитном датчике поворота 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, вы можете создать бесплатную учетную запись для домашнего использования)

Вы можете получить файлы STL, необходимые для 3D-печати, а также файл 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-принтер в магазине Creality. или Амазон . Также ознакомьтесь с моим подробным обзором на моем сайте.

Сборка

В любом случае, вот все детали, напечатанные на 3D-принтере, и теперь мы можем начать со сборки линейного привода.

Список деталей

Вы можете получить компоненты, необходимые для этого проекта линейного сервопривода, по ссылкам ниже:

Механический:

• Ходовой винт 8 мм Tr8x8 ………………….. Amazon / Алиэкспресс
• Шарикоподшипник 8х22х7мм ……………………………. Амазон  / Алиэкспресс
• Шарикоподшипник 4x9x4 мм x 4 …………………………………….. Amazon / Алиэкспресс
• Шарикоподшипник 6x13x5 мм 686-2RS x1 ……………..….. Amazon  / Алиэкспресс
• Резьбовые вставки M3x5 мм и M4x5 мм ……….. Amazon  / Алиэкспресс
• Болты и гайки M3 и M4 ……………….…………….…. Амазон  / Алиэкспресс
  Болты:М3х8 – 10шт; М3х10 – 2шт; Зенкер М3х10 – 1шт; М4х6 – 2шт; М4х25 – 2шт; М4х30 – 2шт; М2х8 – 6шт; Резьбовой штифт М4х5 – 5 шт.

Электроника:

• Магнитный кодер AS5600 ........................ Amazon / AliExpress
• DRV8871 Драйвер двигателя постоянного тока ……………….…. Amazon / AliExpress
• Atmega328p-AU ……………………. Amazon / AliExpress
• Кварцевый генератор 16 МГц ……….. Amazon / AliExpress
• AMS1117 Регулятор напряжения 5 В ………..…. Amazon / AliExpress
• Квадратный потенциометр 3386P ………………. Amazon / AliExpress
• Комплект конденсаторов 0805 ……………….…. Amazon / AliExpress
• Двигатель постоянного тока 12 В – ~ 50 об/мин …………….……. Амазон  / Алиэкспресс

Раскрытие информации:это партнерские ссылки. Как сотрудник Amazon, я зарабатываю на соответствующих покупках.

Сборка механизма ходового винта

Сначала нам нужно установить ходовой винт на место в блоке цилиндров. Для этого сначала нам нужно вставить напечатанную на 3D-принтере гайку с той же резьбой, что и ходовой винт.

Немного сложно накрутить гайку на ходовой винт, так как она плотно прилегает, но это то, что нам нужно. Эта гайка удерживает всю силу, когда стержень нажимается, поэтому, чем плотнее посадка, тем большую силу можно будет удержать. Кроме того, в гайке имеется отверстие для установки резьбовой вставки для крепления ее к валу с помощью потайного винта.

Ходовой винт удерживается в базовом блоке цилиндра с помощью двух шарикоподшипников с внешним диаметром 22 мм.

На задней стороне находится шестерня, приводящая в движение ходовой винт. Эта шестерня также имеет соответствующую резьбу и два отверстия для резьбовых вставок для крепления ее к ходовому винту с помощью потайных винтов.

Это соединение также имеет решающее значение, поскольку оно передает весь крутящий момент двигателя на ходовой винт, поэтому он не должен проскальзывать. 

Чтобы сделать этот узел, сначала нам нужно установить резьбовые вставки на шестерню и гайку, а также некоторые на блок цилиндров.

Шестерню и гайку закручиваем в противоположном направлении, но не слишком туго, так как это добавляет осевых усилий к подшипникам. Затем, используя несколько установочных винтов, мы можем закрепить гайку и шестерню на ходовом винте.

После закрепления мы можем заметить, что ходовой винт еще не закреплен на месте. Нам нужно добавить эту пластину к блоку цилиндров, чтобы подшипники оставались на месте в блоке цилиндров.

На этом сборка завершена; теперь ходовой винт прочно стоит на месте и может свободно вращаться. 

Далее мы можем подготовить стержень. Стержень полый по всей длине для размещения ходового винта. Для соединения гайки ходового винта и стержня сначала необходимо установить резьбовые вставки. 

На верхней стороне стержня нам необходимо установить направляющий подшипник с внешним диаметром 13 мм и внутренним диаметром 6 мм.

Мы помещаем подшипник на полый вал диаметром 6 мм, напечатанный на 3D-принтере, и прикрепляем его к стержню с помощью винта с потайной головкой M3 длиной 10 мм. Теперь стержень готов, и мы можем видеть, как он будет скользить по направляющим цилиндра.

Далее нам нужно прикрепить цилиндр к базовому блоку цилиндров. Однако прежде чем мы это сделаем, нам следует установить микроконцевой выключатель на место.

Сначала нам нужно припаять к нему провода, у NC соединения. Длина проволоки должна составлять около 15 см. Провода пропускаются через отверстие в верхней части цилиндра, после чего мы можем закрепить микроконцевой выключатель на цилиндре с помощью двух болтов М2 длиной 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 используется для установки начального и конечного положений.

Я заказал печатную плату в PCBWay. Здесь мы можем просто загрузить файл Gerber, выбрать свойства нашей платы и заказать ее по разумной цене.

Я спроектировал печатную плату так, чтобы она имела 4 слоя, средние предназначены для заземления, что немного увеличивает цену. Я не менял никаких свойств по умолчанию, за исключением цвета печатной платы, который я выбрал белым, и отмечаю, что принимаю изменение отделки поверхности на иммерсионное золото, если применимо, без дополнительной оплаты.

Вы можете найти и загрузить Gerber в сообществе обмена проектами PCBWay, через которое вы также можете напрямую заказать печатную плату.

В любом случае, вы также можете реализовать этот проект линейного привода, даже без специального сервоконтроллера.

Вы можете использовать датчик AS5600 на коммутационной плате в сочетании с платой Arduino для управления двигателем постоянного тока.

Код

Теперь давайте посмотрим на код этого линейного сервопривода:

/*
 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);
 }
}
Code language: PHP (php)

Описание кода

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

// 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
Code language: PHP (php)

Затем, если мы находимся в «режиме последовательной связи», мы читаем входящие данные, которые вводим на последовательном мониторе. Если введено «сохранить», мы сохраняем текущее положение привода, а если вход «очистить», мы очищаем все уже сохраненные положения.

// 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;
 }Code language: JavaScript (javascript)

Если входное целое число или число от 0 до 150, мы используем это значение в качестве заданного значения.

// 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)
 }
 }Code language: JavaScript (javascript)

Мы вводим значения в миллиметрах, но для отслеживания вращающегося вала мы используем градусы, поэтому преобразуем значения миллиметров в значения в градусах, умножая на 45. Это так, потому что для линейного перемещения на 1 мм ходовой винт должен вращаться на 45 градусов. Если у вас другой шаг ходового винта, это число должно быть другим. 

Если мы наберем «run», с помощью некоторых циклов while и for программа будет повторно проходить сохраненные позиции.

// 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();
 }
 }
 }
 }Code language: JavaScript (javascript)

С другой стороны, если мы находимся в режиме управления потенциометром и RC-приемником, мы проверяем, есть ли у нас аналоговый или цифровой вход.

// 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);
 }
 }Code language: JavaScript (javascript)

Если аналоговый, мы считываем аналоговый входной сигнал с потенциометра и используем это значение в качестве уставки или желаемого положения, в которое должен перейти привод. Аналогично, если вход цифровой, мы считываем входящие данные с RC-приемника и используем это значение в качестве заданного значения. 

Затем мы вызываем пользовательские функции readEncoder() и runMotor(), чтобы считывать текущее положение привода и выполнять ПИД-управление. С помощью функции readEncoder() мы считываем текущее значение датчика в значениях углов, и с помощью этих операторов if мы отслеживаем, в каком квадранте находится текущее положение вала.

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
}Code language: HTML, XML (xml)

Имея эту информацию, мы можем отслеживать, как вращается вал и когда он сделает полный оборот. Общий угол является входным значением для ПИД-регулятора. 

Используя аналоговый вход потенциометра триммера, мы можем регулировать пропорциональное усиление ПИД-регулятора и, наконец, запускаем ПИД-процесс, чтобы получить выходное значение.

// Adjusting sensitivity
 //Pk1 = analogRead(A2) * 0.002; // Adjust the PID gain term 
 //myPID.SetTunings(Pk1, Ik1, Dk1);Code language: JSON / JSON with Comments (json)

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);
 }
}Code language: JavaScript (javascript)

Мы используем это выходное значение для управления двигателями постоянного тока с помощью ШИМ-сигнала, влево или вправо, или в неподвижном положении, в зависимости от выходного значения ПИД-регулятора или в зависимости от ошибки между желаемым и фактическим положением, считываемым энкодером.

Три члена ПИД-регулятора:пропорциональный, интегральный и производный определены вверху, и, регулируя их, мы можем получить различные выходные характеристики.

double Pk1 = 0.65; //speed it gets there
double Ik1 = 0;
double Dk1 = 0.1;Code language: JavaScript (javascript)

От этих значений напрямую зависит качество, то, насколько хорошо актуатор будет работать или реагировать на наши действия.

Тестирование

Здесь я проверяю точность привода. Возвращается на место прилично. Затем начал перемещать стержень по миллиметру за раз. Первое движение было примерно 0,8 мм вместо 1 мм, но следующие 4 были достаточно близки к 1 мм.  Тогда перемещение на 4 мм было отклонено примерно на 0,15 мм. 

Следует отметить, что стержень имеет люфт около 0,25 мм. Этот зазор возникает между ходовым винтом и гайкой ходового винта. В дополнение к этому у нас, вероятно, есть некоторый люфт в 3D-печатных шестернях, а также в шестернях самого двигателя постоянного тока.  

Если мы приложим силу к стержню и проверим точность сейчас, то, конечно, получим еще большую погрешность, но ее можно улучшить, подстроив ПИД-регулятор. 

Тем не менее, это все, что касается этого урока. Надеюсь, вам понравилось и вы узнали что-то новое.