Как превратить любой двигатель постоянного тока в серводвигатель с помощью Atmega328p

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

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

Кратко весь процесс сборки подобного самодельного серводвигателя вы можете посмотреть в следующем видео:

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

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

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

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

Все это возможно благодаря 12-битному энкодеру, используемому в этом серводвигателе, магнитному датчику положения вращения AS5600 и внедренному ПИД-регулятору для управления двигателем постоянного тока.

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

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

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

Принцип работы серводвигателя

Чтобы объяснить принцип работы серводвигателя, давайте разберем типичный RC-серводвигатель и посмотрим, что у него внутри.

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

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

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

Как сделать собственный серводвигатель

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

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

Теперь, что касается датчика положения, самое простое решение — использовать простой потенциометр, такой же, как тот, что мы видели в сервоприводах RC. Проблема с этими типами потенциометров заключается в том, что они имеют ограниченный диапазон вращения всего в 270 градусов, что напрямую ограничивает диапазон вращения серводвигателя. Существуют также другие типы потенциометров, которые могут делать несколько оборотов и могут обеспечить лучший диапазон и разрешение, но они все равно имеют ограниченное вращение.

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

Энкодер AS5600 – магнитный датчик положения вращения

Я решил использовать магнитный энкодер (магнитный датчик положения вращения) AS5600, потому что это очень компактный и простой в реализации энкодер, обеспечивающий высокую точность/разрешение. Просто посмотрите, насколько мал этот микрочип.

Он имеет встроенный датчик Холла, который может определять изменения направления магнитного поля. Поэтому нам просто нужно прикрепить магнит к выходному валу двигателя и расположить его рядом с микросхемой на расстоянии от 0,5 до 3 мм.

Теперь, когда вал двигателя и магнит будут вращаться, датчик Холла будет фиксировать эти изменения в направлении магнитного поля. С помощью встроенного 12-битного АЦП датчик AS5600 может выводить 4096 положений за один оборот или вращение на 360 градусов.

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

Хорошо, что еще нам нужно, это микроконтроллер и драйвер для двигателя постоянного тока. Я выбрал драйвер двигателя постоянного тока DRV8871, который может работать с током до 3,5 ампер, и микроконтроллер Atmega328.

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

Схема серводвигателя

Ниже представлена полная принципиальная схема этого самодельного серводвигателя.

Итак, у нас есть микроконтроллер Atmega328 вместе с его рекомендуемой минимальной схемой, которая включает в себя генератор 16 МГц, несколько конденсаторов и резистор.

Для питания микроконтроллера и других компонентов, которым требуется 5 В, мы используем регулятор напряжения AMS1117, который понижает входное напряжение 12 В до 5 В.

Ниже представлен датчик положения AS5600 с рекомендуемой схемой, которая включает два конденсатора и два подтягивающих резистора для связи по интерфейсу I2C.

Драйверу двигателя постоянного тока DRV8871 требуется всего один резистор для ограничения тока и два развязывающих конденсатора. Затем у нас есть два потенциометра, подключенных к аналоговым входам микроконтроллера, один для регулировки диапазона вращения, а другой для регулировки чувствительности сервопривода. Кнопка используется для установки центральной точки сервопривода, а двухпозиционный DIP-переключатель — для выбора режимов работы сервопривода. Есть разъем для входов сервопривода, либо аналоговый вход напряжения, либо цифровой вход ШИМ от RC-приемника, а также 5 В и заземляющий контакт. Есть также разъем для программирования микроконтроллера через протокол SPI и последовательный порт.

Вот краткое изложение этой схемы и ее рабочего процесса. Входной сигнал или желаемое угловое положение принимается через эти два контакта, и это может быть либо аналоговое напряжение, поступающее от потенциометра, либо цифровой ШИМ-сигнал, поступающий от RC-приемника. Входной сигнал поступает в микроконтроллер, где он сравнивается с фактическим угловым положением, которое обнаруживается энкодером/датчиком положения AS5600. Этот датчик взаимодействует с микроконтроллером через протокол I2C.

Затем микроконтроллер выполняет вычисления, вычисляет ошибку и в соответствии с ней посылает ШИМ-сигнал на драйвер DRV8871, который управляет двигателем постоянного тока до тех пор, пока он не достигнет желаемого положения.

Вся схема питается от напряжения 12 В, а регулятор напряжения AMS1117 обеспечивает напряжение 5 В для микроконтроллера и других компонентов.

Необходимые компоненты

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

Реклама: ООО «АЛИБАБА.КОМ (РУ)» ИНН: 7703380158

Проектирование печатной платы

Согласно принципиальной схеме, я старался сделать печатную плату как можно меньше, и она получилась размером 40х40 мм.

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

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

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

Скачать Gerber файлы для изготовления этой печатной платы можно по этой ссылке (но доступ по ссылке запрещен из России, поэтому используйте специальные меры чтобы обойти это ограничение).

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

Хорошо, теперь мы можем припаять к ней компоненты. Я начал с более мелких компонентов, таких как этот светодиодный индикатор, конденсаторы и резисторы.

На самом деле, это мой первый опыт пайки небольших SMD-компонентов, и у меня это получилось очень, очень плохо.

Самым сложным оказалось припаять микроконтроллер Atmega328, так как контакты очень маленькие и расположены очень близко друг к другу, но мне как-то удалось это сделать.

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

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

Теперь пришло время сделать подходящий редуктор для двигателя постоянного тока и этой платы контроллера.

Пользовательская 3D-модель сервопривода

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

Здесь я использую двигатель постоянного тока диаметром 37 мм и встроенный редуктор, который выдает 50 об/мин. 50 об/мин — хорошая скорость для серводвигателя, но я хотел немного снизить ее, чтобы получить лучший крутящий момент, поэтому я сделал редуктор с 3-кратным уменьшением. Для этой цели я использовал шевронные шестерни, поскольку они эффективны и просты в изготовлении на 3D-принтере.

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

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

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

Конечно, вы также можете скачать 3D-модель, а также файлы STL, необходимые для 3D-печати деталей, по следующим ссылкам:

Сборка серводвигателя

Напечатанные на 3D-принтере детали, необходимые для сборки серводвигателя, показаны на следующем рисунке.

Вместе с ними нам понадобятся болты М3 и резьбовые вставки, а также подшипники.

Сначала я закрепил двигатель постоянного тока на базовой пластине с помощью нескольких болтов М3 длиной 8 мм.

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

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

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

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

Завершим сборку редуктора, установив боковую панель, шарикоподшипник выходного вала и верхнюю крышку.

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

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

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

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

Далее я припаял два провода к двигателю постоянного тока и подключил двигатель к контроллеру с помощью клеммной колодки. Что касается питания, я подключил два провода к клеммной колодке питания, которая с другой стороны имеет разъем питания постоянного тока для подключения источника питания 12 В. И все, наш серводвигатель готов.

Программирование микроконтроллера

Осталось дать жизнь этому сервоприводу, то есть запрограммировать микроконтроллер. Для этого сначала нужно записать загрузчик в микроконтроллер ATmega328p. Без загрузчика микроконтроллер не сможет понять код, который мы ему отправим.

Прошивка загрузчика

Чтобы записать загрузчик на ATmega328p, нам понадобится плата Arduino, в моем случае я буду использовать плату Arduino Nano.

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

Теперь, используя Arduino IDE, нам нужно открыть пример скетча ArduinoISP и загрузить его на плату Arduino Nano. С этим кодом Arduino Nano теперь может записать загрузчик на микроконтроллер ATmega328.

Далее в меню Tools («Инструменты») в качестве программиста нам нужно выбрать Arduino в качестве ISP, а затем нажать Burn Bootloader («Записать загрузчик»).

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

Код загрузки

Закончив с этим, мы можем загрузить код на плату микроконтроллера с помощью модуля интерфейса USB-UART.

Плата микроконтроллера имеет специальные контакты для простого подключения, как показано на этой принципиальной схеме.

Теперь мы можем открыть код для этого сервопривода и загрузить его в микроконтроллер. Но прежде чем мы это сделаем, нам нужно установить библиотеки для датчика AS5600 и ПИД-регулятора. Это можно легко сделать из менеджера библиотек Arduino IDE. Как только мы нажмем кнопку загрузки, код будет записан в наш микроконтроллер ATmega328 через модуль интерфейса USB-UART.

Вот и все, наш серводвигатель индивидуального изготовления готов. Теперь мы можем подключить к нему потенциометр, чтобы проверить его. Просто обратите внимание, что аналоговый вход идет на контакт «S» на плате контроллера, а не на контакт «A».

При проектировании печатной платы я неправильно подключил эти два контакта к ATmega328. Затем мы можем выбрать режим аналогового входа через DIP-переключатель и запитать сервопривод.

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

Исходный код программы

Теперь давайте взглянем на код нашего серводвигателя.

/*
 *    Custom-built Servo Motor - 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 = 2;  //speed it gets there
double Ik1 = 0;
double Dk1 = 0.025;

//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 centerSet 7
#define inputSwitch 3
#define modeSwitch 4

int ch1Value;

int encoderValue, inputValue, pwmValue;
String inString = "";  // string to hold input

int centerAngle = 2047; // 180 degrees
int angleDifference = 0;
int angleValue = 0;
int leftLimit = 30;
int rightLimit = 4067;
int rangeAdjustment = 0;
float sensitivityAdjustment = 0;
float angle = 0;

int quadrantNumber = 2;
int previousQuadrantNumber = 3;
int numberOfTurns = 0;
float totalAngle = 0;

int error = 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(centerSet, INPUT_PULLUP);
  pinMode(inputSwitch, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);

  myPID.SetMode(AUTOMATIC);              // PID Setup
  myPID.SetOutputLimits(-255, 255);
  myPID.SetSampleTime(20);

}

void loop() {
  // Read encoder value - current position
  encoderValue = as5600.readAngle();

  // Continuous rotation mode
  if (digitalRead(modeSwitch) == 0) {
    // Enter desired angle for the servo to go to through the serial monitor
    while (Serial.available() > 0) {
      int inChar = Serial.read();
      if (isDigit(inChar)) {
        // convert the incoming byte to a char and add it to the string:
        inString += (char)inChar;
      }
      // if you get a newline, print the string, then the string's value:
      if (inChar == '\n') {
        Setpoint = inString.toInt(); // Setpoint - desired angle
        // clear the string for new input:
        inString = "";
      }
    }
    if (digitalRead(inputSwitch) == 0) { // Potentiometer as input
      inputValue = analogRead(A0);
      if (inputValue < 400) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 300) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 200) {
        Setpoint = Setpoint - 0.3;
      }

      if (inputValue > 600) {
        Setpoint = Setpoint + 0.3;
      }
      if (inputValue > 700) {
        Setpoint = Setpoint + 0.3;
      }

      if (inputValue > 800) {
        Setpoint = Setpoint + 0.3;
      }
    }
    else if (digitalRead(inputSwitch) == 1) {
      inputValue = pulseIn(ch1, HIGH, 30000); // RC receiver as input
      if (inputValue < 1450) {
        Setpoint--;
      }
      if (inputValue < 1350) {
        Setpoint--;
      }
      if (inputValue < 1250) {
        Setpoint--;
      }
      if (inputValue < 1150) {
        Setpoint--;
      }
      if (inputValue > 1550) {
        Setpoint++;
      }
      if (inputValue > 1650) {
        Setpoint++;
      }
      if (inputValue > 1750) {
        Setpoint++;
      }
      if (inputValue > 1850) {
        Setpoint++;
      }
    }

    // Convert encoder RAW values into angle value
    angle = encoderValue * 0.087890625;
    // Quadrant 1
    if (angle >= 0 && angle <= 90) {
      quadrantNumber = 1;
    }
    // Quadrant 2
    if (angle >= 90 && angle <= 180) {
      quadrantNumber = 2;
    }
    // Quadrant 3
    if (angle >= 180 && angle <= 270) {
      quadrantNumber = 3;
    }
    // Quadrant 4
    if (angle >= 270 && angle <= 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 (totalAngle >= 0) {
      totalAngle = (numberOfTurns * 360) + angle;
    }
    else {
      totalAngle = (numberOfTurns * 360) + angle;
    }

    // Establish Input value for PID
    Input = totalAngle;
  }

  // Limited Rotation Mode
  else if (digitalRead(modeSwitch) == 1) {

    rangeAdjustment = analogRead(A1);
    leftLimit = 0 + 30 + rangeAdjustment;
    rightLimit = 4097 - 30 - rangeAdjustment;


    if (digitalRead(inputSwitch) == 0) {  // Analog input - Potentiometer
      // Get value from potentiometer
      inputValue = analogRead(A0);
      if (inputValue < 15) {
        inputValue = 15;
      }
      if (inputValue > 1008) {
        inputValue = 1008;
      }
      Setpoint = map(inputValue, 15, 1008, -255, 255);
    }
    else if (digitalRead(inputSwitch) == 1) {  // Digital input - RC transmitter
      inputValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input
      Setpoint = map(inputValue, 1000, 2000, -255, 255);
    }

    // Set center angle
    if (digitalRead(centerSet) == LOW) {
      centerAngle = encoderValue;
      angleDifference = 2047 - encoderValue;
      delay(1000);
    }
    // Adjust angle value according to the center point (angleDifference)
    if (centerAngle < 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue < 4097 && encoderValue > (4096 - angleDifference)) {
        angleValue = encoderValue - 4097 + angleDifference;
      }
    }
    if (centerAngle > 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {
        angleValue = encoderValue + 4097 + angleDifference;
      }
    }
    else if (centerAngle == 2047) {
      angleValue = encoderValue;
    }

    // Establish Input value for PID
    Input = map(angleValue , leftLimit, rightLimit, -255, 255);
  }
  // Adjusting sensitivity
  Pk1 = analogRead(A2) * 0.002;
  myPID.SetTunings(Pk1, Ik1, Dk1);

  // Run PID process to get Output value
  myPID.Compute();
  // Move right
  if (Output > 1 ) {
    pwmValue = Output;

    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    digitalWrite(motor_IN1, LOW);
    analogWrite(motor_IN2, pwmValue);
  }
  // Move left
  else if (Output < 1 ) {
    pwmValue = abs(Output);

    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    analogWrite(motor_IN1, pwmValue);
    digitalWrite(motor_IN2, LOW);
  }
  // Do not move
  else if (Output > -1 && Output < 1) {
    pwmValue = 0;
    digitalWrite(motor_IN1, LOW);
    digitalWrite(motor_IN2, LOW);
  }

  //Serial.print(Setpoint);
  //Serial.print("\t");
  //Serial.println(totalAngle);
}

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

* Custom-built Servo Motor - 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 = 2; //speed it gets there

double Ik1 = 0;

double Dk1 = 0.025;

//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 centerSet 7

#define inputSwitch 3

#define modeSwitch 4

int ch1Value;

int encoderValue, inputValue, pwmValue;

String inString = ""; // string to hold input

int centerAngle = 2047; // 180 degrees

int angleDifference = 0;

int angleValue = 0;

int leftLimit = 30;

int rightLimit = 4067;

int rangeAdjustment = 0;

float sensitivityAdjustment = 0;

float angle = 0;

int quadrantNumber = 2;

int previousQuadrantNumber = 3;

int numberOfTurns = 0;

float totalAngle = 0;

int error = 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(centerSet, INPUT_PULLUP);

pinMode(inputSwitch, INPUT_PULLUP);

pinMode(4, INPUT_PULLUP);

myPID.SetMode(AUTOMATIC); // PID Setup

myPID.SetOutputLimits(-255, 255);

myPID.SetSampleTime(20);

}

void loop() {

// Read encoder value - current position

encoderValue = as5600.readAngle();

// Continuous rotation mode

if (digitalRead(modeSwitch) == 0) {

// Enter desired angle for the servo to go to through the serial monitor

while (Serial.available() > 0) {

int inChar = Serial.read();

if (isDigit(inChar)) {

// convert the incoming byte to a char and add it to the string:

inString += (char)inChar;

}

// if you get a newline, print the string, then the string's value:

if (inChar == '\n') {

Setpoint = inString.toInt(); // Setpoint - desired angle

// clear the string for new input:

inString = "";

}

if (digitalRead(inputSwitch) == 0) { // Potentiometer as input

inputValue = analogRead(A0);

if (inputValue < 400) {

Setpoint = Setpoint - 0.3;

}

if (inputValue < 300) {

Setpoint = Setpoint - 0.3;

}

if (inputValue < 200) {

Setpoint = Setpoint - 0.3;

}

if (inputValue > 600) {

Setpoint = Setpoint + 0.3;

}

if (inputValue > 700) {

Setpoint = Setpoint + 0.3;

}

if (inputValue > 800) {

Setpoint = Setpoint + 0.3;

}

else if (digitalRead(inputSwitch) == 1) {

inputValue = pulseIn(ch1, HIGH, 30000); // RC receiver as input

if (inputValue < 1450) {

Setpoint--;

}

if (inputValue < 1350) {

Setpoint--;

}

if (inputValue < 1250) {

Setpoint--;

}

if (inputValue < 1150) {

Setpoint--;

}

if (inputValue > 1550) {

Setpoint++;

}

if (inputValue > 1650) {

Setpoint++;

}

if (inputValue > 1750) {

Setpoint++;

}

if (inputValue > 1850) {

Setpoint++;

}

// Convert encoder RAW values into angle value

angle = encoderValue * 0.087890625;

// Quadrant 1

if (angle >= 0 && angle <= 90) {

quadrantNumber = 1;

}

// Quadrant 2

if (angle >= 90 && angle <= 180) {

quadrantNumber = 2;

}

// Quadrant 3

if (angle >= 180 && angle <= 270) {

quadrantNumber = 3;

}

// Quadrant 4

if (angle >= 270 && angle <= 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 (totalAngle >= 0) {

totalAngle = (numberOfTurns * 360) + angle;

}

else {

totalAngle = (numberOfTurns * 360) + angle;

}

// Establish Input value for PID

Input = totalAngle;

}

// Limited Rotation Mode

else if (digitalRead(modeSwitch) == 1) {

rangeAdjustment = analogRead(A1);

leftLimit = 0 + 30 + rangeAdjustment;

rightLimit = 4097 - 30 - rangeAdjustment;

if (digitalRead(inputSwitch) == 0) { // Analog input - Potentiometer

// Get value from potentiometer

inputValue = analogRead(A0);

if (inputValue < 15) {

inputValue = 15;

}

if (inputValue > 1008) {

inputValue = 1008;

}

Setpoint = map(inputValue, 15, 1008, -255, 255);

}

else if (digitalRead(inputSwitch) == 1) { // Digital input - RC transmitter

inputValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input

Setpoint = map(inputValue, 1000, 2000, -255, 255);

}

// Set center angle

if (digitalRead(centerSet) == LOW) {

centerAngle = encoderValue;

angleDifference = 2047 - encoderValue;

delay(1000);

}

// Adjust angle value according to the center point (angleDifference)

if (centerAngle < 2047) {

angleValue = encoderValue + angleDifference;

if (encoderValue < 4097 && encoderValue > (4096 - angleDifference)) {

angleValue = encoderValue - 4097 + angleDifference;

}

if (centerAngle > 2047) {

angleValue = encoderValue + angleDifference;

if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {

angleValue = encoderValue + 4097 + angleDifference;

}

else if (centerAngle == 2047) {

angleValue = encoderValue;

}

// Establish Input value for PID

Input = map(angleValue , leftLimit, rightLimit, -255, 255);

}

// Adjusting sensitivity

Pk1 = analogRead(A2) * 0.002;

myPID.SetTunings(Pk1, Ik1, Dk1);

// Run PID process to get Output value

myPID.Compute();

// Move right

if (Output > 1 ) {

pwmValue = Output;

if (pwmValue < 30 && pwmValue > 5) {

pwmValue = pwmValue + 30;

}

if (pwmValue <= 5) {

pwmValue = 0;

}

digitalWrite(motor_IN1, LOW);

analogWrite(motor_IN2, pwmValue);

}

// Move left

else if (Output < 1 ) {

pwmValue = abs(Output);

if (pwmValue < 30 && pwmValue > 5) {

pwmValue = pwmValue + 30;

}

if (pwmValue <= 5) {

pwmValue = 0;

}

analogWrite(motor_IN1, pwmValue);

digitalWrite(motor_IN2, LOW);

}

// Do not move

else if (Output > -1 && Output < 1) {

pwmValue = 0;

digitalWrite(motor_IN1, LOW);

digitalWrite(motor_IN2, LOW);

}

//Serial.print(Setpoint);

//Serial.print("\t");

//Serial.println(totalAngle);

}

Описание работы кода

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

// Read encoder value - current position
  encoderValue = as5600.readAngle();

1 2	// Read encoder value - current position encoderValue = as5600.readAngle();

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

// Enter desired angle for the servo to go to through the serial monitor
    while (Serial.available() > 0) {
      int inChar = Serial.read();
      if (isDigit(inChar)) {
        // convert the incoming byte to a char and add it to the string:
        inString += (char)inChar;
      }
      // if you get a newline, print the string, then the string's value:
      if (inChar == '\n') {
        Setpoint = inString.toInt(); // Setpoint - desired angle
        // clear the string for new input:
        inString = "";
      }
    }

// Enter desired angle for the servo to go to through the serial monitor

while (Serial.available() > 0) {

int inChar = Serial.read();

if (isDigit(inChar)) {

// convert the incoming byte to a char and add it to the string:

inString += (char)inChar;

}

// if you get a newline, print the string, then the string's value:

if (inChar == '\n') {

Setpoint = inString.toInt(); // Setpoint - desired angle

// clear the string for new input:

inString = "";

}

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

if (digitalRead(inputSwitch) == 0) { // Potentiometer as input
      inputValue = analogRead(A0);
      if (inputValue < 400) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 300) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 200) {
        Setpoint = Setpoint - 0.3;
      }

      if (inputValue > 600) {
        Setpoint = Setpoint + 0.3;
      }
      if (inputValue > 700) {
        Setpoint = Setpoint + 0.3;
      }

      if (inputValue > 800) {
        Setpoint = Setpoint + 0.3;
      }
    }

if (digitalRead(inputSwitch) == 0) { // Potentiometer as input

inputValue = analogRead(A0);

if (inputValue < 400) {

Setpoint = Setpoint - 0.3;

}

if (inputValue < 300) {

Setpoint = Setpoint - 0.3;

}

if (inputValue < 200) {

Setpoint = Setpoint - 0.3;

}

if (inputValue > 600) {

Setpoint = Setpoint + 0.3;

}

if (inputValue > 700) {

Setpoint = Setpoint + 0.3;

}

if (inputValue > 800) {

Setpoint = Setpoint + 0.3;

}

Мы выполняем ту же коррекцию значения, если входом является RC-приемник.

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

// Convert encoder RAW values into angle value
    angle = encoderValue * 0.087890625;
    // Quadrant 1
    if (angle >= 0 && angle <= 90) {
      quadrantNumber = 1;
    }
    // Quadrant 2
    if (angle >= 90 && angle <= 180) {
      quadrantNumber = 2;
    }
    // Quadrant 3
    if (angle >= 180 && angle <= 270) {
      quadrantNumber = 3;
    }
    // Quadrant 4
    if (angle >= 270 && angle <= 360) {
      quadrantNumber = 4;
    }

// Convert encoder RAW values into angle value

angle = encoderValue * 0.087890625;

// Quadrant 1

if (angle >= 0 && angle <= 90) {

quadrantNumber = 1;

}

// Quadrant 2

if (angle >= 90 && angle <= 180) {

quadrantNumber = 2;

}

// Quadrant 3

if (angle >= 180 && angle <= 270) {

quadrantNumber = 3;

}

// Quadrant 4

if (angle >= 270 && angle <= 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 (totalAngle >= 0) {
      totalAngle = (numberOfTurns * 360) + angle;
    }
    else {
      totalAngle = (numberOfTurns * 360) + angle;
    }

    // Establish Input value for PID
    Input = totalAngle;

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 (totalAngle >= 0) {

totalAngle = (numberOfTurns * 360) + angle;

}

else {

totalAngle = (numberOfTurns * 360) + angle;

}

// Establish Input value for PID

Input = totalAngle;

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

rangeAdjustment = analogRead(A1);
    leftLimit = 0 + 30 + rangeAdjustment;
    rightLimit = 4097 - 30 - rangeAdjustment;

rangeAdjustment = analogRead(A1);

leftLimit = 0 + 30 + rangeAdjustment;

rightLimit = 4097 - 30 - rangeAdjustment;

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

if (digitalRead(inputSwitch) == 0) {  // Analog input - Potentiometer
      // Get value from potentiometer
      inputValue = analogRead(A0);
      if (inputValue < 15) {
        inputValue = 15;
      }
      if (inputValue > 1008) {
        inputValue = 1008;
      }
      Setpoint = map(inputValue, 15, 1008, -255, 255);
    }

if (digitalRead(inputSwitch) == 0) { // Analog input - Potentiometer

// Get value from potentiometer

inputValue = analogRead(A0);

if (inputValue < 15) {

inputValue = 15;

}

if (inputValue > 1008) {

inputValue = 1008;

}

Setpoint = map(inputValue, 15, 1008, -255, 255);

}

Если режимом входа является RC-приемник, мы считываем входящее значение ШИМ с приемника и используем это значение в качестве установленной точки (setpoint).

else if (digitalRead(inputSwitch) == 1) {  // Digital input - RC transmitter
      inputValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input
      Setpoint = map(inputValue, 1000, 2000, -255, 255);
    }

else if (digitalRead(inputSwitch) == 1) { // Digital input - RC transmitter

inputValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input

Setpoint = map(inputValue, 1000, 2000, -255, 255);

}

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

// Set center angle
    if (digitalRead(centerSet) == LOW) {
      centerAngle = encoderValue;
      angleDifference = 2047 - encoderValue;
      delay(1000);
    }

// Set center angle

if (digitalRead(centerSet) == LOW) {

centerAngle = encoderValue;

angleDifference = 2047 - encoderValue;

delay(1000);

}

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

if (centerAngle > 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {
        angleValue = encoderValue + 4097 + angleDifference;
      }
    }
    else if (centerAngle == 2047) {
      angleValue = encoderValue;
    }

    // Establish Input value for PID
    Input = map(angleValue , leftLimit, rightLimit, -255, 255);

if (centerAngle > 2047) {

angleValue = encoderValue + angleDifference;

if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {

angleValue = encoderValue + 4097 + angleDifference;

}

else if (centerAngle == 2047) {

angleValue = encoderValue;

}

// Establish Input value for PID

Input = map(angleValue , leftLimit, rightLimit, -255, 255);

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

// Adjusting sensitivity
  Pk1 = analogRead(A2) * 0.002;
  myPID.SetTunings(Pk1, Ik1, Dk1);

  // Run PID process to get Output value
  myPID.Compute();

// Adjusting sensitivity

Pk1 = analogRead(A2) * 0.002;

myPID.SetTunings(Pk1, Ik1, Dk1);

// Run PID process to get Output value

myPID.Compute();

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

// Move right
  if (Output > 1 ) {
    pwmValue = Output;

    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    digitalWrite(motor_IN1, LOW);
    analogWrite(motor_IN2, pwmValue);
  }

// Move right

if (Output > 1 ) {

pwmValue = Output;

if (pwmValue < 30 && pwmValue > 5) {

pwmValue = pwmValue + 30;

}

if (pwmValue <= 5) {

pwmValue = 0;

}

digitalWrite(motor_IN1, LOW);

analogWrite(motor_IN2, pwmValue);

}

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

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

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

Источник статьи

Принцип работы серводвигателя

Как сделать собственный серводвигатель

Энкодер AS5600 – магнитный датчик положения вращения

Схема серводвигателя

Необходимые компоненты

Проектирование печатной платы

Пользовательская 3D-модель сервопривода

Сборка серводвигателя

Программирование микроконтроллера

Прошивка загрузчика

Код загрузки

Исходный код программы

Похожие статьи

Добавить комментарий Отменить ответ

Добавить комментарий