Сопряжение IMU датчика с сервой посредством PID регулятора

Добрый день.

Стоит задача создать 2 осевой подвес.

Есть IMU тройка модуль. Он расположен на конструкции угловое положение которой задается двумя серво машинками.

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


В теории все просто.

Считываем углы с IMU датчика. Выполнено.
С помощью ПИД регулятора выдаем управляющий шим сигнал на сервомашинку. Вот тут проблема.
Код у меня получился следующий.

Код (C++):

    //D к A4
    //C к A5
    #include <PID_v1.h>      // библиотека для работы PID
    #include <Wire.h>       // библиотека для работы I²C
    #include <TroykaIMU.h>  // библиотека для работы с модулями IMU
    #include <Servo.h>
 
    #define BETA 0.22f      // множитель фильтра
   
    Madgwick filter;      // создаём объект для фильтра Madgwick
   
    Accelerometer accel;       // создаём объект для работы с акселерометром

    Gyroscope gyro;      // создаём объект для работы с гироскопом
   
    float gx, gy, gz, ax, ay, az; // переменные для данных с гироскопов, акселерометров
   
    float pitch, roll; // получаемые углы ориентации
   
    float fps = 100; // переменная для хранения частоты выборок фильтра

    double Setpoint, Input, Output;

    Servo myservo;//серво привод

    PID myPID(&Input, &Output, &Setpoint,1,4,3, DIRECT); // создания пид регулятора
 
    void setup() ///////////////////////////////////////////////////////////////////////////////////////////////////////
    {
   
      Serial.begin(115200);   // открываем последовательный порт
      Serial.println("Begin init...");  // инициализация акселерометра
   
      accel.begin();  // инициализация гироскопа
   
      gyro.begin();  // выводим сообщение об удачной инициализации
   
      Serial.println("Initialization completed");
   
      Input = pitch;
      Setpoint = 0;
      myPID.SetMode(AUTOMATIC);
   
    }
   
    void loop() //////////////////////////////////////////////////////////////////////////////////////////////////////
    {
      // запоминаем текущее время
      unsigned long startMillis = millis();
   
      // считываем данные с акселерометра в единицах G
      accel.readGXYZ(&ax, &ay, &az);
      // считываем данные с акселерометра в радианах в секунду
      gyro.readRadPerSecXYZ(&gx, &gy, &gz);
      // устанавливаем коэффициенты фильтра
      filter.setKoeff(fps, BETA);
      // обновляем входные данные в фильтр
      filter.update(gx, gy, gz, ax, ay, az);
   
      // получение углов yaw, pitch и roll из фильтра
      pitch = filter.getPitchDeg();
      roll = filter.getRollDeg();
   
      // выводим полученные углы в serial-порт
      Serial.print("\t\t");
      Serial.print("pitch: ");
      Serial.print(pitch);
      Serial.print("\t\t");
      Serial.print("roll: ");
      Serial.println(roll);
   
      // вычисляем затраченное время на обработку данных
      unsigned long deltaMillis = millis() - startMillis;
      // вычисляем частоту обработки фильтра
      fps = 1000 / deltaMillis;

      Input = pitch;
      myPID.Compute();
      myservo.attach(9,600,2300);
      myservo.write(Output);
 
    }

 

В примерах что я находил, например климат контроль, входной сигнал (Input) был простым аналогом приходящим на физический пин ардуино. У меня же входной сигнал формируется внутри программы (pitch). Как в ПИД регуляторе указать что за входной сигнал нужно брать не сигнал с пина, а обработанное значение с IMU датчика?

 

То, что вы спрашиваете - элементарно, поменяйте в заголовке PID() &Input на &pitch.

Только мне кажется. что от решения вы очень далеки, судя по совершенно ламерскому вопросу.
С чего вы взяли что этот ПИД возьмет сигнал с датчика и будет выдавать команду сервам в понятном им формате? В каком месте программы вы задаете PID-функцию для преобразования pitch в сигнал для серво? Или вы просто взяли код для климат-контроля в инете - и думаете он для подвеса подойдет?