Продвинутый таймер для кордовых моделей

Скетч для Arduino IDE
Постарался сделать как можно более читабельным
Отвечу на все вопросы.
Будет доработка в плане внешнего программирующего устройства на базе Arduino Uno и LCD дисплея.
Так, благодаря хорошему человеку нашлась четвертая версия

Timer_for_Plane_v4

//*************************************************************************************************************
//
// Тагильцев Геннадий aka Marks
// версия v3
// изменения v2:
// - добавлена функция boolean Engine_Proc (int, int)
// параметры
// - требуемое положение газа
// - условная задержка (в циклах программы)
// возвращает
// true - при достижении требуемого значения
// изменения v3
// - убрал процедуру таймера, вся обработка в одном цикле.
// изменения v4
// - расчет углов только на основании данных акселерометра
//
//**************************************************************************************************************
#include <Servo.h>
//#include <SoftwareSerial.h>
#include <Wire.h>
#include "Kalman.h"

#define Landing_Gear                // если ретрактов нет, ставим коммент на эту стоку строку

#define Button_Pin 12               // порт к которому подключена кнопка старт/стоп или перемычка
#define Port_Motor_ESC 2            // порт к которому подключен ESC

#define Motor_ESC_Max_THR 2200      // максимальные обороты двигателя
#define Motor_ESC_Normall_THR 1800  // номинальные (полетные) обороты
#define Motor_ESC_Landing_THR 1500  // посадочный режим
#define Motor_ESC_Min_THR 800       // двигатель стоп
#define Time_Of_Calibrate 2         // время между Макс газ и мин газ при необходимости калибровке регулятора
#define Takeoff 5                   // время отведенное на взлет и уборку шасси, в течении этого времени процедура старта может быть отменена (т.е. время до уборки шасси) -  10 секунд
#define GearUp 5                    // время время после старта, начало уборки шасси -  5 секунд
#define InFly 25                    // время полета в секундах с момента завершения процедуры старта - сейчас стоит 60 секунд
#define Landing 10                  // время отводимое на посадку, в течении этого времени будут выпущены шасси и снижены обороты мотора до посадочных -  60 секунд
#define delay_g 100              // коэффициент торможения ))


#ifdef Landing_Gear
#define Port_Left_Wheel 6           // порт к которому подключена серва ретракта левого шасси
#define Port_Right_Wheel 7          // порт к которому подключена серва ретракта правого шасси
#define Left_Wheel_Down 2000
#define Left_Wheel_Up 1000
#define Right_Wheel_Down 1000
#define Right_Wheel_Up 2000
#define Up 1
#define Down 0
#endif

  Servo Motor_ESC;                    // объект Servo для регулятора мотора
#ifdef Landing_Gear
  Servo Left_Wheel;                   // объект Servo для ретракта левого шасси
  Servo Right_Wheel;                  // объект Servo для ретракта правого шасси
#endif
//  SoftwareSerial softSerial(2, 3);    // RX, TX>

//**************************************************************************************
// переменные определяющие ветвление программы
// проще говоря, состояние модели
//**************************************************************************************
  boolean Abort = false;                // процедура старта может быть прервана повторным нажатием кнопки
//************************************************************************************
// блок объявления переменных для процедуры кальман
//************************************************************************************

Kalman kalmanX;
Kalman kalmanY;

uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;

uint32_t timer;





//************************************************************************************
//
//************************************************************************************
int Stack [] {
  Motor_ESC_Min_THR,
  Motor_ESC_Landing_THR,
  Motor_ESC_Normall_THR,
  Motor_ESC_Max_THR,
  Time_Of_Calibrate,
  Takeoff,
  GearUp,
  InFly,
  Landing
  };

//************************************************************************************
// временные переменные
//************************************************************************************
  unsigned long Current_Time;         // текущее время, мало ли где то пригодится
  unsigned long Start_Time;           // время старта по таймеру (т.е. момент нажатия на кнопку)
  unsigned long Takeoff_Time;         // время начала полета, шасси уже убраны
  unsigned long GearUp_Time;          // время начала уборки шасси (т.е. с момента нажатия на кнопку)
  unsigned long Fly_Time;             // время завершения полета
  unsigned long Landing_Time;         // время останова двигателя
//*************************************************************************************

//*************************************************************************************
// эти переменные нужны что бы не задаваться вопросом "А как у нас работают сервы"
//*************************************************************************************
  int Step_Left;
  int Step_Right;
//*************************************************************************************

void setup() {

  pinMode(Button_Pin, INPUT);                           // привязали кнопку старт к порту
  digitalWrite(Button_Pin, HIGH);                       // подтянули кнопку к +5V
  Motor_ESC.attach(Port_Motor_ESC,Motor_ESC_Min_THR,Motor_ESC_Max_THR); // привязали регулятор к каналу



//**********************************************************************************
// если при включении нажата кнопка
// программа войдет в режим настройки
//**********************************************************************************
    if (digitalRead(Button_Pin) == LOW) Prog_Mode();
      while (digitalRead(Button_Pin) == LOW) {
     }
  Motor_ESC.writeMicroseconds(Motor_ESC_Min_THR);
  delay (300);
//**********************************************************************************

//************************************************************************************
  int i = Left_Wheel_Down-Left_Wheel_Up;
  if (i>0) {
    Step_Left = -1;
  }
  else {
    Step_Left = 1;
  }
  i = Right_Wheel_Down-Right_Wheel_Up;
  if (i>0) {
    Step_Right = -1;
  }
  else {
    Step_Right = 1;
  }
/*
  при подключении питания сервы встанут в крайнее положение, соответствующее Down
*/
#ifdef Landing_Gear
    Left_Wheel.attach(Port_Left_Wheel);
    delay(500);
    Right_Wheel.attach(Port_Right_Wheel);
    delay(500);
    while (!Gear_Proc(Down));
#endif
//**********************************************************************************
// вышли из режима программирования
//*********************************************************************************
  while (digitalRead(Button_Pin) == HIGH) {
  }
  delay (500);
  Start_Time = millis();
  Takeoff_Time = Start_Time + Stack [5]*1000;
  GearUp_Time = Takeoff_Time + Stack [6]*1000;
  Fly_Time = GearUp_Time + Stack [7]*1000;
  Landing_Time = Fly_Time + Stack [8]*1000;
//*********************************************************************************


//**********************************************************************************
// блок setup для kalman
//**********************************************************************************
  Serial.begin(115200);
  Wire.begin();
  i2cWrite(0x6B,0x00); // Disable sleep mode
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }
  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  timer = micros();
// **********************************************************************************
}

void loop() {
  while (millis() < Takeoff_Time) Engine_Proc(Stack[2],10);

  while (millis() < GearUp_Time) Gear_Proc(Up);

 while (millis() < Fly_Time) {
   int currentTHR = Motor_ESC.readMicroseconds();
   Angel();
    if (accXangle>190 &&  accXangle<350) {
      if (currentTHR != Stack[3]) {
        Motor_ESC.writeMicroseconds(Stack[3]);
      }
    }
    else if (accXangle<170 &&  accXangle>10) {
      if (currentTHR != Stack[1]) {
        Motor_ESC.writeMicroseconds(Stack[1]);
      }
    }
    else {
      Motor_ESC.writeMicroseconds(Stack[2]);
    }
  }

  while(millis() < Landing_Time) {
    if (Engine_Proc(Stack[1],10)) Gear_Proc(Down);
  }

  while (millis() >= Landing_Time) Engine_Proc(Stack[0],10) ;

}




// вошли в режим программирования
// пока тут только калибровка регулятора
//*************************************************************************************
void Prog_Mode () {
  Motor_ESC.writeMicroseconds(Stack[3]);
  delay (Stack [4]*1000);                        // немного ждем и газ в ноль
  Motor_ESC.writeMicroseconds(Stack[0]);
  delay (500);
}





//*************************************************************************************
// процедура уборки/выпуска шасси
// результат
// - TRUE - завершено
// - FALSE - не завершена
// параметр - int -
// - 0 - выпускам
// - 1 - убираем
//*************************************************************************************
boolean Gear_Proc(int l) {
  static int i = delay_g;
  int j = Left_Wheel.readMicroseconds();
  int k = Right_Wheel.readMicroseconds();
if (l == 0) {
// шасси выпускаем
  if (j == Left_Wheel_Down && k == Right_Wheel_Down) return true;

  if (i==0) {
    if (j > Left_Wheel_Down) j--;
    else j++;
    Left_Wheel.writeMicroseconds(j);
  }

  if (i==0) {
    if (k > Right_Wheel_Down) k--;
    else k++;
    Right_Wheel.writeMicroseconds(k);
  }

  if (!i--) i=delay_g;
}
else {
  // шасси выпускаем
  if (j == Left_Wheel_Up && k == Right_Wheel_Up) return true;

  if (i==0) {
    if (j > Left_Wheel_Up) j--;
    else j++;
    Left_Wheel.writeMicroseconds(j);
  }

  if (i==0) {
    if (k > Right_Wheel_Up) k--;
    else k++;
    Right_Wheel.writeMicroseconds(k);
 }
  if (!i--) i=delay_g;
}
  return false;
}






//*************************************************************************************
// Процедура работы с регуляторм
// возвращает true когда результат достигнут
// первый параметр - int - значение до которого надо изменить газ
// второй параметр - int - задержка
//*************************************************************************************
boolean Engine_Proc(int j1, int i1) {
  static int i = 0;
  int j =  Motor_ESC.readMicroseconds();

  if (j==j1) return true;

  if (i == i1 && j<j1) {
    j++;
    Motor_ESC.writeMicroseconds(j);
  }
  if (i == i1 && j>j1) {
    j--;
    Motor_ESC.writeMicroseconds(j);
  }
  if (!i--) i=i1;
  if (digitalRead(Button_Pin) == LOW) Abort = true ;
  return false;
}






//***************************************************************************************
// кальман для гироскопа
// считает углы ))
//***************************************************************************************
void Angel() {
  uint8_t* data = i2cRead(0x3B,14);
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  delay (1);
}

void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}

uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}

Как это работает в настоящее время можно посмотреть на видео.