Автор Тема: GPS-автопилот - алгоритъм  (Прочетена 42046 пъти)

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 946
Re: GPS-автопилот - алгоритъм
« Отговор #150 -: Април 17, 2020, 03:55:30 pm »
Намерих готов код за ПИД, но не ми харесва типа 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

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 745
Re: GPS-автопилот - алгоритъм
« Отговор #151 -: Април 17, 2020, 07:33:47 pm »
Диференциалната съставляваща в тази проргама няма да работи. Реално тя прави същото като пропорционалната - умножава последното отклонение, а не следи за стойности някъде по-назад, за да види тенденцията.
Можеш да работиш с лонг вместо флоат, просто коефициентите ти няма да са десетични числа, а нещо от 0 до 100. Накрая само ще разделиш оутпут-а на 100.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 946
Re: GPS-автопилот - алгоритъм
« Отговор #152 -: Април 17, 2020, 08:27:08 pm »
Ами освен да ползвам твоята формула за диференциалната, като записвам през определено време /примерно през 1 сек./ стойността на входа в масив може би и да смятам средното аритметично, т.е. ще добавя изменение само в реда за диференциалната.

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


Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 745
Re: GPS-автопилот - алгоритъм
« Отговор #153 -: Април 17, 2020, 10:41:26 pm »
Ето ти още един вариант, махнах някои проверки. Ако имаш въпроси - питай.

Накратко - записвам отклоненията в масив, и си взимам толкова колкото ми трябват (колко - задава се съответно от времето на интегралния и диференциалния коефициент 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;
      }
   }

};
« Последна редакция: Април 17, 2020, 11:09:31 pm от EDM electronics »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 946
Re: GPS-автопилот - алгоритъм
« Отговор #154 -: Декември 23, 2020, 09:46:49 pm »
Понеже се задават доста почивни дни и реших да отработя на практика ПИД-регулатора, но не се сещам с каква на практика регулираща система да е, че да е по-лесна и разбираема настройката. Примерно терморегулатор няма да е много лесно, защото е инертна. Горния клип с махалото е най-лесно, но нямам идея това серво с двунаправлена пружина ли е или някаква ба ли му лайката - стрелка. Както и да е, ще го мисля...

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;
      }
   };


« Последна редакция: Декември 23, 2020, 10:23:00 pm от EDM electronics »

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 745
Re: GPS-автопилот - алгоритъм
« Отговор #155 -: Декември 23, 2020, 11:50:19 pm »
Копирал съм кода от работещ контролер, в който съм правил доста промени.

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

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 946
Re: GPS-автопилот - алгоритъм
« Отговор #156 -: Декември 24, 2020, 10:11:49 pm »
Сега разбрах какво правиш, но не всичко. Записваш през определено време изменението на входа САМО в нулевия елемент на масива, като с цикъла преди това местиш данните от предходния елемент в един назад. Така придвижваш записа в нулевия елемент по целия масив с всяко изпълнение на цикъла. Обаче:
Не мога да разбера този запис, мисля, че имаш някаква грешка
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

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 745
Re: GPS-автопилот - алгоритъм
« Отговор #157 -: Декември 24, 2020, 10:58:21 pm »
Проблема не е в "моя телевизор"... във форума е :)
Като искаш да напишеш нещо наклонено (италик) във форума ползваш следните тагове:
[i] italic text [/i]

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