#include <Wire.h>                                 // Подключаем библиотеку для работы с аппаратной шиной I2C
#include <iarduino_I2C_Motor.h>                   // Подключаем библиотеку для работы с мотором I2C-flash
#include <iarduino_I2C_Bumper.h>                  // Подключаем библиотеку для работы с бампером I2C-flash
                                                  //
iarduino_I2C_Motor mot_R (0x0A);                  // Объявляем объект mot_R для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Motor mot_L (0x0B);                  // Объявляем объект mot_L для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Bumper bum(0x0C);                    // Объявляем объект bum  для работы с бампером I2C-flash, указав адрес модуля на шине I2C
                                                  //
float   speed   = -60;                             // Скорость машины в %.
float   kP      = 3 + 0.125*(speed-20);           // Коэффициент пропорциональной составляющей
float   kI      = 0.1;                            // Коэффициент интегральной составляющей
float   kD      = speed*speed/250;                // Коэффициент дифференциальной составляющей
float   ERR[10] = {0,0,0,0,0,0,0,0,0,0};          // Определяем массив последних ошибок
uint8_t i;                                        // Объявляем переменную, указывающую на элемент массива «ERR», который хранит текущую ошибку
                                                  //
void setup(){                                     //
     mot_R.begin();                               // Инициируем работу с левым  мотором I2C-flash
     mot_L.begin();                               // Инициируем работу с правым мотором I2C-flash
     bum.begin();                                 // Инициируем работу с бампером I2C-flash
     mot_R.setDirection(true);                    // Указываем правому мотору, что его вращение должно быть прямым (по часовой стрелке при положительных скоростях)
     mot_L.setDirection(false);                   // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
}                                                 //
                                                  //
void loop(){                                      //
           i++;     if( i>=10 ){ i=0; }           // Переходим к следующему элементу массива «ERR»
           ERR[i] = bum.getErrPID();              // Получаем текущую ошибку
     float SUM    = 0; for(auto j:ERR){SUM+=j;}   // Получаем сумму последних ошибок
     float P      = ERR[i]                 * kP;  // Определяем пропорциональную составляющую регулятора
     float I      = SUM                    * kI;  // Определяем интегральную составляющую регулятора
     float D      = (ERR[i]-ERR[(i+9)%10]) * kD;  // Определяем дифференциальную составляющую регулятора
     float PID    = P + I + D;                    // Получаем результат ПИД-регулятора
     mot_R.setSpeed( speed - PID, MOT_PWM );      // Устанавливаем скорость правого мотора
     mot_L.setSpeed( speed + PID, MOT_PWM );      // Устанавливаем скорость левого  мотора
}
