/* 
драйверы моторов запитаны на 12в к внешнему питанию.
есп должна быть запитана норм беком на 5в 3а, для сервы и мотор редуктора.

все концевики паять к подписанному на корпусе контакту 1 и 3.


#define M1_STEP_PIN       36
#define M1_DIR_PIN        37

#define M2_STEP_PIN       38
#define M2_DIR_PIN        39

#define M3_STEP_PIN       40
#define M3_DIR_PIN        41

#define M4_STEP_PIN       7
#define M4_DIR_PIN        8

#define M5_STEP_PIN       9
#define M5_DIR_PIN        10

#define M6_STEP_PIN       11
#define M6_DIR_PIN        12



*/
// Полношаговый режим DRV8825 (MODE0..2 = LOW), STEP/DIR только.
// Винт 2 мм/об, мотор 1.8° (200 шаг/об) => 100 шаг/мм.
// Авторежим калибровки лифта при старте, далее — цикл по BOOT.
//
// СИСТЕМА ЦЕНТРОВКИ:
// - Нижняя ось: моторы 1,2,3,4 (ход 254 мм)
//   * М1,М4: сначала CCW, потом CW
//   * М2,М3: сначала CW, потом CCW
// - Верхняя ось: моторы 5,6 (ход 230 мм)
//   * М5: сначала CCW, потом CW
//   * М6: сначала CW, потом CCW
//
// ====================== ПОДКЛЮЧЕНИЕ БИБЛИОТЕК ======================
#include <Arduino.h>         // Основная библиотека Arduino
#include <ESP32Servo.h>      // Библиотека для управления сервоприводами

//
// ====================== НАСТРОЙКИ РЕВЕРСА МОТОРОВ ======================
// true = нормальное направление, false = инвертированное
// Центровка - нижняя ось
static const bool M1_DIR_NORMAL       = true;   // мотор 1
static const bool M2_DIR_NORMAL       = true;   // мотор 2
static const bool M3_DIR_NORMAL       = true;   // мотор 3
static const bool M4_DIR_NORMAL       = true;   // мотор 4
// Центровка - верхняя ось
static const bool M5_DIR_NORMAL       = true;   // мотор 5
static const bool M6_DIR_NORMAL       = true;   // мотор 6

// Механизм замены
// Лифт: вверх (к N1) = отрицательные значения, вниз = положительные
// Если ваш мотор крутится ПО ЧАСОВОЙ при подъеме, и это правильно для вашей механики
static const bool LIFT_DIR_NORMAL     = true;   // направление лифта

// Магазин: должен начинать вращение ПРОТИВ ЧАСОВОЙ (CCW)
// Если нужно изменить начальное направление, поменяйте эту переменную
static const bool MAG_DIR_NORMAL      = true;   // направление магазина
static const bool MAG_START_CCW       = true;   // true = начинаем с вращения против часовой

// Серво: поменяйте местами значения если нужен реверс
static const int SERVO_OPEN_US_VAL    = 1430;   // позиция "открыто" (разжато)
static const int SERVO_CLAMP_US_VAL   = 1570;   // позиция "зажато"

// Редуктор разжима: если нужен реверс, поменяйте HIGH/LOW в функции gripSet()
static const bool GRIP_DIR_NORMAL     = true;   // true = нормальное, false = реверс

// ====================== РЕЖИМ ОТЛАДКИ ======================
static bool DEBUG_MODE = true;   // true = пошаговый режим с остановками
static int TEST_MODE = 3; // 1=пошагово BOOT; 2=одним нажатием с калибровкой; 3=одним нажатием без калибровки; 4=бесконечный как (3)
static bool loopActive = false;           // режим 4: автоповтор цикла
static bool runOneCycleRequest = false;   // режим 2: после калибровки запустить цикл
static bool debugWaitingForButton     = false;  // флаг ожидания кнопки в отладке
static const char* debugStepName      = "";     // название текущего шага для отладки
static bool skipCurrentAction         = false;  // флаг пропуска текущего действия

// ---------------------- ПИНЫ (по порядку) ----------------------
#define PIN_BOOT          0   // Кнопка BOOT, INPUT_PULLUP, активный LOW

// Центровка - нижняя ось (4 мотора)
#define M1_STEP_PIN       36
#define M1_DIR_PIN        37
#define M2_STEP_PIN       38
#define M2_DIR_PIN        39
#define M3_STEP_PIN       40
#define M3_DIR_PIN        41
#define M4_STEP_PIN       7
#define M4_DIR_PIN        8
// Центровка - верхняя ось (2 мотора)
#define M5_STEP_PIN       9
#define M5_DIR_PIN        10
#define M6_STEP_PIN       11
#define M6_DIR_PIN        12

// Лифт АКБ (DRV8825 full-step)
#define LIFT_STEP_PIN     13
#define LIFT_DIR_PIN      14

// Серво зажима (LEDC 50 Гц PWM)
#define SERVO_PIN         15

// Редуктор разжимного механизма (H-bridge IN1/IN2)
#define GRIP_IN1_PIN      16
#define GRIP_IN2_PIN      17

// Магазин (шаговый)
#define MAG_STEP_PIN      18
#define MAG_DIR_PIN       35  // Изменено с 19 на 35

// Концевики (INPUT_PULLUP, активный LOW «на замыкание»)
#define LIM_N1_PIN        1  // верх лифта (ноль лифта) - изменено с 20
#define LIM_N2_PIN        2  // низ разжимного механизма - изменено с 21
#define LIM_N3_PIN        3  // верх разжимного механизма - изменено с 34
#define LIM_N4_PIN        4  // предел поворота магазина (360°) - изменено с 35

// Резервный пин для ENABLE драйверов (если понадобится)
#define DRIVERS_ENABLE_PIN 40  // Опционально для управления питанием драйверов

// ---------------------- КИНЕМАТИКА ----------------------
static const int STEPS_PER_MM      = 200;     // 200 шаг/об / 2 мм/об = 100 шаг/мм

// Центровка
static const int BOTTOM_AXIS_MM    = 254;     // ход нижней оси (М1-М4)
static const int TOP_AXIS_MM       = 230;     // ход верхней оси (М5-М6)
static const int BOTTOM_AXIS_STEPS = BOTTOM_AXIS_MM * STEPS_PER_MM; // 25400
static const int TOP_AXIS_STEPS    = TOP_AXIS_MM * STEPS_PER_MM;    // 23000

// Лифт и магазин
static const int LIFT_MM_MAGLEVEL  = 98;     // 115 мм (изменено с 100)
static const int LIFT_MM_LEVEL2    = 117;     // 115 + 30 мм ниже (изменено со 130)
static const int LIFT_STEPS_115    = LIFT_MM_MAGLEVEL * STEPS_PER_MM; // 11500 (было LIFT_STEPS_115)
static const int LIFT_STEPS_145    = LIFT_MM_LEVEL2  * STEPS_PER_MM;  // 14500 (было LIFT_STEPS_145)
// Магазин: 200 шаг/об => 90° = 50 шагов (если без редукции). Подстройте при необходимости.
static const int MAG_MICROSTEP   = 8;       // 1/8 шага (MS1=H, MS2=H, MS3=L)
static const int MAG_STEPS_90    = 50 * MAG_MICROSTEP;   // 90° при полном шаге 50 -> умножаем на микрошаг
static const int MAG_STEPS_360   = 200 * MAG_MICROSTEP;  // 360° при полном шаге 200 -> умножаем на микрошаг



// ---------------------- СКОРОСТИ (шаг/с) И ПУЛЬС ----------------------
static const int STEP_PULSE_US     = 3;       // >= 2 мкс для DRV8825
static const int CENTER_SPEED_SPS  = 1200;    // скорость центровки
static const int LIFT_SPEED_SPS    = 1500;    // лифт
static const int MAG_SPEED_SPS     = 800;     // магазин

// интервал между шагами в мкс = 1e6 / SPS
#define SPS_TO_INTERVAL_US(sps) ( (sps) > 0 ? (1000000UL / (unsigned long)(sps)) : 1000UL )

// ---------------------- ТАЙМАУТЫ (мс) ----------------------
static const uint32_t TMO_FIND_N1_MS       = 20000;  // лифт до N1
static const uint32_t TMO_LIFT_MOVE_MS     = 20000;  // лифт 130 мм
static const uint32_t TMO_CENTER_MOVE_MS   = 40000;  // центровка туда/обратно
static const uint32_t TMO_MAG_90_MS        = 10000;  // магазин 90°
static const uint32_t TMO_GRIP_UP_MS       = 8000;   // разжим вверх до N3
static const uint32_t TMO_GRIP_DOWN_MS     = 8000;   // разжим вниз до N2

// ---------------------- СЕРВО ----------------------
static const int SERVO_MIN_US     = 800;    // минимальный импульс
static const int SERVO_MAX_US     = 2200;   // максимальный импульс
static const uint32_t SERVO_HOLD_TIME_MS = 500;  // время удержания позиции перед отключением

// ---------------------- ПАУЗЫ ПО СЦЕНАРИЮ ----------------------
static const uint32_t PAUSE_SHORT_MS = 1000;    // «Пауза 1 сек»
static const uint32_t PAUSE_60S_MS   = 60000;   // ожидание 60 сек

// ---------------------- ДЕБОУНС ----------------------
static const uint32_t DEBOUNCE_MS    = 25;
static const uint32_t LONG_PRESS_MS  = 2000;

// ====================== УТИЛИТЫ: дебаунс входа ======================
struct DebouncedInput {
  uint8_t pin;
  bool activeLow;
  bool stableState;
  bool lastRead;
  uint32_t lastChangeMs;
  bool lastStableReported;

  void begin(uint8_t p, bool pullup = true, bool actLow = true) {
    pin = p; activeLow = actLow;
    pinMode(pin, pullup ? INPUT_PULLUP : INPUT);
    lastRead = digitalRead(pin);
    stableState = lastRead;
    lastChangeMs = millis();
    lastStableReported = stableState;
  }

  // Возвращает актуальное стабильное логическое «активно» с учётом activeLow
  bool readActive() {
    update();
    bool phys = stableState;
    return activeLow ? (phys == LOW) : (phys == HIGH);
  }

  // Событие «фронт нажатия» (активный переход)
  bool fell() {
    update();
    bool nowActive = activeLow ? (stableState == LOW) : (stableState == HIGH);
    bool prevActive = activeLow ? (lastStableReported == LOW) : (lastStableReported == HIGH);
    if (nowActive && !prevActive) { lastStableReported = stableState; return true; }
    lastStableReported = stableState;
    return false;
  }

  // Длительность текущего активного состояния (мс)
  uint32_t activeDuration() {
    update();
    bool act = activeLow ? (stableState == LOW) : (stableState == HIGH);
    if (!act) return 0;
    return millis() - lastChangeMs;
  }

  void update() {
    bool r = digitalRead(pin);
    uint32_t now = millis();
    if (r != lastRead) {
      lastRead = r;
      lastChangeMs = now;
    }
    if ((now - lastChangeMs) >= DEBOUNCE_MS) {
      stableState = r;
    }
  }
};

// ====================== УТИЛИТЫ: простой шаговик ======================
class SimpleStepper {
public:
  void begin(uint8_t stepPin, uint8_t dirPin, int sps, bool dirHighIsPositive = true) {
    _step = stepPin; _dir = dirPin; _dirSign = dirHighIsPositive ? 1 : -1;
    pinMode(_step, OUTPUT); pinMode(_dir, OUTPUT);
    digitalWrite(_step, LOW);
    digitalWrite(_dir, LOW);
    setSpeedSps(sps);
    _curPos = 0;
    _targetPos = 0;
    _moving = false;
    _lastStepUs = micros();
  }

  void setSpeedSps(int sps) {
    _intervalUs = SPS_TO_INTERVAL_US(sps);
  }

  long position() const { return _curPos; }
  long target() const { return _targetPos; }
  long remaining() const { return _targetPos - _curPos; }
  bool isBusy() const { return _moving; }

  void setPosition(long pos) { _curPos = pos; }

  void moveTo(long pos) {
    _targetPos = pos;
    updateDir();
    _moving = (_targetPos != _curPos);
  }

  void moveBy(long delta) {
    moveTo(_curPos + delta);
  }

  // Должен вызываться часто (в loop)
  void update() {
    if (!_moving) return;
    int dir = (_targetPos > _curPos) ? +1 : -1;
    // выставить DIR
    digitalWrite(_dir, (dir * _dirSign) > 0 ? HIGH : LOW);
    uint32_t now = micros();
    if ((now - _lastStepUs) >= _intervalUs) {
      // STEP импульс
      digitalWrite(_step, HIGH);
      delayMicroseconds(STEP_PULSE_US);
      digitalWrite(_step, LOW);
      _lastStepUs = now;
      _curPos += dir;
      if (_curPos == _targetPos) _moving = false;
    }
  }

private:
  void updateDir() {
    int dir = (_targetPos > _curPos) ? +1 : -1;
    digitalWrite(_dir, (dir * _dirSign) > 0 ? HIGH : LOW);
  }

  uint8_t _step, _dir;
  int _dirSign = +1;
  volatile long _curPos = 0;
  volatile long _targetPos = 0;
  volatile bool _moving = false;
  uint32_t _lastStepUs = 0;
  uint32_t _intervalUs = 1000;
};

// ====================== УТИЛИТЫ: редуктор (H-bridge) ======================
// Объявляем enum ПЕРЕД использованием
enum GripDir { GRIP_STOP=0, GRIP_DOWN=1, GRIP_UP=2 };

inline void gripBegin() {
  pinMode(GRIP_IN1_PIN, OUTPUT);
  pinMode(GRIP_IN2_PIN, OUTPUT);
  digitalWrite(GRIP_IN1_PIN, LOW);
  digitalWrite(GRIP_IN2_PIN, LOW);
}

inline void gripSet(GripDir d) {
  if (GRIP_DIR_NORMAL) {
    switch (d) {
      case GRIP_STOP: 
        digitalWrite(GRIP_IN1_PIN, LOW); 
        digitalWrite(GRIP_IN2_PIN, LOW); 
        Serial.println("РЕДУКТОР: Стоп");
        break;
      case GRIP_DOWN: 
        digitalWrite(GRIP_IN1_PIN, HIGH); 
        digitalWrite(GRIP_IN2_PIN, LOW); 
        Serial.println("РЕДУКТОР: Вниз");
        break;
      case GRIP_UP:   
        digitalWrite(GRIP_IN1_PIN, LOW);  
        digitalWrite(GRIP_IN2_PIN, HIGH); 
        Serial.println("РЕДУКТОР: Вверх");
        break;
    }
  } else {
    // Реверс направления
    switch (d) {
      case GRIP_STOP: 
        digitalWrite(GRIP_IN1_PIN, LOW); 
        digitalWrite(GRIP_IN2_PIN, LOW); 
        Serial.println("РЕДУКТОР: Стоп");
        break;
      case GRIP_DOWN: 
        digitalWrite(GRIP_IN1_PIN, LOW); 
        digitalWrite(GRIP_IN2_PIN, HIGH); 
        Serial.println("РЕДУКТОР: Вниз (реверс)");
        break;
      case GRIP_UP:   
        digitalWrite(GRIP_IN1_PIN, HIGH);  
        digitalWrite(GRIP_IN2_PIN, LOW); 
        Serial.println("РЕДУКТОР: Вверх (реверс)");
        break;
    }
  }
}

// ====================== УТИЛИТЫ: серво управление ======================
Servo myServo;  // Объект сервопривода
uint32_t servoLastMoveTime = 0;  // время последнего движения серво
bool servoNeedsDetach = false;   // флаг необходимости отключения серво

inline void servoSetup() {
  // Инициализация серво произойдет при первом использовании
  Serial.println("Серво инициализирован (библиотека ESP32Servo)");
}

inline void servoAttachIfNeeded() {
  if (!myServo.attached()) {
    myServo.attach(SERVO_PIN, SERVO_MIN_US, SERVO_MAX_US);
    delay(10);  // небольшая задержка для стабилизации
  }
}

inline void servoWriteUS(int us) {
  if (us < SERVO_MIN_US) us = SERVO_MIN_US;
  if (us > SERVO_MAX_US) us = SERVO_MAX_US;
  
  servoAttachIfNeeded();
  myServo.writeMicroseconds(us);
  servoLastMoveTime = millis();
  servoNeedsDetach = true;
  
  Serial.printf("Серво переместился на %d мкс\n", us);
}

inline void servoUpdate() {
  // Отключаем серво через SERVO_HOLD_TIME_MS после последнего движения
  if (servoNeedsDetach && myServo.attached() && 
      (millis() - servoLastMoveTime > SERVO_HOLD_TIME_MS)) {
    myServo.detach();
    servoNeedsDetach = false;
    Serial.println("Серво отключен для предотвращения перегрева");
  }
}

// Обёртки для серво с учётом настроек реверса
inline void servoOpen() {
  servoWriteUS(SERVO_OPEN_US_VAL);
  Serial.printf("СЕРВО: Открыто (%d мкс)\n", SERVO_OPEN_US_VAL);
}

inline void servoClamp() {
  servoWriteUS(SERVO_CLAMP_US_VAL);
  Serial.printf("СЕРВО: Зажато (%d мкс)\n", SERVO_CLAMP_US_VAL);
}

// ====================== ГЛОБАЛЬНОЕ: объекты ======================
DebouncedInput btnStart;
DebouncedInput limN1, limN2, limN3, limN4;

// Моторы центровки
SimpleStepper stepM1, stepM2, stepM3, stepM4;  // нижняя ось
SimpleStepper stepM5, stepM6;                   // верхняя ось

// Моторы механизма замены
SimpleStepper stepLift, stepMag;

// состояние автомата
enum State {
  INIT,
  CALIB_LIFT_UP_TO_N1,
  CALIB_LIFT_TO_MAG100,
  CALIB_GRIP_DOWN_TO_N2,
  IDLE,

  DEBUG_WAIT,  // новое состояние для отладочных остановок

  RUN_CENTER_FORWARD,   // движение осей центровки вперёд
  RUN_CENTER_BACK,      // движение осей центровки назад
  WAIT_60S,

  SWAP_LIFT_UP_TO_N1,
  SWAP_SERVO_CLAMP,
  PAUSE_1S_A, // универсальная пауза
  SWAP_GRIP_UP_TO_N3,
  PAUSE_1S_B,
  SWAP_LIFT_TO_100,
  PAUSE_1S_C,
  SWAP_SERVO_OPEN,
  PAUSE_1S_D,
  SWAP_LIFT_TO_130,
  PAUSE_1S_E,
  SWAP_MAG_90,
  PAUSE_1S_F,
  SWAP_LIFT_UP_TO_N1_WITH_SERVO_ON_100,
  PAUSE_1S_G,
  SWAP_GRIP_DOWN_TO_N2,
  PAUSE_1S_H,
  SWAP_SERVO_OPEN_FINAL,
  PAUSE_1S_I,

  FAULT
};

State state = INIT;
State stateAfterPause = IDLE;
State stateAfterDebugWait = IDLE;  // состояние после отладочной остановки
uint32_t stateStartMs = 0;
uint32_t pauseUntilMs = 0;

// доп. переменные для сценариев
long magStartPos = 0;
bool magReversedPlan = false;
bool magDirectionCW = !MAG_START_CCW;  // начальное направление магазина (если MAG_START_CCW=true, то начинаем с CCW)

bool passed100mmOnLift = false;

// ---------------------- ВСПОМОГАТЕЛЬНЫЕ: предварительные объявления ----------------------
void gotoState(State s);
void debugStop(const char* stepName, State nextState);
void startPause(uint32_t ms, State after);
void enterFault(const char* why);

// ---------------------- ВСПОМОГАТЕЛЬНЫЕ ----------------------
void printLimitSwitches() {
  Serial.printf("Концевики: N1=%d, N2=%d, N3=%d, N4=%d\n",
    limN1.readActive() ? 1 : 0,
    limN2.readActive() ? 1 : 0,
    limN3.readActive() ? 1 : 0,
    limN4.readActive() ? 1 : 0
  );
}

void printCenterMotorPositions() {
  Serial.printf("Нижняя ось: М1=%ld, М2=%ld, М3=%ld, М4=%ld\n",
    stepM1.position(), stepM2.position(), stepM3.position(), stepM4.position()
  );
  Serial.printf("Верхняя ось: М5=%ld, М6=%ld\n",
    stepM5.position(), stepM6.position()
  );
}

void printSwapMotorPositions() {
  Serial.printf("Моторы замены: Лифт=%ld, Магазин=%ld\n",
    stepLift.position(), stepMag.position()
  );
}

void debugStop(const char* stepName, State nextState) {
  if (DEBUG_MODE) {
    Serial.println("=====================================");
    Serial.printf("ОТЛАДКА: %s\n", stepName);
    printCenterMotorPositions();
    printSwapMotorPositions();
    printLimitSwitches();
    Serial.println("Нажмите BOOT для продолжения...");
    Serial.println("Или нажмите BOOT еще раз для ПРОПУСКА действия");
    Serial.println("=====================================");
    
    debugStepName = stepName;
    stateAfterDebugWait = nextState;
    debugWaitingForButton = true;
    skipCurrentAction = false;
    state = DEBUG_WAIT;
  } else {
    gotoState(nextState);
  }
}

inline void gotoState(State s) {
  state = s;
  stateStartMs = millis();
  skipCurrentAction = false;  // сбрасываем флаг пропуска
  
  const char* stateName = "НЕИЗВЕСТНО";
  switch(s) {
    case INIT: stateName = "ИНИЦИАЛИЗАЦИЯ"; break;
    case CALIB_LIFT_UP_TO_N1: stateName = "КАЛИБРОВКА: Лифт вверх до N1"; break;
    case CALIB_LIFT_TO_MAG100: stateName = "КАЛИБРОВКА: Лифт на 115мм"; break;
    case CALIB_GRIP_DOWN_TO_N2: stateName = "КАЛИБРОВКА: Редуктор вниз до N2"; break;
    case IDLE: stateName = "ОЖИДАНИЕ"; break;
    case DEBUG_WAIT: stateName = "ОТЛАДКА: Ожидание"; break;
    case RUN_CENTER_FORWARD: stateName = "ЦЕНТРОВКА: Движение вперед"; break;
    case RUN_CENTER_BACK: stateName = "ЦЕНТРОВКА: Возврат назад"; break;
    case WAIT_60S: stateName = "ОЖИДАНИЕ 60 секунд"; break;
    case SWAP_LIFT_UP_TO_N1: stateName = "ЗАМЕНА: Лифт вверх до N1"; break;
    case SWAP_SERVO_CLAMP: stateName = "ЗАМЕНА: Зажим серво"; break;
    case SWAP_GRIP_UP_TO_N3: stateName = "ЗАМЕНА: Редуктор вверх до N3"; break;
    case SWAP_LIFT_TO_100: stateName = "ЗАМЕНА: Лифт на 115мм"; break;
    case SWAP_SERVO_OPEN: stateName = "ЗАМЕНА: Открытие серво"; break;
    case SWAP_LIFT_TO_130: stateName = "ЗАМЕНА: Лифт на 145мм"; break;
    case SWAP_MAG_90: stateName = "ЗАМЕНА: Поворот магазина 90°"; break;
    case SWAP_LIFT_UP_TO_N1_WITH_SERVO_ON_100: stateName = "ЗАМЕНА: Лифт вверх с захватом"; break;
    case SWAP_GRIP_DOWN_TO_N2: stateName = "ЗАМЕНА: Редуктор вниз до N2"; break;
    case SWAP_SERVO_OPEN_FINAL: stateName = "ЗАМЕНА: Финальное открытие серво"; break;
    case FAULT: stateName = "ОШИБКА"; break;
    default: break;
  }
  Serial.printf("СОСТОЯНИЕ -> %s\n", stateName);
}

inline bool timeoutPassed(uint32_t msLimit) {
  return (millis() - stateStartMs) > msLimit;
}
inline void startPause(uint32_t ms, State after) {
  pauseUntilMs = millis() + ms;
  stateAfterPause = after;
  gotoState(PAUSE_1S_A); // используем единый обработчик пауз
}
inline void enterFault(const char* why) {
  Serial.println("=====================================");
  Serial.printf("!!! ОШИБКА: %s\n", why);
  printCenterMotorPositions();
  printSwapMotorPositions();
  printLimitSwitches();
  Serial.println("Удерживайте BOOT 2 сек для сброса ошибки");
  Serial.println("=====================================");
  
  // Остановить все моторы
  stepM1.moveTo(stepM1.position());
  stepM2.moveTo(stepM2.position());
  stepM3.moveTo(stepM3.position());
  stepM4.moveTo(stepM4.position());
  stepM5.moveTo(stepM5.position());
  stepM6.moveTo(stepM6.position());
  stepLift.moveTo(stepLift.position());
  stepMag.moveTo(stepMag.position());
  gripSet(GRIP_STOP);
  servoOpen();
  gotoState(FAULT);
}

// Проверка завершения движения всех моторов центровки
bool allCenterMotorsIdle() {
  return !stepM1.isBusy() && !stepM2.isBusy() && !stepM3.isBusy() && 
         !stepM4.isBusy() && !stepM5.isBusy() && !stepM6.isBusy();
}

// Проверка достижения целевых позиций
bool centerMotorsAtPosition(long bottomPos, long topPos) {
  return stepM1.position() == bottomPos && stepM2.position() == bottomPos &&
         stepM3.position() == bottomPos && stepM4.position() == bottomPos &&
         stepM5.position() == topPos && stepM6.position() == topPos;
}

// ---------------------- SETUP ----------------------
void setup() {
  Serial.begin(115200);
  delay(200);
  
  Serial.println("\n\n=====================================");
  Serial.println("ESP32-S3 Система замены АКБ дрона v3.1");
  Serial.println("6-осевая система центровки");
  Serial.println("Магазин с автореверсом на 360°");
  Serial.println("=====================================");
  Serial.printf("РЕЖИМ ОТЛАДКИ: %s\n", DEBUG_MODE ? "ВКЛЮЧЕН" : "ВЫКЛЮЧЕН");
  Serial.println("\nНаправления моторов:");
  Serial.printf("  Нижняя ось: М1=%s, М2=%s, М3=%s, М4=%s\n",
    M1_DIR_NORMAL ? "Норм" : "Реверс",
    M2_DIR_NORMAL ? "Норм" : "Реверс",
    M3_DIR_NORMAL ? "Норм" : "Реверс",
    M4_DIR_NORMAL ? "Норм" : "Реверс"
  );
  Serial.printf("  Верхняя ось: М5=%s, М6=%s\n",
    M5_DIR_NORMAL ? "Норм" : "Реверс",
    M6_DIR_NORMAL ? "Норм" : "Реверс"
  );
  Serial.printf("  Замена: Лифт=%s, Магазин=%s\n",
    LIFT_DIR_NORMAL ? "Норм" : "Реверс",
    MAG_DIR_NORMAL ? "Норм" : "Реверс"
  );
  Serial.printf("Направление редуктора: %s\n", GRIP_DIR_NORMAL ? "Нормальное" : "Реверс");
  Serial.printf("Серво: Открыто=%d мкс, Зажато=%d мкс\n", SERVO_OPEN_US_VAL, SERVO_CLAMP_US_VAL);
  Serial.println("=====================================\n");

  // Входы
  btnStart.begin(PIN_BOOT, true, true);
  limN1.begin(LIM_N1_PIN, true, true);
  limN2.begin(LIM_N2_PIN, true, true);
  limN3.begin(LIM_N3_PIN, true, true);
  limN4.begin(LIM_N4_PIN, true, true);

  // Моторы центровки с учётом настроек реверса
  stepM1.begin(M1_STEP_PIN, M1_DIR_PIN, CENTER_SPEED_SPS, M1_DIR_NORMAL);
  stepM2.begin(M2_STEP_PIN, M2_DIR_PIN, CENTER_SPEED_SPS, M2_DIR_NORMAL);
  stepM3.begin(M3_STEP_PIN, M3_DIR_PIN, CENTER_SPEED_SPS, M3_DIR_NORMAL);
  stepM4.begin(M4_STEP_PIN, M4_DIR_PIN, CENTER_SPEED_SPS, M4_DIR_NORMAL);
  stepM5.begin(M5_STEP_PIN, M5_DIR_PIN, CENTER_SPEED_SPS, M5_DIR_NORMAL);
  stepM6.begin(M6_STEP_PIN, M6_DIR_PIN, CENTER_SPEED_SPS, M6_DIR_NORMAL);
  
  // Моторы замены
  stepLift.begin(LIFT_STEP_PIN, LIFT_DIR_PIN, LIFT_SPEED_SPS, LIFT_DIR_NORMAL);
  stepMag.begin(MAG_STEP_PIN, MAG_DIR_PIN, MAG_SPEED_SPS, MAG_DIR_NORMAL);

  // Все моторы в ноль
  stepM1.setPosition(0); stepM2.setPosition(0); stepM3.setPosition(0);
  stepM4.setPosition(0); stepM5.setPosition(0); stepM6.setPosition(0);
  stepLift.setPosition(0); stepMag.setPosition(0);

  // Серво
  servoSetup();
  servoOpen(); // по умолчанию разжато

  // Редуктор
  gripBegin();
  gripSet(GRIP_STOP);

  // Начальное состояние концевиков
  Serial.println("Начальное состояние:");
  printLimitSwitches();
  
  // Режимы тестирования: запуск последовательности
  if (TEST_MODE == 1) { DEBUG_MODE = true; debugStop("Начало калибровки", CALIB_LIFT_UP_TO_N1); }
else { DEBUG_MODE = false; gotoState(IDLE); Serial.printf("TEST_MODE=%d: готов. Нажми BOOT.\n", TEST_MODE); Serial.printf("РЕЖИМ ОТЛАДКИ: %s\n", DEBUG_MODE ? "ВКЛЮЧЕН" : "ВЫКЛЮЧЕН"); }
}

// ---------------------- LOOP ----------------------
void loop() {
  // обновление дебаунсов
  btnStart.update();
  limN1.update(); limN2.update(); limN3.update(); limN4.update();

  // Обновление всех шаговиков каждый тик
  stepM1.update(); stepM2.update(); stepM3.update();
  stepM4.update(); stepM5.update(); stepM6.update();
  stepLift.update(); stepMag.update();
  
  // Обновление серво (автоотключение)
  servoUpdate();

  // Обработка отладочной остановки
  if (state == DEBUG_WAIT) {
    static uint32_t firstPressTime = 0;
    static bool waitingSecondPress = false;
    
    if (btnStart.fell()) {
      if (!waitingSecondPress) {
        // Первое нажатие - продолжаем выполнение
        Serial.println("Продолжение выполнения...\n");
        debugWaitingForButton = false;
        firstPressTime = millis();
        waitingSecondPress = true;
        gotoState(stateAfterDebugWait);
      } else if (millis() - firstPressTime < 1000) {
        // Второе нажатие в течение 1 секунды - пропускаем действие
        Serial.println("ПРОПУСК текущего действия!\n");
        skipCurrentAction = true;
        waitingSecondPress = false;
        // Переходим к следующему состоянию напрямую
      }
    }
    
    // Сброс ожидания второго нажатия через 1 секунду
    if (waitingSecondPress && millis() - firstPressTime > 1000) {
      waitingSecondPress = false;
    }
    
    if (!debugWaitingForButton) return;
  }

  // Сброс FAULT длинным нажатием BOOT
  if (state == FAULT) {
    if (btnStart.readActive() && btnStart.activeDuration() > LONG_PRESS_MS) {
      Serial.println("FAULT reset by long press");
      gotoState(IDLE);
    }
    return;
  }

  // Нажатие BOOT в IDLE → запуск в зависимости от TEST_MODE
if (state == IDLE) {
  if (TEST_MODE == 1) {
    if (btnStart.fell()) { debugStop("Starting centering cycle", RUN_CENTER_FORWARD); }
  } else if (TEST_MODE == 2) {
    if (btnStart.fell()) { runOneCycleRequest = true; gotoState(CALIB_LIFT_UP_TO_N1); }
  } else if (TEST_MODE == 3) {
    if (btnStart.fell()) { debugStop("Starting centering cycle", RUN_CENTER_FORWARD); }
  } else if (TEST_MODE == 4) {
    if (btnStart.fell()) { loopActive = !loopActive; Serial.printf("LOOP MODE %s\n\n", loopActive ? "ON" : "OFF"); if (loopActive) debugStop("Starting centering loop", RUN_CENTER_FORWARD); }
    if (loopActive && (millis() - stateStartMs > 200)) { debugStop("Loop auto-restart", RUN_CENTER_FORWARD); }
  }
  if (TEST_MODE == 2 && runOneCycleRequest) { /* ожидание завершения калибровки */ }
}

  switch (state) {
    // ---------- КАЛИБРОВКА ПРИ СТАРТЕ ----------
    case CALIB_LIFT_UP_TO_N1: {
      // едем вверх до N1
      if (limN1.readActive()) {
        stepLift.setPosition(0);
        Serial.println("Lift homed at N1 -> pos=0");
        // вниз до 100 мм
        stepLift.moveTo(LIFT_STEPS_115);
        debugStop("Lift homed, moving to 100mm", CALIB_LIFT_TO_MAG100);
        break;
      }
      // ехать вверх
      if (!stepLift.isBusy()) {
        stepLift.moveBy(-200000); // достаточно большое число вверх
      }
      if (timeoutPassed(TMO_FIND_N1_MS)) {
        enterFault("Timeout homing Lift N1");
      }
    } break;

    case CALIB_LIFT_TO_MAG100: {
      if (!stepLift.isBusy()) {
        // серво «разжато»
        servoOpen();
        // опускаем разжимной механизм вниз до N2
        gripSet(GRIP_DOWN);
        debugStop("Lift at 100mm, lowering grip", CALIB_GRIP_DOWN_TO_N2);
      } else if (timeoutPassed(TMO_LIFT_MOVE_MS)) {
        enterFault("Timeout lowering Lift to 100mm");
      }
    } break;

    case CALIB_GRIP_DOWN_TO_N2: {
      if (limN2.readActive()) {
        gripSet(GRIP_STOP);
        Serial.println("Grip down at N2 (calibrated)");
        debugStop("Calibration complete", IDLE);
      } else if (timeoutPassed(TMO_GRIP_DOWN_MS)) {
        gripSet(GRIP_STOP);
        enterFault("Timeout GRIP down to N2");
      }
    } break;

    // ---------- ОЖИДАНИЕ ----------
    case IDLE: {
  // ничего; ждём BOOT
  if (TEST_MODE == 2 && runOneCycleRequest) {
    runOneCycleRequest = false;
    debugStop("Starting cycle after calibration", RUN_CENTER_FORWARD);
  }
}
break;

    // ---------- ЦЕНТРОВКА ----------
    case RUN_CENTER_FORWARD: {
      // Запуск движения всех моторов центровки вперед
      if (allCenterMotorsIdle() && centerMotorsAtPosition(0, 0)) {
        // Нижняя ось: M1,M4 - CCW (-), M2,M3 - CW (+)
        stepM1.moveBy(-BOTTOM_AXIS_STEPS);
        stepM4.moveBy(-BOTTOM_AXIS_STEPS);
        stepM2.moveBy(+BOTTOM_AXIS_STEPS);
        stepM3.moveBy(+BOTTOM_AXIS_STEPS);
        
        // Верхняя ось: M5 - CCW (-), M6 - CW (+)
        stepM5.moveBy(-TOP_AXIS_STEPS);
        stepM6.moveBy(+TOP_AXIS_STEPS);
        
        Serial.printf("Centering FORWARD: Bottom=%dmm, Top=%dmm\n", BOTTOM_AXIS_MM, TOP_AXIS_MM);
      }
      
      // Проверка завершения
      if (allCenterMotorsIdle()) {
        bool bottomOk = (stepM1.position() == -BOTTOM_AXIS_STEPS) && 
                       (stepM4.position() == -BOTTOM_AXIS_STEPS) &&
                       (stepM2.position() == +BOTTOM_AXIS_STEPS) && 
                       (stepM3.position() == +BOTTOM_AXIS_STEPS);
        bool topOk = (stepM5.position() == -TOP_AXIS_STEPS) && 
                    (stepM6.position() == +TOP_AXIS_STEPS);
                    
        if (bottomOk && topOk) {
          debugStop("Centering forward complete, returning", RUN_CENTER_BACK);
        }
      }
      
      if (timeoutPassed(TMO_CENTER_MOVE_MS)) {
        enterFault("Timeout centering forward");
      }
    } break;

    case RUN_CENTER_BACK: {
      // Возврат всех моторов в исходное положение
      if (allCenterMotorsIdle()) {
        // Все моторы возвращаются в 0
        stepM1.moveTo(0);
        stepM2.moveTo(0);
        stepM3.moveTo(0);
        stepM4.moveTo(0);
        stepM5.moveTo(0);
        stepM6.moveTo(0);
        
        Serial.println("Centering BACK to home positions");
      }
      
      // Проверка завершения
      if (allCenterMotorsIdle() && centerMotorsAtPosition(0, 0)) {
        debugStop("Centering complete, waiting 60s", WAIT_60S);
        startPause(PAUSE_60S_MS, SWAP_LIFT_UP_TO_N1);
      }
      
      if (timeoutPassed(TMO_CENTER_MOVE_MS)) {
        enterFault("Timeout centering back");
      }
    } break;

    case WAIT_60S: {
      // Обработка происходит в PAUSE_1S_A
    } break;

    // ---------- ЗАМЕНА БАТАРЕИ ----------
    case SWAP_LIFT_UP_TO_N1: {
      if (limN1.readActive()) {
        stepLift.setPosition(0);
        passed100mmOnLift = false;
        debugStop("Swap: Lift at N1, clamping servo", SWAP_SERVO_CLAMP);
      } else {
        if (!stepLift.isBusy()) stepLift.moveBy(-200000);
        if (timeoutPassed(TMO_FIND_N1_MS)) {
          enterFault("Timeout Lift up to N1 (swap)");
        }
      }
    } break;

    case SWAP_SERVO_CLAMP: {
      servoClamp();
      debugStop("Servo clamped, pausing 1s", PAUSE_1S_C);
      startPause(PAUSE_SHORT_MS, SWAP_GRIP_UP_TO_N3);
    } break;

    case SWAP_GRIP_UP_TO_N3: {
      gripSet(GRIP_UP);
      gotoState(PAUSE_1S_B);
      stateAfterPause = SWAP_LIFT_TO_100;
      stateStartMs = millis();
    } break;

    case PAUSE_1S_B: {
      if (limN3.readActive()) {
        gripSet(GRIP_STOP);
        debugStop("Grip at N3, lowering lift to 100mm", SWAP_LIFT_TO_100);
        startPause(PAUSE_SHORT_MS, SWAP_LIFT_TO_100);
      } else if (timeoutPassed(TMO_GRIP_UP_MS)) {
        gripSet(GRIP_STOP);
        enterFault("Timeout GRIP up to N3");
      }
    } break;

    case SWAP_LIFT_TO_100: {
      if (!stepLift.isBusy()) stepLift.moveTo(LIFT_STEPS_115);
      if (!stepLift.isBusy() && stepLift.position()==LIFT_STEPS_115) {
        debugStop("Lift at 100mm, opening servo", SWAP_SERVO_OPEN);
      } else if (timeoutPassed(TMO_LIFT_MOVE_MS)) {
        enterFault("Timeout Lift to 100mm");
      }
    } break;

    case SWAP_SERVO_OPEN: {
      servoOpen();
      debugStop("Servo opened, lowering to 130mm", PAUSE_1S_D);
      startPause(PAUSE_SHORT_MS, SWAP_LIFT_TO_130);
    } break;

    case SWAP_LIFT_TO_130: {
      if (!stepLift.isBusy()) stepLift.moveTo(LIFT_STEPS_145);
      if (!stepLift.isBusy() && stepLift.position()==LIFT_STEPS_145) {
        debugStop("Lift at 130mm, rotating magazine", SWAP_MAG_90);
      } else if (timeoutPassed(TMO_LIFT_MOVE_MS)) {
        enterFault("Timeout Lift to 130mm");
      }
    } break;

    case SWAP_MAG_90: {
      // Логика поворота магазина с автореверсом
      if (!stepMag.isBusy()) {
        magStartPos = stepMag.position();
        long targetPos;
        if (magDirectionCW) {
          targetPos = magStartPos + MAG_STEPS_90;
          Serial.printf("Magazine rotating CW from %ld to %ld\n", magStartPos, targetPos);
        } else {
          targetPos = magStartPos - MAG_STEPS_90;
          Serial.printf("Magazine rotating CCW from %ld to %ld\n", magStartPos, targetPos);
        }
        stepMag.moveTo(targetPos);
      }
      
      if (!stepMag.isBusy()) {
        long currentPos = stepMag.position();
        
        if (currentPos >= MAG_STEPS_360) {
          magDirectionCW = false;
          Serial.println("Magazine reached 360°, next rotation will be CCW");
          if (currentPos > MAG_STEPS_360) {
            stepMag.setPosition(MAG_STEPS_360);
          }
        } else if (currentPos <= 0) {
          magDirectionCW = true;
          Serial.println("Magazine reached 0°, next rotation will be CW");
          if (currentPos < 0) {
            stepMag.setPosition(0);
          }
        }
        
        if (limN4.readActive()) {
          Serial.println("N4 limit switch triggered!");
          magDirectionCW = !magDirectionCW;
          Serial.printf("Direction reversed due to N4, next will be %s\n", 
                       magDirectionCW ? "CW" : "CCW");
        }
        
        debugStop("Magazine rotated 90°, lifting with servo action", SWAP_LIFT_UP_TO_N1_WITH_SERVO_ON_100);
      } else if (timeoutPassed(TMO_MAG_90_MS)) {
        enterFault("Timeout Magazine 90deg");
      }
    } break;

    case SWAP_LIFT_UP_TO_N1_WITH_SERVO_ON_100: {
      if (!stepLift.isBusy()) stepLift.moveBy(-200000);
      long pos = stepLift.position();
      if (!passed100mmOnLift && pos <= LIFT_STEPS_115) {
        servoClamp();
        passed100mmOnLift = true;
        Serial.println("Passed 100mm level, servo clamped");
      }
      if (limN1.readActive()) {
        stepLift.setPosition(0);
        debugStop("Lift at N1 with battery, lowering grip", SWAP_GRIP_DOWN_TO_N2);
      } else if (timeoutPassed(TMO_FIND_N1_MS)) {
        enterFault("Timeout Lift up with servo action");
      }
    } break;

    case SWAP_GRIP_DOWN_TO_N2: {
      gripSet(GRIP_DOWN);
      gotoState(PAUSE_1S_H);
      stateStartMs = millis();
    } break;

    case PAUSE_1S_H: {
      if (limN2.readActive()) {
        gripSet(GRIP_STOP);
        debugStop("Grip at N2, opening servo final", SWAP_SERVO_OPEN_FINAL);
        startPause(PAUSE_SHORT_MS, SWAP_SERVO_OPEN_FINAL);
      } else if (timeoutPassed(TMO_GRIP_DOWN_MS)) {
        gripSet(GRIP_STOP);
        enterFault("Timeout GRIP down to N2 (swap end)");
      }
    } break;

    case SWAP_SERVO_OPEN_FINAL: {
      servoOpen();
      debugStop("Swap complete, returning to IDLE", PAUSE_1S_I);
      startPause(PAUSE_SHORT_MS, IDLE);
    } break;

    // ---------- ОБЩИЙ ОБРАБОТЧИК ПАУЗЫ ----------
    case PAUSE_1S_A:
    case PAUSE_1S_C:
    case PAUSE_1S_D:
    case PAUSE_1S_E:
    case PAUSE_1S_F:
    case PAUSE_1S_G:
    case PAUSE_1S_I: {
      if ((int32_t)(millis() - pauseUntilMs) >= 0) {
        if (DEBUG_MODE && state != PAUSE_1S_A) {
          char pauseName[50];
          sprintf(pauseName, "Pause completed, continuing to next step");
          debugStop(pauseName, stateAfterPause);
        } else {
          gotoState(stateAfterPause);
        }
      }
    } break;

    // ---------- ОТЛАДОЧНАЯ ОСТАНОВКА ----------
    case DEBUG_WAIT: {
      // обрабатывается в начале loop
    } break;

    // ---------- АВАРИЯ ----------
    case FAULT:
    default:
      // обработка в начале loop
      break;
  }

  // Периодический вывод состояния в режиме отладки (каждые 5 сек)
  static uint32_t lastDebugPrint = 0;
  if (DEBUG_MODE && state != DEBUG_WAIT && state != IDLE && millis() - lastDebugPrint > 5000) {
    lastDebugPrint = millis();
    Serial.println("--- Status ---");
    printCenterMotorPositions();
    printSwapMotorPositions();
    printLimitSwitches();
    Serial.println("--------------");
  }
}




/*
1️⃣ ВКЛЮЧЕНИЕ И ИНИЦИАЛИЗАЦИЯ (0-5 сек) 
=====================================
2️⃣ АВТОМАТИЧЕСКАЯ КАЛИБРОВКА (5-15 сек)
Шаг 1: Калибровка лифта

Лифт начинает подниматься вверх (мотор крутится по часовой)
Движение до срабатывания концевика N1 (верхняя точка)
Позиция обнуляется: position = 0
[ОТЛАДКА] Нажмите BOOT для продолжения

Шаг 2: Опускание на рабочий уровень

Лифт опускается на 115 мм (к уровню магазина)
Серво устанавливается в положение "открыто" (1050 PWM)
[ОТЛАДКА] Нажмите BOOT для продолжения

Шаг 3: Калибровка разжимного механизма

Редуктор опускается вниз до концевика N2
Система готова к работе
[ОТЛАДКА] Нажмите BOOT для продолжения

СОСТОЯНИЕ -> ОЖИДАНИЕ
3️⃣ РЕЖИМ ОЖИДАНИЯ
Система ждет нажатия кнопки BOOT для запуска цикла.

Все моторы остановлены
Серво отключено (не держит PWM)
Минимальное энергопотребление

4️⃣ ЦИКЛ ЦЕНТРОВКИ ДРОНА (нажатие BOOT)
Фаза 1: ЦЕНТРОВКА ВПЕРЕД (15-20 сек)
Нижняя ось (254 мм):

М1, М4: вращаются ПРОТИВ часовой → створки сходятся слева и справа
М2, М3: вращаются ПО часовой → створки сходятся спереди и сзади

Верхняя ось (230 мм):

М5: вращается ПРОТИВ часовой
М6: вращается ПО часовой
→ Верхние захваты сходятся

ЦЕНТРОВКА ВПЕРЕД: Нижняя=254мм, Верхняя=230мм
Нижняя ось: М1=-25400, М2=25400, М3=25400, М4=-25400
Верхняя ось: М5=-23000, М6=23000
[ОТЛАДКА] Нажмите BOOT для продолжения
🚁 Дрон зафиксирован по центру платформы!
Фаза 2: ВОЗВРАТ В ИСХОДНОЕ (15-20 сек)
Все моторы возвращаются в позицию 0:

М1-М4: расходятся в исходное положение
М5-М6: расходятся в исходное положение

ЦЕНТРОВКА НАЗАД в исходные позиции
Все позиции = 0
[ОТЛАДКА] Нажмите BOOT для продолжения
Фаза 3: ОЖИДАНИЕ 60 СЕКУНД
СОСТОЯНИЕ -> ОЖИДАНИЕ 60 секунд
В это время можно выполнять операции с дроном (зарядка, диагностика и т.д.)
5️⃣ АВТОМАТИЧЕСКАЯ ЗАМЕНА БАТАРЕИ (после 60 сек)
Этап 1: ИЗВЛЕЧЕНИЕ СТАРОЙ БАТАРЕИ

Подъем лифта к дрону

Лифт поднимается до N1 (верх)
[ОТЛАДКА] "Замена: Лифт в N1, зажим серво"


Захват батареи

Серво зажимается (1400 PWM) - захватывает батарею
Пауза 1 сек для стабилизации
[ОТЛАДКА] "Серво зажат, пауза 1с"


Освобождение от дрона

Редуктор поднимается до N3 - разжимает крепление в дроне
[ОТЛАДКА] "Редуктор в N3, опускание лифта"


Опускание с батареей

Лифт опускается на 115 мм (уровень магазина)
[ОТЛАДКА] "Лифт на 115мм, открытие серво"


Отпускание старой батареи

Серво открывается (1050 PWM)
Лифт опускается на 145 мм (ниже магазина)
[ОТЛАДКА] "Лифт на 145мм, поворот магазина"



Этап 2: ПОВОРОТ МАГАЗИНА
Магазин вращается ПРОТИВ ЧАСОВОЙ с 0 до -50

Магазин поворачивается на 90° против часовой
Если достигнут -360° или сработал N4 → следующий раз будет по часовой
[ОТЛАДКА] "Магазин повернут на 90°"

Этап 3: УСТАНОВКА НОВОЙ БАТАРЕИ

Подъем за новой батареей

Лифт поднимается вверх
При прохождении 115 мм → серво зажимает новую батарею

Пройден уровень 115мм, серво зажат

Продолжает подъем до N1
[ОТЛАДКА] "Лифт в N1 с батареей"


Установка в дрон

Редуктор опускается до N2 - фиксирует батарею в дроне
[ОТЛАДКА] "Редуктор в N2, финальное открытие"


Освобождение батареи

Серво открывается (1400 PWM)
Батарея установлена!
[ОТЛАДКА] "Замена завершена"



6️⃣ ВОЗВРАТ В ОЖИДАНИЕ
СОСТОЯНИЕ -> ОЖИДАНИЕ
Система готова к следующему циклу. Нажмите BOOT для повтора.
📊 Временная диаграмма полного цикла:
0:00 - Включение
0:05 - Калибровка (15 сек)
0:20 - Ожидание (нажмите BOOT)
0:25 - Центровка вперед (20 сек)
0:45 - Центровка назад (20 сек)  
1:05 - Ожидание 60 сек
2:05 - Извлечение старой батареи (30 сек)
2:35 - Поворот магазина (5 сек)
2:40 - Установка новой батареи (30 сек)
3:10 - Завершение, ожидание
Общее время цикла: ~3 минуты 10 секунд
🎮 Управление в режиме отладки:

BOOT x1 = продолжить выполнение
BOOT x2 быстро = пропустить текущее действие
BOOT удержание 2 сек = сброс ошибки

⚡ Аварийные ситуации:
Если что-то пошло не так:

Система остановит все моторы
Серво откроется (безопасное положение)
Выведет сообщение об ошибке с указанием причины
Удерживайте BOOT 2 секунды для сброса


*/