ModelCode.ino

//-----------------------------------------------------------------------\\
// Программа по чтению информации с датчиков (гироскопа и ИК-дальномера) \\
//  и формированию по ним команд для сервоприводов.                      \\
// Программа выполнена для статьи:                                       \\
// "Разработка подсистемы управления гибким крылом беспилотного          \\
//   летательного аппарата на базе аэрошюта".                            \\
// Ссылка на статью:                                                     \\
// Автор программы: ElMigele                                             \\
//-----------------------------------------------------------------------\\

// Библиотеки
#include <Servo.h>          // Библиотека для работы с сервоприводом
#include <Wire.h>           // Библиотека для работы I²C
#include <TroykaIMU.h>      // Библиотека для работы с модулями IMU
                            //  доступна для скачивания:
                            //  https://github.com/amperka/Troyka-IMU

// Создание объектов для работы
Servo servo1;               // ... с сервоприводом, связанным с гироскопом
Servo servo2;               // ... с сервоприводом, связанным с дальномером
Gyroscope gyro;             // ... с гироскопом

// Пины подключения
int RangeFinderPin = 0;     // ... дальномера
int ServoPin1      = 6;     // ... сервопривода-1
int ServoPin2      = 9;     // ... сервопривода-2

// Переменные для данных с гироскопа и дальномера
int ang, dist;
int oldAng = 0;

void setup() {
  Serial.begin(9600);         // Установка скорости общения с компьютером
  servo1.attach(ServoPin1);   // Прикрепление сервопривода к пину
  servo1.write(0);            // Установление значения сервопривода в 0
  servo2.attach(ServoPin2);   // ...
  servo2.write(0);            // ...
  gyro.begin();               // Инициализация гироскопа   
  gyro.setRange(RANGE_500DPS);// Устанавливаем чувствительность гироскопа
                              // 250dps — по умолчанию, 500dps, 2000dps
}

void loop() {
// Гироскоп
  ang = gyro.readDegPerSecZ();           // Чтение угла с гироскопа по
                                         //  оси Z в градусах в секунду
                                         //  (по другим осям не работаем)
  oldAng = oldAng + ang;
  int res  = map(oldAng, -180, 180, 0, 255);
  Serial.print("Ang: ");
  Serial.print(oldAng);
  Serial.print("\t\t");
  Serial.print("Sr1: ");
  Serial.print(res);
  Serial.print("\t\t");
  servo1.write(res);                     // Поворот сервопривода на угол,
                                         //  задаваемый гироскопом       
// Дальномер
  int oldDist = dist;
  dist = analogRead(RangeFinderPin);     // Чтение расстояния с дальномера
  int pos  = map(dist, 0, 200, 0, 255); // Интерпретация расстояния
                                         //  (диапазон [0, 200]) в угол
                                         //  поворота сервопривода
                                         //  (диапазон [0, 255])
  Serial.print("Pos: ");
  Serial.print(dist);  
  Serial.print("\t\t");
  Serial.print("Sr2: ");
  Serial.println(pos);
  servo2.write(pos);                     // Поворот сервопривода на угол,
                                         //  задаваемый ИК-дальномером
 delay(1000);
}