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

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 469
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 144
Re: GPS-автопилот - алгоритъм
« Отговор #151 -: Април 17, 2020, 07:33:47 pm »
Диференциалната съставляваща в тази проргама няма да работи. Реално тя прави същото като пропорционалната - умножава последното отклонение, а не следи за стойности някъде по-назад, за да види тенденцията.
Можеш да работиш с лонг вместо флоат, просто коефициентите ти няма да са десетични числа, а нещо от 0 до 100. Накрая само ще разделиш оутпут-а на 100.

Неактивен EDM electronics

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

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


Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 144
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 »