GPS-автопилот - алгоритъм

[EDM electronics]

Намерих готов код за ПИД, но не ми харесва типа float, по-добре да работя с цели числа и без това вече паметта е кът.

    // ГЛОБАЛЬНЫЕ ПЕРЕМЕННЫЕ
    // величины регулятора
     int setpoint = 0;   // заданная величина, которую должен поддерживать регулятор
     int input = 0;      // сигнал с датчика (например температура, которую мы регулируем)
     int output = 0;     // выход с регулятора на управляющее устройство (например величина ШИМ или угол поворота серво)
     int pidMin = 0;     // минимальный выход с регулятора
     int pidMax = 255;   // максимальный выход с регулятора

    // коэффициенты
     float Kp = 1.0;
     float Ki = 1.0;
     float Kd = 1.0;
     float _dt_s = 0.1; // время итерации в секундах

    // вспомогательные переменные
     int prevInput = 0;
     float integral = 0.0;

    // ПИД
    // функция расчёта выходного сигнала
      int computePID() {
      float error = setpoint - input;           // ошибка регулирования
      float delta_input = prevInput - input;    // изменение входного сигнала
      prevInput = input;
      output = 0;
      output += (float )error * Kp;                  // пропорционально ошибке регулирования
      output += (float )delta_input * Kd / _dt_s;    // дифференциальная составляющая
      integral += (float )error * Ki * _dt_s;        // расчёт интегральной составляющей
      // тут можно ограничить интегральную составляющую!
      output += integral;                           // прибавляем интегральную составляющую
      output = constrain(output, pidMin, pidMax);   // ограничиваем выход
      return output;
    }

Тук открих и едно видео за настройка на ПИД:

<a href="https://www.youtube.com/v/fusr9eTceEo" target="_blank" class="new_win">https://www.youtube.com/v/fusr9eTceEo</a>

[juliang]

Диференциалната съставляваща в тази проргама няма да работи. Реално тя прави същото като пропорционалната - умножава последното отклонение, а не следи за стойности някъде по-назад, за да види тенденцията.
Можеш да работиш с лонг вместо флоат, просто коефициентите ти няма да са десетични числа, а нещо от 0 до 100. Накрая само ще разделиш оутпут-а на 100.

[EDM electronics]

Ами освен да ползвам твоята формула за диференциалната, като записвам през определено време /примерно през 1 сек./ стойността на входа в масив може би и да смятам средното аритметично, т.е. ще добавя изменение само в реда за диференциалната.

Или как да стане според теб?

[juliang]

Ето ти още един вариант, махнах някои проверки. Ако имаш въпроси - питай.

Накратко - записвам отклоненията в масив, и си взимам толкова колкото ми трябват (колко - задава се съответно от времето на интегралния и диференциалния коефициент ti и td). Максимум 100 измервания назад. За някои променливи ползвам DINT, или long за ардуиното, вероятно ще трябва да си го смениш, щото е писано за данфоски контролер. Искам да избегна препълване на int-a...
Входа и изхода са от 0 до 1000, т.е. ако ще работиш с градуси можеш да си позволиш точност от 0.5 градуса - ще му даваш градусите умножени по 2, и ще делиш изхода на 2 за да станат пак градуси. Ако те устройв точност 1 градус, си работиш директно с градуси.

Махнал съм някои проверки, така че трябва да гарантираш че множителите Kp, Kd и Ki ще са от 0 до 100, както и че времената ti и td няма да са по-големи от 100.
Ще си викаш метода там когато решиш - предполагам че веднъж в секунда ще ти е достатъчно.

struct ClassicPID
{
    // public
    BOOL Enable;
    INT Setpoint;
    INT Input;
    INT Kp;
    INT Kd;
    INT Ki;
    INT Td;
    INT Ti;
    INT Output;


    // private
    INT errors[100];
    DINT integral;
    INT integralTime;
    INT derivative;
    INT derivativeTime;
    INT i;
    DINT DIntOutput;

    void Init()
    {
        DIntOutput = 0;
        integralTime = 0;
        derivativeTime = 0;
    }
   
    void Main()
    {
        for (i = 99; i > 0; i--)
        {
            errors = errors[i - 1]
        }
        errors[0] = Setpoint - Input;

      integralTime = Ti;
      integral = 0;
      for (i = 0; i < integralTime; i++)
      {
         integral = integral + errors * Ki;
      }
      integral = integral / integralTime;

      derivativeTime = Td;
       derivative = (errors[0] - errors[derivativeTime]) * Kd / derivativeTime;
      
      DIntOutput = DIntOutput + errors[0] * Kp + integral + derivative * 10; // * 10 може да се махне ако ПИД-а е много нервен
      if (DIntOutput < 0)
      {
         Output = 0;
         DIntOutput = 0;
      }
      else if (DIntOutput > 10000)
      {
         Output = 1000;
         DIntOutput = 10000;
      }
      else
      {
         Output = DIntOutput / 10;
      }
   }

};

[EDM electronics]

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

juliang , благодаря най-напред за примерния код, но да, ще имам въпроси и то не един, защото в него има неразбираеми неща. Или си допуснал грешка, или аз нещо не разбирам:

1. Защо ползваш структура и тая структура обхваща целия код /гледам скобата с точка и [/color]запетая си я поставил най-долу/, заедно с функцията мейн? - според мен структурата трябва да съдържа само различен тип променливи, не и оператори, функции, цикли, но и нямам идея защо въобще ползваш структура...

2. Не разбирам за какво е тая променлива BOOL Enable, като тя не се ползва въобще в кода, явно ти е остатък от програмата и служи за друго?

3. Функцията void Init() това сетапа ли е на твоя компилатор или е някаква функция, на която не виждам прототип в мейна?

4. Не ми е ясно предназначението на променливите  INT Td; и   INT Ti;? - те не участват в сметките, защо им присвояваш стойности.

5. Не ми е ясно в първия цикъл на мейн, как присвояваш стойности на всяка една променлива на масива? Според мен ти само правиш запис на броя на променливите в масива по адрес, но не им присвояваш стойност, т.е. по дефолт всички те приемат нулева стойност, а вече извън цикъла присвояваш стойност само на първата променлива от него еrrors[0] = Setpoint - Input;
Как присвояваш стойности на останалите 99 променливи?
После и другия въпрос: тия стойности на масива не са ли стойностите в изхода и не трябва ли да се записват през определено време?

Много са въпросите, но няма как да повторя кода без да съм разбрал как работи. Аз обикновено пиша коментар на всеки ред, защото дори след не дълго време не мога да си го разчета така лесно . Да, губя така повече време, но един ден, като отворя, се ориентирам много бързо, особено като е по-дълъг кода.

struct ClassicPID
{
    // public
    BOOL Enable;
    INT Setpoint;
    INT Input;
    INT Kp;
    INT Kd;
    INT Ki;
    INT Td;
    INT Ti;
    INT Output;


    // private
    INT errors[100];
    DINT integral;
    INT integralTime;
    INT derivative;
    INT derivativeTime;
    INT i;
    DINT DIntOutput;

    void Init()
    {
        DIntOutput = 0;
        integralTime = 0;
        derivativeTime = 0;
    }
   
    void Main()
    {
        for (i = 99; i > 0; i--)
        {
            errors = errors[i - 1]
        }
        errors[0] = Setpoint - Input;

      integralTime = Ti;
      integral = 0;
      for (i = 0; i < integralTime; i++)
      {
         integral = integral + errors * Ki;
      }
      integral = integral / integralTime;

      derivativeTime = Td;
       derivative = (errors[0] - errors[derivativeTime]) * Kd / derivativeTime;
      
      DIntOutput = DIntOutput + errors[0] * Kp + integral + derivative * 10; // * 10 може да се махне ако ПИД-а е много нервен
      if (DIntOutput < 0)
      {
         Output = 0;
         DIntOutput = 0;
      }
      else if (DIntOutput > 10000)
      {
         Output = 1000;
         DIntOutput = 10000;
      }
      else
      {
         Output = DIntOutput / 10;
      }
   };

[juliang]

Копирал съм кода от работещ контролер, в който съм правил доста промени.

1. Такъв е синтаксиса на този тип контролери. А и на езика Си, на който се пишат програмите. Struct-а е... как да го кажа ... обект, нещо. Това нещо си носи както променливите, така и логиката. в една програм може да има много "неща", много "обекта". Самия контролер при захранваането последователно извиква функциите Init на всички обекти в прогамата, и после циклично извиква последователно всички Main методи на обектите. Така функционира... не ме питай защо, вероятно щото голяма част от кода се генерира автоматично и е по-лесно.

2. Enable е мой пропуск - трябва да го допиша в логиката. Всеки един "блок", или "обект" трябва да има тази променилва и ако тя е False, това означава че контролера е спрян. Тогава почти всички блокове трябва да спрат да работят, остават само тези които следят сензорите или бутоните, с които контролера да се пусне отново. Не че е грешка, но е признак на недоглждане Просто в началото на Main метода трябва да допиша "if !Enable then return;" и блока няма да изпълнява нищо.

3. Да, тази функция се изпълнява еднократно при захранването на контролера.

4. Тук ... исках да се подсигуря. Td и Ti са променливи, които са записани в енергонезависимата памет на контролера -  те се задават от потребителя чрез менюто и се запомянт дори и тока да спре. Не съм сигурен дали програмно мога да правя нещо друго освен да ги прочитам, не знам дали мога да ги ползвам в изчисленията, така че просто копирам стойността им в integralTime и derivativeTime и вече работя с тях.

5. В началото всички елементи на масива са 0. При всяко изпълнение на Main-а аз обхождам масива, като в 99-та клетка слагам стойността на 98-а, в 98-а стойността на 97-а и т.н. т.е. "придвижвам" масива с една клетка надясно. Накрая поставям новата стйност в клетка нула. Малко бавно става, но процесите които управлявам са още по-бавни. Един цикъл на цялата програма - не само контролера, а целия проект - се изпълнва за около 0.15 секунди, което е много-много по-бързо отколкото ми трябва.

6. В масива се съхраняват отклоненията на входа от зададената стойност. Трябват ми за да мога да анализирам "накъде вървят нещата", т.е. колко бързо се променя входа или колко дълго време съм бил под или над исканаат стойност.
Изхода е само един - Output. Тъй като тоя контролер работи изключително тежко с числа с плаваща запетая, работя с цели числа и чак на края ги деля на 10. По-голяма точност не ми трябва - специално тои контролер го ползвам за управление на температури на вода, така че стойностите са от 0 до 100 градуса макс. Изхода на контролера е от 0 до 1000, т.е. имам разделителна способност от 0.1 градуса, което е повече от достатъчно.

Като цяло съм доста спънат от самия контролер и другите блокове в него, които идват от производителя и трябва да спазвам "добрия тон" като се пъхам между тях. Почти всички заводски блокове работят със стойности от 0 до 1000 и е безмислено да работя с отрицателни числа или с числа по-големи от 1000 - ианче ще трябва да ги конветирам в нещо разбираемо за другите блокчета. Дори входовете и изходите ми са мащабирани за стойности от 0 до 1000, което при вх/изх 0-10 волта е 0.01 волт - повече от достатъчно като точност. Помпата с 3 000 об/мин я упрявлявам с точност 3 об/мин, което е безполезно точно, една моторна задвижка с вход 0-10 волта и обхват 0-90 градуса да искам от нея точност 0.1 ъглови градуса положение също е несериозно.

[EDM electronics]

Сега разбрах какво правиш, но не всичко. Записваш през определено време изменението на входа САМО в нулевия елемент на масива, като с цикъла преди това местиш данните от предходния елемент в един назад. Така придвижваш записа в нулевия елемент по целия масив с всяко изпълнение на цикъла. Обаче:
Не мога да разбера този запис, мисля, че имаш някаква грешка
for (i = 99; i > 0; i--)
        {
            errors = errors[i - 1]
        }

Мисля, че не можеш така да представиш масива само с името му errors, без скобите и броя на елементи му или пък само един елемент записан вътре в него. Или вместо броя поне променлива, примерно i.

Така както си го записал няма да ти мести стойността, да не би записа да е правилно така:
errors [ i ] = errors[i - 1]


Тук виждам същото при четенето на масива:

for (i = 0; i < integralTime; i++)
      {
         integral = integral + errors * Ki;
      }


Не трябва ли да е така:
integral = integral + errors [ i ] * Ki;

[juliang]

Проблема не е в "моя телевизор"... във форума е
Като искаш да напишеш нещо наклонено (италик) във форума ползваш следните тагове:

Код: [Маркирай кода]

[i] italic text [/i]

и ... кат съм пейстнал кода, форума е изял това в скобите ...

[EDM electronics]

struct ClassicPID
{
    // public
    BOOL Enable;
    INT Setpoint;
    INT Input;
    INT Kp;
    INT Kd;
    INT Ki;
    INT Td;
    INT Ti;
    INT Output;


    // private
    INT errors[100];
    DINT integral;
    INT integralTime;
    INT derivative;
    INT derivativeTime;
    INT i;
    DINT DIntOutput;

    void Init()
    {
        DIntOutput = 0;
        integralTime = 0;
        derivativeTime = 0;
    }
   
    void Main()
    {
        for (i = 99; i > 0; i--)
        {
            errors = errors[i - 1]
        }
        errors[0] = Setpoint - Input;

      integralTime = Ti;
      integral = 0;
      for (i = 0; i < integralTime; i++)
      {
         integral = integral + errors * Ki;
      }
      integral = integral / integralTime;

      derivativeTime = Td;
       derivative = (errors[0] - errors[derivativeTime]) * Kd / derivativeTime;
      
      DIntOutput = DIntOutput + errors[0] * Kp + integral + derivative * 10; // * 10 може да се махне ако ПИД-а е много нервен
      if (DIntOutput < 0)
      {
         Output = 0;
         DIntOutput = 0;
      }
      else if (DIntOutput > 10000)
      {
         Output = 1000;
         DIntOutput = 10000;
      }
      else
      {
         Output = DIntOutput / 10;
      }
   }

};

Юлиане, да те похваля,
обясненията ти за разбиране работата на ПИД в по-преден пост са отлични и другото отличаващо се в твоя код е използването на масив за запис на повече от една предходна стойност, което дава по добра оценка на процеса в по-дълъг период от време. В повечето ПИД-функции които съм гледал се взема само предходното значение за сравнение и ако има само пик, а не някаква тенденция, то тоя регулатор няма да работи добре при всички условия. Има обаче нещо липсващо в кода ти - това е времето /честотата на дискретизация/. Дал си integralTime, но това е само някаква променлива - число, не е време. После, като четеш интегралната съставна е неразбираемо, защо integralTime го пъхаш в цикъла за четене. Според мен трябва да четеш броя на масива - да поставиш 99.
for (i = 0; i < integralTime; i++)
      {
         integral = integral + errors * Ki;
      }

Според мен трябва грешката /разликата между зададената и измерената стойност errors[0] = Setpoint - Input/ да се извършва с някакъв период в зависимост от инертността на системата. В твоя код не виждам таймер и така, както си го написал означава, че ще се прави запис при всяка итерация на основния уайл?

Аз бих ползвал софтуерен таймер, който няма блокираща функция. При мен системата е изключително инертна. За да отчета някакво изменение на позицията от дадена GPS-точка, измервам разстоянието от тази запомнена точка до новата, на която ще ме отнесе течението. В случая ще регулирам оборотите на двигателя, който ще ме връща в запаметената точка в зависимост от вятъра и течението. Колкото по-малко течение, толкова по-ниски обороти и обратно. Затова трябват поне 10 сек за всяко следващо измерване, иначе няма смисъл, защото за по-малко време стойностите ще са много близки по значение. Примерно 10 записа са ми достатъчни през 10 сек или за 100 сек ще имам пълна картина на условията - има ли постоянно течение или само пикове-пориви и колко силно е то.

Какво ще кажеш за времето на дискретизацията?

Къде в твоя код определяш времето за всяко измерване на сетпойнта?

[juliang]

Контролера на Данфос има една функция, която ми показва за колко време се върти един Main цикъл. В моя случай е някъде около 150 милисекунди, или около 7 пъти в секундата. Затова и се боричкам в края на цикъла да деля на 100 или на 1 000, щото и при мен системите в повечето случаи са инертни.
Най-лесниото решение е в началото на Main-а да имаш един брояч, който да изхвърля логиката в 9 от 10 преминавания, или там колкото ти трябват за да се постигне желаната инертност.
И имай предвид че това което съм постнал е само библиотечката за ПИД-а. програмата върши още една камара неща, поне при мен - рисува по екрана, чете сензори и бутони... става доста досадно когато трябва да задържиш бутона за спиране в продължение на една-две секунди за да може loop-а да мине през проверката за натиснат бутон...
Да, интегралното време е бройка преминавания през дадената библиотека, а не време като секунди. Същото е и с деривативното време - и то е брой цикли, а не секунди. Но когато знаеш за какво време програмата прави един loop, можеш да си сметнеш секундите... не че ти трябват точно секунди.

Защо не чета 99 стойности назад? Ами... ако пробваш алгоритъма ще разбереш. Понякога 90 от тия 99 записа сочат, че си бил под исканата стойност, а само последните 10 ти указват, че вече си я надхвърлил. А ти си я надхвърлил отдавна, и с много... а интегралната тегли още и още нагоре. В тоя случай интегралната компонента прави системата ужасно нестабилна. Особено болезнен е цирка когато системата стартира и разликата между реалната и зададената стойност е много голяма.
Но пък имам и случаи, в който трябва с помпа да поддържам налягане в една тръбна система, пълна с вода. Система без разширителен съд. И тогава интегралната компонента на практика ми е абсолютно ненужна, щото промяната в налягането на системата става с добавянето буквално на 2 шепи вода Чист пропорционален алгоритъм, без никакви предвиждания и истории на процеса.

На практика интегралната компонента е много рядко приложима в инертни системи, които не изискват голяма точност. Там голяма роля играе деривативната - на теб ти е много важно да знаеш накъде се е запътил процеса, а не къде е бил, за да можеш да вземеш превантивни мерки преди да си се олял в корекцията.

[EDM electronics]

1. Контролера на Данфос има една функция, която ми показва за колко време се върти един Main цикъл.
Най-лесниото решение е в началото на Main-а да имаш един брояч, който да изхвърля логиката в 9 от 10 преминавания, или там колкото ти трябват за да се постигне желаната инертност.

2. Особено болезнен е цирка когато системата стартира и разликата между реалната и зададената стойност е много голяма.

При STM32 има една библиотека HAL, която подобно на Ардуино предлага готови функции при апаратното програмиране и се ползва масово. Та там има една функция HAL_GetTick(), която представлява един брояч с точен период от 1 mS, независимо как ще настроиш честотата на контролера. Винаги тая функция дава 1 mS. Това е един 32-битов регистър-брояч, който се свързва на системния брояч-регистър на Систика, като се взима извод от пин с период 1 mS. В Ардуино същата функция е milis(). С тази функция се правят всички софтуерни таймери. Те са такива с не голяма точност. Ако трябват таймери с точност се правят вече апаратни. Пример:

unsigned long T;

if(HAL_GetTick() - T > 10) {   //ако разликата между брояча и променливата е по-голяма от 10 mS
T = HAL_GetTick();                //приравняваме стойността на брояча

изпълняваме нужния код или вдигаме флаг
на всеки 10 mS
}

Така могат да се реализират безброй таймери само с една променлива и за всякакво време > 1mS. Таймера няма блокираща функция.


2. Няма ли тоя Данфос външно прекъсване? Да чакаш секунди за бутон ми се струва много голям компромис?


Гледах един клип как се настройва ПИД в реални условия най-лесно:
Закача се датчика на входа или променливата която ще се мери - при мен е разстояние между 2 точки. В плотера на компилатора /повечето съвременни имат/ се поставят три променливи: вход, изход и грешка. На графиката се изписват трите графики една върху друга и се вижда има ли колебания, за колко време се стабилизира процеса и най-важното съвпада ли сетпойнта с грешката, трябва да е равно на нула. Така без практически опити кой знае какви може да се настроят коефициентите.

[juliang]

Гледах един клип как се настройва ПИД в реални условия най-лесно:
Закача се датчика на входа или променливата която ще се мери - при мен е разстояние между 2 точки. В плотера на компилатора /повечето съвременни имат/ се поставят три променливи: вход, изход и грешка. На графиката се изписват трите графики една върху друга и се вижда има ли колебания, за колко време се стабилизира процеса и най-важното съвпада ли сетпойнта с грешката, трябва да е равно на нула. Така без практически опити кой знае какви може да се настроят коефициентите.

И аз гледах клипове. Гледах и как се справят алгоритмите за адаптация на контролера на горелките (на Сименс, дет се занимават десетилетия с такива неща). Гледах как се справя с адаптацията алгоритъма на едни контролери KS 20 на West. В половината от случаите успява да се справи горе-долу, в другите случаи резултата е посредствен. Налага се да се дооправят коефициентите на ръка.
да оправиш коефициентите с чертане на графики... няма как да стане.

[EDM electronics]

1. Да, интегралното време е бройка преминавания през дадената библиотека, а не време като секунди. Същото е и с деривативното време - и то е брой цикли, а не секунди. Но когато знаеш за какво време програмата прави един loop, можеш да си сметнеш секундите... не че ти трябват точно секунди.

2. Защо не чета 99 стойности назад? Ами... ако пробваш алгоритъма ще разбереш. Понякога 90 от тия 99 записа сочат, че си бил под исканата стойност, а само последните 10 ти указват, че вече си я надхвърлил.

Останаха неясноти - важни:
1. Според твоето обяснение integralTime и derivativeTime това не е някакво време, а броя на измерванията или броя на елементите на масива, т.е. делиш на техния брой? В други обяснения на ПИД от Интернет този делител е наистина време и това е времето на дискретизация, т.е. времето между две измервания на входа. От твоите теоретични обяснения обаче схващам, че това са броя измервания.

2. Ако ползваш само 10 измервания от масива, защо тогава правиш запис на 100? Да го разбирам ли, че останалите 90 ще ползваш, ако решиш да промениш параметрите на ПИДа при някакви други условия?

[juliang]

1. Двата параметъра са на практика броячи - променят се с единица при всяко преминаване през Main метода. Да, ако трябва да сме съвсем точни - трябва да са секунди. Но от друга страна - защо точно секунди? Може да са стотни от минутата. А при бързи процеси може да са милисекунди. При бавни - може да са минути или часове дори... За какво ти е измерване в секунди, ако правиш робот който примерно играе тенис на маса? Тук със секунди ще се оправиш ли?

<a href="https://www.youtube.com/v/7Jw8m4pbTYI" target="_blank" class="new_win">https://www.youtube.com/v/7Jw8m4pbTYI</a>

2. Да. Kp, Ki и Kd са свободно избираеми от потребителя и се променят на място, като се следи поведението на цялата система и реакцията на изпълнителния механизъм.

Единия случай и го обясних - трбопровод, и една помпа трябва да реагира на отваряне на кран и да възстанови налягането. На практика реакцията трябва да е моментална - и на отваряне, и на затваряне на кран.
Втория случай е управление на горелката на отоплителен котел в голям хотел в Боровец. Като запалиш котела, топлата вода трябва да отиде на 100 метра настрани и на 10 етажа нагоре, и после да се върне обратно. Това отнема около 5 минути. Горелката се управлява по температура на връщащата се вода. Т.е. ти чак след 5 минути можеш да разбереш дали мощността ти е достатъча, дали трябва да добавиш още или вече си ощавил гостите Тогава горелката мърда с 1% мощност на 10-20 секунди, и Kd е огромна цифра - дори и най-малкия намек за повишаване на температурата, дори и да не е достигнало заданието трябва да е сигнал за алгоритъма управляващ горелката да забави хода на мощността й нагоре, или дори да почне да се връща към по-ниски мощности. Тоест Kd трябва да е достатъчно голям, за да може да се противопостави на пропорционалния Kp, който настоява мощността да се вдига, щото температураат е с 20 градуса по-ниска от заднието. 20 градуса по-ниска, ама на връщането... няма как да знаеш дали в хотела има отворен 1 радиатор, или са се прибрали 500 замръзнали скиора, които са отворили всички радиатори, пуснали са душове, вани и квот се сетиш А е изключитлно дискомфортно ако има отворен 1 душ и 1 радиатор, ти да пуснеш 1 мегават котел на 100% и да пратиш вода с температура 95 градуса към нещастния един гост на хотела

А, да... за интегралната част... тя в много от случаите е безполезна, ако не гониш някаква точност като тази на видеото. нейната единствена цел е да коригира натрупаните с времето грешки в измерването ил ив изпълнението на командите. Тоест ти искаш да вървиш точно на 43.276 градуса, но руля може да даде 43 или 44 градуса. Измервателното устройство отчита че има малка грешка, но Kp е достатъчно малък за да повлияе на руля. В тоя случай в Ki ще се натрупва постоянна грешка която в един момент ще доведе до отклоняване на руля въпреки че Kp не е показал нужда от това.

[EDM electronics]

Направих от това, което съм разбрал функция ПИД с десет елемента от масив. Считам, че е достатъчно. Функцията връща резултата и ще се вика от таймер с време в зависимост от инертността на системата. В моя случай ще я викам на 5 сек. Така ще прави запис в масива за 50 сек - достатъчно дълго време за оценка на тенденцията.


int PID_regulator(float input, float setpoint, float kp, float ki, float kd, int minOut, int maxOut) {
 
  static float errors[10];                                          //10 елемента за запис на входните числа
  float integral = 0;                                                 //интегрална съставна
  float derivative = 0;                                             //дефиринциална съставна
  static int Output = 0;                                           //изход - връща значение

  errors[0] = setpoint - input;                                     //текущо значение и пропорционална съставна
  for(int i = 9; i > 0; i--) errors[ i ] = errors[i - 1];     //преместваме стойността на errors[0] по масива
  for(i = 0; i < 9; i++) integral += errors[ i ] * ki;     //прочитаме записите в масива, събираме ги
  integral /= 10;                                                         //делим на броя елементи от масива
  derivative = (errors[0] - errors[9]) * Kd / 10;          //деферинциална съставна
  Output = errors[0] * kp + integral + derivative;       //събираме трите съставни в изхода
  if(Output < minOut) Output = minOut;                    //ограничаване по минимум
  else if(Output > maxOut) Output  = maxOut;          //ограничаване по максимум
  else Output;                                                              //четем изхода без ограничения
  return Output;                                                          //връщаме значението от изхода
 }