Уловки для управления двигателями постоянного тока

Приложения и онлайн-сервисы

IDE Arduino

Об этом проекте

Управление скоростью и направлением двигателей постоянного тока с помощью ПИД-регулятора и выходов ШИМ

Введение

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

(Пожалуйста, никогда не обвиняйте Близнецов. Независимо от того, что и когда вы хотели бы предпринять какие-либо действия с чем-то или без него, Близнецы немедленно приходят и принимают меры, просто чтобы помочь вам держать все под контролем. В то время как Инерция позволяет вещам «подумать», прежде чем действовать, Трение ограничивает их ускорение и скорость. А «мощность» - это «ничто», если она не находится под полным контролем.)

Таким образом, если вы попытаетесь управлять скоростью двигателя напрямую, отправив входной сигнал в виде сигнала ШИМ на выход, фактическая скорость никогда не будет соответствовать вашей уставке, и будет значительная разница (ошибка), как видно на изображении чуть выше. Здесь нам нужен другой способ, и он называется «ПИД-регулирование».

ПИД-регулятор

Что такое ПИД-регулирование? Представьте, как водить машину:чтобы начать движение с полной остановки, вам нужно нажать на педаль газа больше, чем во время обычного круиза. Во время движения с (почти) постоянной скоростью вам не нужно слишком сильно нажимать на педаль газа, вы просто восстанавливаете потерянную скорость, когда это необходимо. Кроме того, вы слегка отпускаете его, если ускорение выше, чем вам нужно. Это тоже способ «эффективного вождения».

Таким образом, ПИД-регулятор делает то же самое:Контроллер считывает разницу «Сигнал ошибки (e)» между уставкой и фактическим выходом. Он состоит из 3 различных компонентов, называемых «Пропорциональный», «Интегральный» и «Производный»; поэтому имя контроллера стоит после первой буквы каждого из них. Пропорциональная составляющая просто определяет крутизну (ускорение) выхода контроллера по отношению к фактическому сигналу ошибки. Интегральная часть суммирует сигналы ошибки во времени, чтобы минимизировать окончательную ошибку. А компонент Derivative следит за ускорением сигнала ошибки и ставит «корректировку». Я не буду приводить здесь дальнейшие подробности, пожалуйста, поищите их в Интернете, если вам интересно.

В моей программе Arduino ПИД-регулятор написан как функция, показанная ниже:

  float controllerPID (float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd) {float P, I, D; / * Базовая формула:U =_Kp * (_E + 0.5 * (1 / _Ki) * (_ E + _Eprev) * _ dT + _Kd * (_ E-_Eprev) / _ dT); * / P =_Kp * _E; / * Пропорциональная составляющая * / I =_Kp * 0.5 * _Ki * (_E + _Eprev) * _dT; / * Интегральный компонент * / D =_Kp * _Kd * (_E-_Eprev) / _dT; / * Производный компонент * / return (P + I + D);}  

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

  / * Сигнал ошибки, выход ПИД-регулятора и конечный выход (ШИМ) на двигатель * / E =RPMset - RPM; float cPID =controllerPID (E, Eprev, dT, Kp, Ki, Kd); if ( RPMset ==0) OutputRPM =0; иначе OutputRPM =OutputRPM + cPID; если (OutputRPM <_minRPM) OutputRPM =_minRPM;  

Цепь питания двигателя постоянного тока

Конечно, никогда не рекомендуется управлять двигателем постоянного тока напрямую с выхода Arduino или аналогичной платы управления. Двигатели постоянного тока требуют значительного количества тока по сравнению с теми, которые не могут подаваться через выходы плат контроллера. Значит нужно управлять катушками реле. Но здесь возникает другая проблема:в реле есть механические части, и они могут выйти из строя в среднесрочной или долгосрочной перспективе. Здесь нам понадобится еще один компонент - транзисторы.

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

После этого я решил использовать четыре биполярных переходных транзистора BC307A PNP на «мосту» для определения направления тока через катушки двигателя (на самом деле набор NPN BC337 будет работать лучше из-за способности выдерживать значительно более высокие токи коллектора, но я этого не сделал. у меня есть их в то время).

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

• Найдите « напряжение включения базы-эмиттера . ”( VBEon ) транзистора. Это минимальное напряжение, которое необходимо приложить к базе для включения транзистора.

• Найдите типичное значение « Коэффициент усиления постоянного тока . ”( hfe ) транзистора около тока коллектора, близкого к току двигателя. Обычно это соотношение между током коллектора ( IC ) и Базовый ток ( IB ), hfe =IC / IB .

• Найдите « Максимальный непрерывный ток коллектора . ”Транзисторов ( ICmax ). Постоянный ток двигателя никогда не должен превышать это значение в абсолютном выражении. Я могу использовать BC307, так как двигатель, который я использую, требует 70 мА, в то время как транзистор имеет ICmax (абс.) =100 мА.

Теперь вы можете определить номинал резистора, который будет подключен к базе:сначала вы должны учесть ограничения выхода вашей карты контроллера и попытаться сохранить базовый ток на минимальном уровне (поэтому рекомендуется выбирать усиление постоянного тока транзисторов как максимальное. по возможности. Примите номинальное напряжение выхода на плате контроллера как « Напряжение срабатывания ”( VT ) и найдите требуемый базовый ток ( IBreq ) разделив ток двигателя ( IM ) в коэффициент усиления постоянного тока ( hfe ) транзистора: IBreq =IM / hfe .

Затем определите напряжение, которое должно падать на резисторе ( VR ), вычитая напряжение включения базы-эмиттера ( VBEon ) из Напряжение срабатывания : VR =VT - VBEon .

Наконец, разделите напряжение на резистор . ( VR ) на Требуемый базовый ток ( IBreq ), чтобы найти номинал резистора . ( R ): R =VR / IBreq .

[Комбинированная формулировка: R =(VT - VBEon) * hfe / IM ]

В моем случае:

• Ток двигателя:IM =70 мА

• Параметры BC307A:ICmax =100 мА, hfe =140 (я измерил приблизительно), VBEon =0,62 В

• Напряжение срабатывания:VT =3,3 В (выход PWM Arduino Due)

• R =5360 Ом (поэтому я решил использовать 4900 Ом, создаваемые 2K2 и 2K7, чтобы обеспечить полный диапазон оборотов в минуту, и цепь просто отнимает ~ 0,6 мА от выхода PWM - подходящая конструкция.)

Обратное направление и важные примечания

Чтобы изменить направление вращения двигателя постоянного тока, достаточно изменить направление тока. Для этого мы можем просто создать мостовую схему с четырьмя наборами транзисторов. На схеме; ШИМ-выход №2 активирует T1A и T1B, а ШИМ-выход №3 активирует T2A и T2B, поэтому ток, проходящий через двигатель, изменяется.

Но здесь мы должны рассмотреть еще одну проблему:при запуске электродвигатели потребляют переходный пусковой ток, значительно превышающий номинальный ток, который вы читаете во время нормальной / непрерывной работы (производители указывают только номинальные токи). Пусковой ток может составлять примерно 130% от номинального для двигателей малой мощности и увеличивается в зависимости от мощности двигателя. Таким образом, если вы запитываете двигатель напрямую от источника напряжения и сразу же меняете полярность во время работы, двигатель потребляет слишком большой ток, поскольку он не остановлен полностью. В конечном итоге это может привести к перегоранию источника питания или сгоранию катушек двигателя. Это может быть не так важно и ощущаться для двигателей очень малой мощности, но становится важным, если уровни мощности, на которых вы работаете, увеличиваются. Но если вы включаете двигатель через транзистор или набор транзисторов (например, пара Дарлингтона), у вас нет такой проблемы, поскольку транзисторы уже ограничивают ток.

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

  if (Direction! =prevDirection) {/ * Отключение обоих выходов ШИМ для двигателя * / analogWrite (_chMotorCmdCCW, 0); analogWrite (_chMotorCmdCW, 0); / * Подождите, пока скорость двигателя не уменьшится * / do {RPM =60 * (float) readFrequency (_chSpeedRead, 4) / _ DiscSlots; } while (RPM> _minRPM); }  

Скорочтение

В своем приложении я использовал дешевый датчик скорости HC-020K. Он отправляет импульсы на уровне своего напряжения питания, а в даташите указано, что напряжение питания составляет 5 В. Однако моя плата - Arduino Due, и она не может ее принять. Я запитал его напрямую от выхода Due 3,3 В, и да, это сработало. И следующая функция написана для чтения частоты и вывода HC-020K.

  int readFrequency (int _DI_FrequencyCounter_Pin, float _ReadingSpeed) {pinMode (_DI_FrequencyCounter_Pin, INPUT); byte _DigitalRead, _DigitalRead_Previous =0; беззнаковый длинный _Time =0, _Time_Init; float _Frequency =0; если ((_ReadingSpeed ​​<=0) || (_ReadingSpeed> 10)) return (-1); иначе {_Time_Init =micros (); сделать {_DigitalRead =digitalRead (_DI_FrequencyCounter_Pin); если ((_DigitalRead_Previous ==1) &&(_DigitalRead ==0)) _Frequency ++; _DigitalRead_Previous =_DigitalRead; _Time =micros (); } while (_Time <(_Time_Init + (1000000 / _ReadingSpeed))); } return (_ReadingSpeed ​​* _Frequency); }  

Обратите внимание, что колесо HC-020K имеет 20 слотов, просто считайте частоту, которую нужно разделить на 20, чтобы получить число оборотов в секунду как частоту. Затем результат нужно умножить на 60, чтобы получить число оборотов в минуту.

  RPM =60 * (float) readFrequency (_chSpeedRead, 4) / _DiscSlots;  

Графические сенсорные элементы

Для отображения входных данных и результатов я использовал ЖК-экран Makeblock Me TFT и написал функцию CommandToTFT () для отправки на него команд. На самом деле причина этой функции - просто изменить точку последовательного подключения только в одной строке в программе, когда это необходимо.

Функции Cartesian_Setup (), Cartesian_ClearPlotAreas () и Cartesian_Line () написаны для подготовки области графического построения, очистки области построения при достижении конца горизонтальной оси (здесь это «время») и построения графиков соответственно. Обратитесь к руководству Makeblock Me TFT LCD для получения дополнительных сведений, если вам интересны графические функции здесь, потому что я не буду их здесь объяснять, поскольку они фактически выходят за рамки этого блога.

Окончание

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

Наконец, обычно вы не можете снизить скорость двигателя постоянного тока ниже 10-20% от его номинальной скорости, даже если нет нагрузки. Однако можно уменьшить почти до 5%, используя ПИД-регулирование для ненагруженного двигателя постоянного тока после его запуска.

Изменить (25 февраля 2018 г.): Если вы хотите использовать транзисторы NPN вместо PNP, вы должны также учитывать обратный ток в обоих типах. При срабатывании (включении) ток течет от эмиттера к коллектору на транзисторах PNP, но для типов NPN все наоборот (от коллектора к эмиттеру). Следовательно, транзисторы PNP поляризованы как E (+) C (-), а для NPN это должно быть C (+) E (-).

Код

• Код с графическими настройками
• Код без графических обновлений

Код с графическими сенсорными экранами Arduino

 / * ##################################################################### ## Цветовые константы для ЖК-дисплея Makeblock Me TFT ##################################################################### ###### * / # определить _BLACK 0 # определить _RED 1 # определить _GREEN 2 # определить _BLUE 3 # определить _YELLOW 4 # определить _CYAN 5 # определить _PINK 6 # определить _WHITE 7 / * ######## ############################################################################## Назначения ввода / вывода ####### ############################################## * / int _chSpeedSet =A0, // Скорость уставка _chKp =A1, // Чтение пропорционального коэффициента для ПИД-регулятора _chKi =A2, // Чтение интегрального коэффициента для ПИД-регулятора _chKd =A3, // Чтение производного коэффициента для ПИД-регулятора _chMotorCmdCCW =3, // Вывод ШИМ на двигатель для счетчика- поворот по часовой стрелке _chMotorCmdCW =2, // Вывод ШИМ на двигатель для поворота по часовой стрелке _chSpeedRead =24, // Показание скорости _chDirection =25; // Показание переключателя направления / * ####################################################################################### #### Другие константы ############################################################## ### * / #define _minRPM 0 // Минимальная частота вращения для инициирования изменения направления #define _maxRPM 6000 // Максимальный предел скорости вращения #define _Tmax 90 // Максимальный лимит времени для построения графиков #define _DiscSlots 20 // Кол-во слотов на индексном диске / * ############################################ Глобальные переменные ############################################### */Нить Cartesian_SetupDetails; boolean Direction, prevDirection; // Настройки сигнализации float RALL =500.0, RAL =1000.0, RAH =4000.0, RAHH =4500.0; float Seconds =0.0, prevSeconds =0.0, prevRPM =0.0, prevRPMset =0.0, RPM =0.0, RPMset =0,0, OutputRPM =0,0, Kp =0,0, Ki =0,0, Kd =0,0, Kpmax =2,0, Kimax =1,0, Kdmax =1,0, E =0,0, Eprev =0,0, dT =1,0; / * ###### ####################################### CommandToTFT (TFTCmd) Командная функция для Makeblock Me Входные параметры TFT LCD:(String) TFTCmd:Командная строка ##################################### ######### * / void CommandToTFT (String TFTCmd) {/ * Последовательное соединение, используемое для отображения * / Serial1.println (TFTCmd); delay (5);} / * ########### Конец CommandToTFT () ########### * // * ########### ################################# * // * ############ ################################## Декартова_настройка (Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2 , Window_Y2, MinDashQty, ColorF, ColorX, ColorY) Функция рисования декартовой оси XY для Makeblock Me TFT LCD Входные параметры:(float) Xmin, Xmax, Ymin, Ymax:значения диапазона оси (int) Window_X1, Window_Y1___:верхний левый угол окно графика (int) Window_X2, Window_Y2___:нижний правый угол окна графика (int) MinDashQty_____________:Кол-во штрихов на кратчайшей оси (int) ColorB, ColorX, ColorY:Цвета рисования для рамок, осей X и осей Y. Использование внешняя функция CommandToTFT (). ########################################################################## ### * / String Cartesian_Setup (float Xmin, float Xmax, float Ymin, float Ymax, int Window_X1, int Window_Y1, int Window_X2, int Window_Y2, int MinDashQty, int ColorF, int ColorX, int ColorY) {/ * Ограничения экрана * / const int Отображение Разрешение X =319, DisplayResolutionY =239; / * Ограничение строк заголовка * / String XminTxt; if (abs (Xmin)> =1000000000) XminTxt ="X =" + String (Xmin / 1000000000) + "G"; иначе, если (abs (Xmin)> =1000000) XminTxt ="X =" + String (Xmin / 1000000) + "M"; иначе, если (abs (Xmin)> =1000) XminTxt ="X =" + String (Xmin / 1000) + "K"; иначе XminTxt ="X =" + String (Xmin); String XmaxTxt; если (abs (Xmax)> =1000000000) XmaxTxt ="X =" + String (Xmax / 1000000000) + "G"; иначе, если (abs (Xmax)> =1000000) XmaxTxt ="X =" + String (Xmax / 1000000) + "M"; иначе, если (abs (Xmax)> =1000) XmaxTxt ="X =" + String (Xmax / 1000) + "K"; иначе XmaxTxt ="X =" + String (Xmax); String YminTxt; if (abs (Ymin)> =1000000000) YminTxt ="Y =" + String (Ymin / 1000000000) + "G"; иначе, если (abs (Ymin)> =1000000) YminTxt ="Y =" + String (Ymin / 1000000) + "M"; иначе, если (abs (Ymin)> =1000) YminTxt ="Y =" + String (Ymin / 1000) + "K"; иначе YminTxt ="Y =" + String (Ymin); String YmaxTxt; если (abs (Ymax)> =1000000000) YmaxTxt ="Y =" + String (Ymax / 1000000000) + "G"; иначе, если (abs (Ymax)> =1000000) YmaxTxt ="Y =" + String (Ymax / 1000000) + "M"; иначе, если (abs (Ymax)> =1000) YmaxTxt ="Y =" + String (Ymax / 1000) + "K"; иначе YmaxTxt ="Y =" + String (Ymax); / * Пределы * / int XminPx =Window_X1 + 1; int XmaxPx =Window_X2-1; int YmaxPx =Window_Y1 + 1; int YminPx =Window_Y2-1; / * Источник * / int OriginX =XminPx + (int) ((XmaxPx - XminPx) * abs (Xmin) / (abs (Xmax) + abs (Xmin))); int OriginY =YmaxPx + (int) ((YminPx - YmaxPx) * abs (Ymax) / (abs (Ymax) + abs (Ymin))); / * Кадр * / CommandToTFT ("BOX (" + String (Window_X1) + "," + String (Window_Y1) + "," + String (Window_X2) + "," + String (Window_Y2) + "," + String ( ColorF) + ");"); / * Ось X * / CommandToTFT ("PL (" + String (Window_X1 + 1) + "," + String (OriginY) + "," + String (Window_X2-1) + "," + String (OriginY) + ") , "+ String (ColorX) +"); "); / * Ось Y * / CommandToTFT ("PL (" + String (OriginX) + "," + String (Window_Y1 + 1) + "," + String (OriginX) + "," + String (Window_Y2-1) + ") , "+ String (ColorY) +"); "); / * Штрих:Минимальное количество штрихов задается параметром «MinDashQty» и будет штриховаться на самой короткой стороне оси относительно начала координат. На других участках наносимые штрихи должны определяться с учетом отношения к самой короткой стороне оси. * / / * Тире * / int XlengthLeft =abs (XminPx-OriginX); int XlengthRight =abs (XmaxPx-OriginX); int YlengthLower =abs (YminPx-OriginY); int YlengthUpper =abs (YmaxPx-OriginY); int XlengthLeft_Mod, XlengthRight_Mod, YlengthLower_Mod, YlengthUpper_Mod; если (XlengthLeft <=1) XlengthLeft_Mod =32767; иначе XlengthLeft_Mod =XlengthLeft; если (XlengthRight <=1) XlengthRight_Mod =32767; иначе XlengthRight_Mod =XlengthRight; если (YlengthLower <=1) YlengthLower_Mod =32767; иначе YlengthLower_Mod =YlengthLower; если (YlengthUpper <=1) YlengthUpper_Mod =32767; иначе YlengthUpper_Mod =YlengthUpper; int MinAxisLength =min (min (XlengthLeft_Mod, XlengthRight_Mod), min (YlengthLower_Mod, YlengthUpper_Mod)); int XdashesLeft =MinDashQty * XlengthLeft / MinAxisLength; int XdashesRight =MinDashQty * XlengthRight / MinAxisLength; int YdashesLower =MinDashQty * YlengthLower / MinAxisLength; int YdashesUpper =MinDashQty * YlengthUpper / MinAxisLength; int DashingInterval =2; // Мин. Интервал btw.dashes / * X-Dash L * / DashingInterval =(int) (XlengthLeft / XdashesLeft); if (! (DashingInterval <2)) for (int i =OriginX; i> =XminPx; i- =DashingInterval) CommandToTFT ("PL (" + String (i) + "," + String (OriginY-2) + " , "+ String (i) +", "+ String (OriginY + 2) +", "+ String (ColorX) +"); "); / * X-Dash R * / DashingInterval =(int) (XlengthRight / XdashesRight); if (! (DashingInterval <2)) for (int i =OriginX; i <=XmaxPx; i + =DashingInterval) CommandToTFT ("PL (" + String (i) + "," + String (OriginY-2) + ", «+ String (i) +», «+ String (OriginY + 2) +», «+ String (ColorX) +»); »); / * Y-Dash-L * / DashingInterval =(int) (YlengthLower / YdashesLower); if (! (DashingInterval <2)) for (int i =OriginY; i <=YminPx; i + =DashingInterval) CommandToTFT ("PL (" + String (OriginX-2) + "," + String (i) + ", «+ String (OriginX + 2) +», «+ String (i) +», «+ String (ColorY) +»); »); / * Y-Dash-U * / DashingInterval =(int) (YlengthUpper / YdashesUpper); if (! (DashingInterval <2)) for (int i =OriginY; i> =YmaxPx; i- =DashingInterval) CommandToTFT ("PL (" + String (OriginX-2) + "," + String (i) + " , "+ String (OriginX + 2) +", "+ String (i) +", "+ String (ColorY) +"); "); / * Расчет координат для отображения значений конечных точек оси * / int XminTxtX =Window_X1 - (int) (XminTxt.length () * 6) - 1, XminTxtY =OriginY, XmaxTxtX =Window_X2 + 1, XmaxTxtY =OriginY, YminTxtX =OriginX, YminTxtY =Window_Y2 + 1, YmaxTxtX =OriginX, YmaxTxtY =Window_Y1 - 12 - 1; / * Элементы управления:если какая-либо координата равна -1, она должна выходить за пределы отображения, и соответствующее значение не должно отображаться * / if (XminTxtX <0) XminTxtX =-1; если ((XminTxtY-12) <0) XminTxtY =-1; если ((XmaxTxtX + 6 * XmaxTxt.length ())> DisplayResolutionX) XmaxTxtX =-1; если ((XmaxTxtY + 12)> DisplayResolutionY) XmaxTxtY =-1; если ((YminTxtX + 6 * YminTxt.length ())> DisplayResolutionX) YminTxtX =-1; если ((YminTxtY + 12)> DisplayResolutionY) YminTxtY =-1; если ((YmaxTxtX + 6 * YmaxTxt.length ())> DisplayResolutionX) YmaxTxtX =-1; если (YmaxTxtY <0) YmaxTxtY =-1; / * Заголовки пределов диапазона * / if ((XminTxtX! =-1) &&(XminTxtY! =-1)) CommandToTFT ("DS12 (" + String (XminTxtX) + "," + String (XminTxtY) + ", '" + String (XminTxt) + "'," + String (ColorX) + ");"); if ((XmaxTxtX! =-1) &&(XmaxTxtY! =-1)) CommandToTFT ("DS12 (" + String (XmaxTxtX) + "," + String (XmaxTxtY) + ", '" + String (XmaxTxt) + " ', "+ String (ColorX) +"); "); if ((YminTxtX! =-1) &&(YminTxtY! =-1)) CommandToTFT ("DS12 (" + String (YminTxtX) + "," + String (YminTxtY) + ", '" + String (YminTxt) + " ', "+ String (ColorY) +"); "); if ((YmaxTxtX! =-1) &&(YmaxTxtY! =-1)) CommandToTFT ("DS12 (" + String (YmaxTxtX) + "," + String (YmaxTxtY) + ", '" + String (YmaxTxt) + " ', "+ String (ColorY) +"); "); / * Возвращаемое значение String Cartesian_Setup () вернет графическую конфигурацию упаковки строки в следующем формате:«» Строка начинается с '<' и заканчивается '>' . Каждое значение разделено символом ',' * / / * Initialize * / String Cartesian_SetupDetails ="<"; Cartesian_SetupDetails + =(Строка (Xmin) + ","); Cartesian_SetupDetails + =(Строка (Xmax) + ","); Cartesian_SetupDetails + =(Строка (Ymin) + ","); Cartesian_SetupDetails + =(Строка (Ymax) + ","); Cartesian_SetupDetails + =(Строка (Window_X1) + ","); Cartesian_SetupDetails + =(Строка (Window_Y1) + ","); Cartesian_SetupDetails + =(Строка (Window_X2) + ","); Cartesian_SetupDetails + =(Строка (Window_Y2) + ","); / * Close-Out * / Cartesian_SetupDetails + =">"; return Cartesian_SetupDetails;} / * ########### Конец декартовой_Setup () ########### * // * ######################################################################### * // * ###################################################################### Cartesian_ClearPlotAreas (Descriptor, Color) Функция сброса / очистки области графика для Makeblock Me TFT LCD Входные параметры:(String) Descriptor:Setup Descriptor - возвращается Cartesian_Setup () (int) Color______:Цвет, который будет использоваться для заполнения области графика Использует внешнюю функцию CommandToTFT (). ################################################################################ * / void Cartesian_ClearPlotAreas (дескриптор строки, int Color) {int X1, Y1, X2, Y2; / * Граничные координаты для областей графика * / / * Извлечение значений из дескриптора * / / * L [0] L [1] L [2] L [3] W [0] W [1] W [2] W [3] ] * / / * Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2 * / float L [4]; int W [4]; / * Значения хранятся в дескрипторе * / int j =0; / * Счетчик * / String D_Str =""; for (int i =1; i <=(Descriptor.length () - 1); i ++) if (Descriptor [i] ==',') {if (j <4) L [j] =D_Str.toFloat ( ); иначе W [j-4] =D_Str.toInt (); D_Str =""; j ++; } else D_Str + =Дескриптор [i]; / * Источник * / int OriginX =(W [0] +1) + (int) (((W [2] -1) - (W [0] +1)) * abs (L [0]) / ( абс (L [1]) + абс (L [0]))); int OriginY =(W [1] +1) + (int) (((W [3] -1) - (W [1] +1)) * abs (L [3]) / (abs (L [3 ]) + абс (L [2]))); / * Очистка участков графика * / //Area.1:X + Y + X1 =OriginX + 2; Y1 =W [1] + 1; X2 =W [2] - 1; Y2 =OriginY - 2; CommandToTFT ("BOXF (" + String (X1) + "," + String (Y1) + "," + String (X2) + "," + String (Y2) + "," + String (Color) + "); "); // Область 2:X- Y + X1 =W [0] + 1; Y1 =W [1] + 1; X2 =Origin X - 2; Y2 =OriginY - 2; CommandToTFT ("BOXF (" + String (X1) + "," + String (Y1) + "," + String (X2) + "," + String (Y2) + "," + String (Color) + "); "); // Область 3:X- Y- X1 =W [0] + 1; Y1 =OriginY + 2; X2 =Origin X - 2; Y2 =W [3] - 1; CommandToTFT ("BOXF (" + String (X1) + "," + String (Y1) + "," + String (X2) + "," + String (Y2) + "," + String (Color) + "); "); // Область 4:X + Y- X1 =OriginX + 2; Y1 =OriginY + 2; X2 =W [2] - 1; Y2 =W [3] - 1; CommandToTFT ("BOXF (" + String (X1) + "," + String (Y1) + "," + String (X2) + "," + String (Y2) + "," + String (Color) + "); ");} / * ########## Конец Cartesian_ClearPlotAreas () ########### * // * ############ ########################################### * // * # ############################################ Декартова линия (Xp, Yp, X, Y, Descriptor, Color) Функция декартовых линий для Makeblock Me TFT LCD Входные параметры:(int) Xp, Yp_____:Координаты предыдущего графика - значение y относительно x (int) X, Y_______:текущие координаты графика - значение y vs Дескриптор x (String):дескриптор установки - возвращается Cartesian_Setup () (int) Color______:Цвет маркировки, который будет использоваться на (x, y). Используется внешняя функция CommandToTFT (). ############# ################################ * / void Cartesian_Line (float Xp, float Yp, float X, float Y , String Descriptor, int Color) {/ * Извлечение значений из дескриптора * / / * L [0] L [1] L [2] L [3] W [0] W [1] W [2] W [3] * / / * Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2 * / float L [4 ]; int W [4]; / * Значения хранятся в дескрипторе * / int j =0; / * Счетчик * / String D_Str =""; for (int i =1; i <=(Descriptor.length () - 1); i ++) if (Descriptor [i] ==',') {if (j <4) L [j] =D_Str.toFloat ( ); иначе W [j-4] =D_Str.toInt (); D_Str =""; j ++; } else D_Str + =Дескриптор [i]; / * Источник * / int OriginX =(W [0] +1) + (int) (((W [2] -1) - (W [0] +1)) * abs (L [0]) / ( абс (L [1]) + абс (L [0]))); int OriginY =(W [1] +1) + (int) (((W [3] -1) - (W [1] +1)) * abs (L [3]) / (abs (L [3 ]) + абс (L [2]))); int XminPx =W [0] + 1; int XmaxPx =W [2] - 1; int YmaxPx =W [1] + 1; int YminPx =W [3] - 1; если (Y> L [3]) Y =L [3]; если (Y 
 =(OriginX-2)) &&(DispXp <=(OriginX + 2))) || ((DispYp>
 =(OriginY-2)) &&(DispYp <=(OriginY + 2) )) || ((DispX> =(OriginX-2)) &&(DispX <=(OriginX + 2))) || ((DispY> =(OriginY-2)) &&(DispY <=(OriginY + 2) )))) CommandToTFT ("PL (" + String (DispXp) + "," + String (DispYp) + "," + String (DispX) + "," + String (DispY) + "," + String (Color ) + ");");} / * ########### Конец декартовой_линии () ########### * // * ######## ##################################### * // * ####### ##################################### readFrequency (_DI_FrequencyCounter_Pin, _ReadingSpeed) Ввод функции чтения частоты Параметры:(int) _DI_FrequencyCounter_Pin:Цифровой вывод для чтения (с плавающей запятой) _ReadingSpeed____________:Пользовательская скорость чтения от 0 до 10 (Примечание 1) Примечание 1:_ReadingSpeed ​​- это значение, указывающее, как долго должны учитываться изменения. Это не может быть 0 (ноль), отрицательные значения или значение больше 10. При изменении _ReadingSpeed ​​1 секунда должна быть разделена на это значение для расчета необходимой продолжительности счета. Например; - _ReadingSpeed ​​=0,1 -> ввод должен считаться в течение 10 секунд (=1 / 0,1) - _ReadingSpeed ​​=0,5 -> ввод должен считаться в течение 2 секунд (=1 / 0,5) - _ReadingSpeed ​​=2,0 -> ввод должен считаться в течение 0,5 секунд (=1/2) - _ReadingSpeed ​​=4.0 -> ввод должен быть подсчитан в течение 0,25 секунды (=1/4). Важно отметить, что увеличение _ReadingSpeed ​​является недостатком, особенно на более низких частотах (обычно ниже 100 Гц), поскольку увеличивается ошибка подсчета. до 20% ~ 40% за счет уменьшения частоты. ##################################### ######## * / int readFrequency (int _DI_FrequencyCounter_Pin, float _ReadingSpeed) {pinMode (_DI_FrequencyCounter_Pin, INPUT); byte _DigitalRead, _DigitalRead_Previous =0; беззнаковый длинный _Time =0, _Time_Init; float _Frequency =0; если ((_ReadingSpeed ​​<=0) || (_ReadingSpeed> 10)) return (-1); иначе {_Time_Init =micros (); сделать {_DigitalRead =digitalRead (_DI_FrequencyCounter_Pin); если ((_DigitalRead_Previous ==1) &&(_DigitalRead ==0)) _Frequency ++; _DigitalRead_Previous =_DigitalRead; _Time =micros(); } while ( _Time <(_Time_Init + (1000000/_ReadingSpeed)) ); } return (_ReadingSpeed * _Frequency);}/* ########### End of readFrequency() ########### *//* ############################################## *//* ############################################### controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) PID Controller Function Input Parameters:(float) RangeMin:Minimum limit for output (float) RangeMax:Maximum limit for output (float) _E_____:Current error signal (float) _Eprev :Previous error signal (float) _dT____:Time difference as seconds (float) _Kp____:Proportional coefficient (float) _Ki____:Integral coefficient (float) _Kp____:Derivative coefficient Adjustment procedure:1. Set Kp=0, Ki=0, Kd=0. 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note critical gain Kc =Kp. 3. Adjust final coefficients as follows. for P-control only :Kp =0.50*Kc for PI-control only :Kp =0.45*Kc, Ki =1.2/Pc for PID-control :Kp =0.60*Kc, Ki =2.0/Pc, Kd=Pc/8 4. Fine tuning could be done by slightly changing each coefficient.############################################### */ float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd){ float P, I, D; /* Base Formula:U =_Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); */ P =_Kp * _E; /* Proportional Component */ I =_Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ D =_Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ return (P+I+D);}/* ########### End of controllerPID() ########### *//* ############################################## *//* ############################################### Setup############################################### */void setup(){ Serial1.begin(9600); Serial1.println("CLS(0);");delay(20); analogReadResolution(12); pinMode(_chDirection,INPUT); // Direction selector reading pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn // Initial killing the PWM outputs to motor analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); // Initial reading for direction selection Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW prevDirection=Direction; // The section below prepares TFT LCD // Cartesian_Setup(Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX, ColorY) Cartesian_SetupDetails =Cartesian_Setup(0, _Tmax, _minRPM, _maxRPM, 20, 20, 220, 120, 10, 0, 7, 7); CommandToTFT("DS12(250,10,'Dir:CW '," + String(_WHITE) + ");"); CommandToTFT("DS12(250,25,'____ Set'," + String(_YELLOW) + ");"); CommandToTFT("DS12(250,40,'____ RPM'," + String(_GREEN) + ");"); /* Alarm Values */ CommandToTFT("DS12(250,55,'AHH:" + String(RAHH) + "'," + String(_WHITE) + ");"); CommandToTFT("DS12(250,70,'AH :" + String(RAH) + "'," + String(_WHITE) + ");"); CommandToTFT("DS12(250,85,'AL :" + String(RAL) + "'," + String(_WHITE) + ");"); CommandToTFT("DS12(250,100,'ALL:"+ String(RALL) + "'," + String(_WHITE) + ");"); /* Alarm Window */ CommandToTFT("BOX(240,55,319,115," + String(_WHITE) + ");"); /* Alarm Lamps */ CommandToTFT("BOX(240,55,248,70," + String(_WHITE) + ");"); CommandToTFT("BOX(240,70,248,85," + String(_WHITE) + ");"); CommandToTFT("BOX(240,85,248,100," + String(_WHITE) + ");"); CommandToTFT("BOX(240,100,248,115," + String(_WHITE) + ");");}/* ############################################### Loop############################################### */void loop(){ // Initialization Time:Necessary for PID controller. int InitTime =micros(); // X-Axis Auto-Reset for Graphing if ( Seconds> 90.0 ) { Seconds =0.0; Cartesian_ClearPlotAreas(Cartesian_SetupDetails,0); } // Reading Inputs /* Controller Coefficients */ Kp =Kpmax * (float)analogRead(_chKp) / 4095; Ki =Kimax * (float)analogRead(_chKi) / 4095; Kd =Kdmax * (float)analogRead(_chKd) / 4095; /* Direction Selector */ Direction =digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ /* Actual RPM and RPM Setpoint Note that maximum selectable RPM is 5000. */ RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; RPMset =5000 * (float)analogRead(_chSpeedSet) / 4095; // Calculations and Actions /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ E =RPMset - RPM; float cPID =controllerPID(E, Eprev, dT, Kp, Ki, Kd); if ( RPMset ==0 ) OutputRPM =0; else OutputRPM =OutputRPM + cPID; if ( OutputRPM <_minRPM ) OutputRPM =_minRPM; if ( OutputRPM> _maxRPM ) OutputRPM =_maxRPM; /* Changing Direction when inverted Note that no any graphical indication is performed on this function. */ if ( Direction !=prevDirection ) { /* Killing both of the PWM outputs to motor */ analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); /* Wait until motor speed decreases */ do { RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } while ( RPM> _minRPM ); } // Writing Outputs if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); // Graphing /* Indicating Direction */ if (Direction==HIGH) CommandToTFT("DS12(280,10,'CCW '," + String(_WHITE) + ");"); else CommandToTFT("DS12(280,10,'CW '," + String(_WHITE) + ");"); /* Plotting Curve */ Cartesian_Line(prevSeconds, prevRPMset, Seconds, RPMset, Cartesian_SetupDetails, _YELLOW); Cartesian_Line(prevSeconds, prevRPM, Seconds, RPM, Cartesian_SetupDetails, _GREEN); /* Indicating values of RPM Setpoint, PID Controller Coefficients, Error Signal, PID Controller Output and Final RPM Output (PWM) */ CommandToTFT( "DS12(20,150,'Set:" + String(RPMset) + " rpm " + "RPM:" + String(RPM) + " rpm '," + String(_WHITE) + ");"); CommandToTFT( "DS12(20,170,'Kp=" + String(Kp) + " " + "Ki=" + String(Ki) + " " + "Kd=" + String(Kd) + " " + "dT=" + String(dT*1000) + " ms '," + String(_WHITE) + ");"); CommandToTFT( "DS12(20,190,'e=" + String(E) + " " + "cPID=" + String(cPID) + " " + "RPMout=" + String(OutputRPM) + " '," + String(_WHITE) + ");"); /* Resetting Alarm Lamps */ CommandToTFT("BOXF(241,56,247,69," + String(_BLACK) + ");"); CommandToTFT("BOXF(241,71,247,84," + String(_BLACK) + ");"); CommandToTFT("BOXF(241,86,247,99," + String(_BLACK) + ");"); CommandToTFT("BOXF(241,101,247,114," + String(_BLACK) + ");"); /* Activating Necessary Alarm Lamps */ if (RPM>=RAHH) CommandToTFT("BOXF(241,56,247,69," + String(_RED) + ");"); if ((RPM>=RAH)&&(RPMRALL)&&(RPM<=RAL)) CommandToTFT("BOXF(241,86,247,99," + String(_RED) + ");"); if (RPM<=RALL) CommandToTFT("BOXF(241,101,247,114," + String(_RED) + ");"); // Storing Values generated on previous cycle Eprev =E; prevRPMset =RPMset; prevRPM =RPM; prevSeconds =Seconds; prevDirection =Direction; // Calculating control application cycle time and passed Seconds dT =float ( micros() - InitTime ) / 1000000.0; Seconds+=dT; } 

Code without Graphical Touch-UpsArduino

/* ############################################### I/O Assignments############################################### */int _chSpeedSet =A0, // Speed setpoint _chKp =A1, // Proportional coefficient reading for PID controller _chKi =A2, // Integral coefficient reading for PID controller _chKd =A3, // Derivative coefficient reading for PID controller _chMotorCmdCCW =3, // PWM output to motor for counter-clockwise turn _chMotorCmdCW =2, // PWM output to motor for clockwise turn _chSpeedRead =24, // Speed reading _chDirection =25; // Direction selector reading/* ############################################### Other Constants ############################################### */#define _minRPM 0 // Minimum RPM to initiate direction changing#define _maxRPM 6000 // Maximum RPM limit#define _DiscSlots 20 // Qty of slots on Index Disc/* ############################################### Global Variables############################################### */boolean Direction, prevDirection;float RPM=0.0, RPMset=0.0, OutputRPM=0.0, Kp=0.0, Ki=0.0, Kd=0.0, Kpmax=2.0, Kimax=1.0, Kdmax=1.0, E=0.0, Eprev=0.0, dT=1.0;/* ############################################### readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed) Frequency Reading Function Input Parameters:(int) _DI_FrequencyCounter_Pin :Digital pin to be read (float) _ReadingSpeed____________:Custom reading speed between 0...10 (Note.1) Note.1:_ReadingSpeed is a value to specify how long shall the changes be counted. It cannot be 0(zero), negative values or a value greater than 10. When _ReadingSpeed changed, 1 second shall be divided by this value to calculate required counting duration. For example; - _ReadingSpeed =0.1 -> input shall be counted during 10 seconds (=1/0.1) - _ReadingSpeed =0.5 -> input shall be counted during 2 seconds (=1/0.5) - _ReadingSpeed =2.0 -> input shall be counted during 0.5 seconds (=1/2) - _ReadingSpeed =4.0 -> input shall be counted during 0.25 seconds (=1/4) Importantly note that, increasing of _ReadingSpeed is a disadvantage especially on lower frequencies (generally below 100 Hz) since counting error increases up to 20%~40% by decreasing frequency.############################################### */int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed){ pinMode(_DI_FrequencyCounter_Pin,INPUT); byte _DigitalRead, _DigitalRead_Previous =0; unsigned long _Time =0, _Time_Init; float _Frequency =0; if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); else { _Time_Init =micros(); do { _DigitalRead =digitalRead(_DI_FrequencyCounter_Pin); if ( (_DigitalRead_Previous==1) &&(_DigitalRead==0) ) _Frequency++; _DigitalRead_Previous =_DigitalRead; _Time =micros(); } while ( _Time <(_Time_Init + (1000000/_ReadingSpeed)) ); } return (_ReadingSpeed * _Frequency);}/* ########### End of readFrequency() ########### *//* ############################################## *//* ############################################### controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) PID Controller Function Input Parameters:(float) RangeMin:Minimum limit for output (float) RangeMax:Maximum limit for output (float) _E_____:Current error signal (float) _Eprev :Previous error signal (float) _dT____:Time difference as seconds (float) _Kp____:Proportional coefficient (float) _Ki____:Integral coefficient (float) _Kp____:Derivative coefficient Adjustment procedure:1. Set Kp=0, Ki=0, Kd=0. 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note critical gain Kc =Kp. 3. Adjust final coefficients as follows. for P-control only :Kp =0.50*Kc for PI-control only :Kp =0.45*Kc, Ki =1.2/Pc for PID-control :Kp =0.60*Kc, Ki =2.0/Pc, Kd=Pc/8 4. Fine tuning could be done by slightly changing each coefficient.############################################### */ float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd){ float P, I, D; /* Base Formula:U =_Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); */ P =_Kp * _E; /* Proportional Component */ I =_Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ D =_Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ return (P+I+D);}/* ########### End of controllerPID() ########### *//* ############################################## *//* ############################################### Setup############################################### */void setup(){ analogReadResolution(12); pinMode(_chDirection,INPUT); // Direction selector reading pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn // Initial killing the PWM outputs to motor analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); // Initial reading for direction selection Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW prevDirection=Direction;}/* ############################################### Loop############################################### */void loop(){ // Initialization Time:Necessary for PID controller. int InitTime =micros(); // Reading Inputs /* Controller Coefficients */ Kp =Kpmax * (float)analogRead(_chKp) / 4095; Ki =Kimax * (float)analogRead(_chKi) / 4095; Kd =Kdmax * (float)analogRead(_chKd) / 4095; /* Direction Selector */ Direction =digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ /* Actual RPM and RPM Setpoint Note that maximum selectable RPM is 5000. */ RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; RPMset =5000 * (float)analogRead(_chSpeedSet) / 4095; // Calculations and Actions /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ E =RPMset - RPM; float cPID =controllerPID(E, Eprev, dT, Kp, Ki, Kd); if ( RPMset ==0 ) OutputRPM =0; else OutputRPM =OutputRPM + cPID; if ( OutputRPM <_minRPM ) OutputRPM =_minRPM; if ( OutputRPM> _maxRPM ) OutputRPM =_maxRPM; /* Changing Direction when inverted */ if ( Direction !=prevDirection ) { /* Killing both of the PWM outputs to motor */ analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); /* Wait until motor speed decreases */ do { RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } while ( RPM> _minRPM ); } // Writing Outputs if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); // Storing Values generated on previous cycle Eprev =E; prevDirection =Direction; // Calculating control application cycle time and passed Seconds dT =float ( micros() - InitTime ) / 1000000.0;}

Схема