Установка бампера с 9 датчиками линии на робота "Малыш"

Введение:

В этом уроке мы установим на робота «Малыш» бампер с 9 датчиками линии, и загрузим скетч, позволяющий роботу ехать по линии. 

Нам понадобится:

• Робот «Малыш» в сборе x1 шт.
• Бампер с 9 датчиками линий с шагом 14мм., FLASH-I2C x1 шт.


Также понадобится установить библиотеку:
• iarduino_I2C_Bumper

При необходимости, ознакомьтесь с нашей инструкцией по установке библиотек в Arduino IDE.

Видео:

Установка бампера

Если на вашем роботе установлены датчики линии, снимите их. Все остальные подключения оставьте без изменений!

При помощи винтов и гаек устанавливаем бампер на Малыша. Бампер подключается к Motor Shield по шине I2С.

Скетч

// Подключаем библиотеки:                                  //
#include <Wire.h>                                          // * Подключаем библиотеку для работы с аппаратной шиной I2C.
#include <iarduino_I2C_Bumper.h>                           //   Подключаем библиотеку для работы с бампером I2C-flash.
iarduino_I2C_Bumper  bum    (0x0C);                        //   Объявляем объект bum для работы с бампером I2C-flash, указав адрес модуля на шине I2C.

const float    val_Speed    = 130.0f;                      //   Условная скорость движения машины для заполения ШИМ.
const float    pid_KP       = val_Speed * 0.15f;           //   Коэффициент пропорциональной обратной связи ПИД регулятора.
const float    pid_KI       = val_Speed * 0.005f;          //   Коэффициент интегральной     обратной связи ПИД регулятора.
const float    pid_KD       = val_Speed * 0.15f;           //   Коэффициент дифференциальной обратной связи ПИД регулятора.

const uint8_t pinH1   = 7;                                 // Создаём константу указывая номер вывода H1 MotorShield (он управляет направлением 1 мотора)
const uint8_t pinE1   = 6;                                 // Создаём константу указывая номер вывода E1 MotorShield (он управляет скоростью    1 мотора)
const uint8_t pinE2   = 5;                                 // Создаём константу указывая номер вывода E2 MotorShield (он управляет скоростью    2 мотора)
const uint8_t pinH2   = 4;                                 // Создаём константу указывая номер вывода H2 MotorShield (он управляет направлением 2 мотора)
uint8_t mSpeed  = 0;                                       // Создаём переменную для хранения скорости    моторов
bool    mDirect = HIGH;                                    // Создаём переменную для хранения направления моторов
float   arr_ERR[10]  = {0,0,0,0,0,0,0,0,0,0};              //   Массив последних ошибок для формирования интегральной составляющей ПИД регулятора.
uint8_t i, j;                                              //   Переменные для работы с массивом последних ошибок.

void setup(){
  delay(500);                                                // * Ждём завершение переходных процессов связанных с подачей питания.
//   ИНИЦИАЛИЗАЦИЯ МОДУЛЕЙ:                                  //
  bum.   begin();                                            //   Инициируем работу с бампером I2C-flash.
//   НАСТРОЙКА МОДУЛЕЙ:                                      //
  bum.setTurnPeriod(BUM_TURN_800);                           // Задать период мигания поворотников равен 800 мс.
  bum.setTurnSignal(BUM_TURN_AUTO);                          //  Работа поворотников в автоматическом режиме
  bum.settingsTurnAuto(BUM_AUTO_ON_2, BUM_AUTO_OFF_ANY, false); // поворотники работают в автоматическом режиме.

  pinMode(pinH1, OUTPUT); digitalWrite(pinH1, LOW);          // Конфигурируем вывод pinH1 как выход и устанавливаем на нём уровень логического «0»
  pinMode(pinE1, OUTPUT); digitalWrite(pinE1, LOW);          // Конфигурируем вывод pinE1 как выход и устанавливаем на нём уровень логического «0»
  pinMode(pinE2, OUTPUT); digitalWrite(pinE2, LOW);          // Конфигурируем вывод pinE2 как выход и устанавливаем на нём уровень логического «0»
  pinMode(pinH2, OUTPUT); digitalWrite(pinH2, LOW);          // Конфигурируем вывод pinH2 как выход и устанавливаем на нём уровень логического «0»
  
  digitalWrite(pinH1, mDirect );
  digitalWrite(pinH2, mDirect );
}

void loop(){
  if( bum.getLineSum() > 4 ){                           //   Если больше 4 датчиков бампера фиксируют линию, то ...
    bum.setLineType(BUM_LINE_CHANGE);                   //   Меняем тип линии трассы.
  }

  if (!bum.getLineSum()){}
  else{
     
  i++; i%=10; j=10;                                        //   Определяем значения переменных работы с массивом ошибок.
//   Получаем текущую ошибку центрирования линии:          //
     arr_ERR[i] = bum.getErrPID();                         //   Функция getErrPID() возвращает ошибку от 0 до ±4.5, где 0 - линия по центру, ±4.0 - линия на крайнем датчике, ±4.5 - линия потеряна.
//   Вычисляем все составляющие ПИД регулятора:            //
     float pid_P = arr_ERR[i];                             //   Пропорциональная составляющая «pid_P» представлена величиной текущей ошибки «ARR_ERR[i]».
     float pid_I = 0.0f; while(j--){pid_I+=arr_ERR[j-1];}  //   Интегральная     составляющая «pid_I» представлена суммой последних ошибок взятых из массива «arr_ERR».
     float pid_D = arr_ERR[i]-arr_ERR[(i+9)%10];           //   Дифференциальная составляющая «pid_D» представлена разницей между текущей «ARR_ERR[i]» и предыдущей «arr_ERR[(i+9)%10]» ошибкой.
     float PID   = pid_P*pid_KP+pid_I*pid_KI+pid_D*pid_KD; //   Вычисляем результат ПИД регулирования.
//   Устанавливаем скорость вращения колёс:                //
     analogWrite(pinE1, val_Speed + PID);
     analogWrite(pinE2, val_Speed - PID);
  }
}

ПИД-регулятор (пояснения к скетчу)

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

Функция getErrPID() возвращает ошибку от 0 до ±4.5, где 0 - линия по центру, ±4.0 - линия на крайнем датчике, ±4.5 - линия потеряна. Эти значения записываются в массив ошибок, в котором хранятся последние 10 значений ошибок. Благодаря тому, что мы работаем не с последней ошибкой, а с "историей" ошибок, мы можем судить о том, насколько быстро линия уходит из-под центра датчика, и насколько успешно у машинки получается вернуться к "состоянию равновесия", т.е. достаточное ли управляющее воздействие в данный момент даёт регулятор.

ПИД-регулятор использует для расчёта три составляющие:

• Пропорциональная составляющая: равна последней ошибке, полученной с датчика;
• Интегральная составляющая: равна сумме последних ошибок езды  - ошибок из массива;
• Дифференциальная составляющая: равна разнице между текущей ошибкой и предыдущей;

Степень влияния каждой составляющей на регулирование определяется коэффициентами, которые мы указывали в качестве констант. Изменяя их, можно добиться более выраженной реакции машинки на отклонение от линии. Попробуйте поменять их и посмотрите на результат. 

После вычисления результата ПИД-регулирования, устанавливается скорость вращения колёс - одно из них замедляется, другое, напротив, ускоряется. 

Ссылки:

• Робот «Малыш»
• Бампер с 9 датчиками линий с шагом 14мм., FLASH-I2C
• Библиотека iarduino_I2C_Bumper