Gdzie tu jest blad?


#include "FastAccelStepper.h"  // Biblioteka do sterowania silnikami krokowymi
#include <Preferences.h>       // Biblioteka do przechowywania ustawień
#include <Arduino.h>

#define NUM_STEPPERS 3  // Liczba silników krokowych

const int limitSwitchPin1 = 7;  // krańcówki
const int limitSwitchPin2 = 6;
const int limitSwitchPin3 = 5;

const long CALIBRATION_TOLERANCE = 10;
volatile bool calibrationInProgress = false;

// Definicja struktury silnika krokowego
struct StepperMotor {
  FastAccelStepper* stepper;
  int stepPin;
  int dirPin;
  int calibrationPin;
  int alarmPin;
  int stepperResolution;  // Kroki na jednostkę (stopnie lub milimetry)
  int maxSpeed;
  int stepsMin;
  int stepsMax;
  long zeroOffset;
  long calibrationPosition;
  String name;
  bool dirHigh;
  bool calibrationDirectionForward;  // Kierunek bazowania
  String speedPrefKey;
  String accelerationPrefKey;
  String posPrefKey;
  String zeroOffsetPrefKey;
  String emergencyPosPrefKey;
  int speed;
  int acceleration;
  bool speedModifiedForEncoder;
  int prevSpeed;
  int prevAcceleration;
  int calibrationSpeedFactor;
  int calibrationMoveAwaySteps;
  bool calibrationRunInitialDirectionForward;
  bool calibrationSecondMoveDirectionForward;
  int calibrationLimitSwitchActiveState;
  int calibrationDelayBetweenMoves;
  int calibrationLoopDelay;
  int encoderSpeed;
  int encoderAcceleration;
  long targetPosition;
  unsigned long previousMillis;
  unsigned long previousMillisNewZero;

  // Wędka - prędkości
  enum SpeedMode {
    VERY_FAST,
    FAST,
    MEDIUM_FAST,
    MEDIUM,
    SLOW,
    VERY_SLOW
  };

  SpeedMode currentSpeedMode;  // Aktualny tryb prędkości
};

Preferences preferences;
StepperMotor steppers[NUM_STEPPERS];
FastAccelStepperEngine engine = FastAccelStepperEngine();

// Definicje pinów
// const int safetyButton = 19; //Wylaczone chwilowo
const int encoderPinA = 42;
const int encoderPinB = 41;
const int XaxisPin = 38;
const int YaxisPin = 37;
const int ZaxisPin = 36;
const int multiplier1Pin = 35;
const int multiplier10Pin = 45;

// Zmienne globalne
int pinAstateCurrent = LOW;
int pinAStateLast = LOW;

const unsigned long interval = 100;
const unsigned long intervalNewZero = 100;

bool processEncoderActive = false;
// bool safetyLocked = false;  // Flaga blokady bezpieczeństwa //wylaczone chwilowo

// Flaga trybu LIVE
volatile bool liveModeActive = false;

int liveDelayOffset = 5;    // delay po wyświetleniu P10
int liveDelayAbsolute = 5;  // delay po wyświetleniu P1

void checkLimitSwitchConnectivity();
void stopStepperMotor(StepperMotor& motor);
void stopAllSteppers();
void checkStopInLoop();

void checkLimitSwitchConnectivity() {
  if (calibrationInProgress) return;

  const int indices[] = { 0, 2 };  // silniki M (0) i L (2)
  for (int idx : indices) {
    StepperMotor& m = steppers[idx];
    long pos = m.stepper->getCurrentPosition();
    int pinState = digitalRead(m.calibrationPin);

    // jeśli jesteśmy dalej niż tolerancja od pozycji kalibracyjnej...
    if (abs(pos - m.calibrationPosition) > CALIBRATION_TOLERANCE) {
      // ...i pin wskazuje na aktywację (LOW) => rozłączenie/przerwanie
      if (pinState == m.calibrationLimitSwitchActiveState) {
        Serial.print("Krańcówka silnika ");
        Serial.print(m.name);
        Serial.println(" ROZŁĄCZONA!");
      }
    }
  }
}

void startLive(float degrees) {

  if (liveModeActive) return;

  long steps = long(degrees * steppers[0].stepperResolution);
  long currentPos = steppers[0].stepper->getCurrentPosition();
  long targetPos = currentPos + steps;

  if (targetPos < steppers[0].stepsMin || targetPos > steppers[0].stepsMax) {
    Serial.println("LIVE: ruch poza zakresem—nie wykonano.");
    return;
  }

  liveModeActive = true;
  steppers[0].stepper->move(steps);

  while (steppers[0].stepper->isRunning()) {
    checkStopInLoop();
    enforceLimits();
    safetyCheck();
    {
      long stepsOff = steppers[0].stepper->getCurrentPosition() - steppers[0].zeroOffset;
      float degOff = float(stepsOff) / steppers[0].stepperResolution;
      Serial.print("P10: ");
      Serial.println(degOff, 3);
      delay(liveDelayOffset);
    }
    {
      long stepsAbs = steppers[0].stepper->getCurrentPosition();
      float degAbs = float(stepsAbs) / steppers[0].stepperResolution;
      Serial.print("P1:  ");
      Serial.println(degAbs, 3);
      delay(liveDelayAbsolute);
    }
  }
  liveModeActive = false;
}

void setup() {
  Serial.begin(460800);
  setupPins();
  engine.init();
  preferences.begin("stepper", false);
  setupSteppers();
  readPreferences();
  Serial.println("Setup zakończony.");
}

void loop() {
  checkStopInLoop();
  if (liveModeActive) {
    return;
  }
  /*
  if (!safetyLocked) {
    enforceLimits();
    safetyCheck();
    checkLimitSwitchConnectivity();
    if (processEncoderActive) {
      processEncoderAndMoveSteppers();
    }
    if (Serial.available() > 0) {
      processSerialCommand();
    }
  } else {
    if (Serial.available() > 0) {
      String command = Serial.readStringUntil('\n');
      command.trim();
      if (command == "Odblokuj") {
        unlockSafety();
      } else if (command == "reset") {
        resetESP32();
      } else {
        Serial.println("System zablokowany. Użyj komendy 'Odblokuj' lub 'reset'.");
      }
    }
  }
} */
  //Obsługa enkodera i komend niewyłączona
  if (processEncoderActive) {
    processEncoderAndMoveSteppers();
  }
  if (Serial.available() > 0) {
    processSerialCommand();
  }
}

// Funkcje konfigurujące
void setupPins() {
  // pinMode(safetyButton, INPUT_PULLUP); //czasowo wylaczone
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(XaxisPin, INPUT_PULLDOWN);
  pinMode(YaxisPin, INPUT_PULLDOWN);
  pinMode(ZaxisPin, INPUT_PULLDOWN);
  pinMode(multiplier1Pin, INPUT_PULLDOWN);
  pinMode(multiplier10Pin, INPUT_PULLDOWN);
  pinMode(limitSwitchPin1, INPUT_PULLUP);
  pinMode(limitSwitchPin2, INPUT_PULLUP);
  pinMode(limitSwitchPin3, INPUT_PULLUP);

  /* for (int i = 0; i < NUM_STEPPERS; ++i) {
    pinMode(steppers[i].alarmPin, INPUT_PULLDOWN);
  }
  */
  //zakomentowane alarmy
}

void setupSteppers() {
  steppers[0] = {
    .stepper = nullptr,
    .stepPin = 3,
    .dirPin = 8,
    .calibrationPin = 7,
    .alarmPin = 46,
    .stepperResolution = 320,
    .maxSpeed = 64000,
    .stepsMin = -33024,
    .stepsMax = 38400,
    .zeroOffset = 0,
    .calibrationPosition = -33024,
    .name = "M",
    .dirHigh = false,
    .calibrationDirectionForward = false,
    .speedPrefKey = "speed1",
    .accelerationPrefKey = "acceleration1",
    .posPrefKey = "pos1",
    .zeroOffsetPrefKey = "zeroOffset1",
    .emergencyPosPrefKey = "emergencyPos1",
    .speed = 0,
    .acceleration = 0,
    .calibrationSpeedFactor = 50,
    .calibrationMoveAwaySteps = 1000,
    .calibrationRunInitialDirectionForward = false,
    .calibrationSecondMoveDirectionForward = false,
    .calibrationLimitSwitchActiveState = LOW,
    .calibrationDelayBetweenMoves = 500,
    .calibrationLoopDelay = 0,
    .encoderSpeed = 32000,
    .encoderAcceleration = 16000,
    .targetPosition = 0,
    .previousMillis = 0,
    .previousMillisNewZero = 0
  };

  steppers[1] = {
    .stepper = nullptr,
    .stepPin = 16,
    .dirPin = 15,
    .calibrationPin = 6,
    .alarmPin = 9,
    .stepperResolution = 320,
    .maxSpeed = 32000,
    .stepsMin = -32000,
    .stepsMax = 0,
    .zeroOffset = 0,
    .calibrationPosition = 0,
    .name = "N",
    .dirHigh = false,
    .calibrationDirectionForward = true,
    .speedPrefKey = "speed2",
    .accelerationPrefKey = "acceleration2",
    .posPrefKey = "pos2",
    .zeroOffsetPrefKey = "zeroOffset2",
    .emergencyPosPrefKey = "emergencyPos2",
    .speed = 0,
    .acceleration = 0,
    .calibrationSpeedFactor = 25,
    .calibrationMoveAwaySteps = -1600,
    .calibrationRunInitialDirectionForward = true,
    .calibrationSecondMoveDirectionForward = true,
    .calibrationLimitSwitchActiveState = HIGH,
    .calibrationDelayBetweenMoves = 500,
    .calibrationLoopDelay = 0,
    .encoderSpeed = 32000,
    .encoderAcceleration = 32000,
    .targetPosition = 0,
    .previousMillis = 0,
    .previousMillisNewZero = 0
  };

  steppers[2] = {
    .stepper = nullptr,
    .stepPin = 18,
    .dirPin = 17,
    .calibrationPin = 5,
    .alarmPin = 10,
    .stepperResolution = 260,
    .maxSpeed = 26000,
    .stepsMin = -31200,
    .stepsMax = 31200,
    .zeroOffset = 0,
    .calibrationPosition = -31200,
    .name = "L",
    .dirHigh = true,
    .calibrationDirectionForward = false,
    .speedPrefKey = "speed3",
    .accelerationPrefKey = "acceleration3",
    .posPrefKey = "pos3",
    .zeroOffsetPrefKey = "zeroOffset3",
    .emergencyPosPrefKey = "emergencyPos3",
    .speed = 0,
    .acceleration = 0,
    .calibrationSpeedFactor = 20,
    .calibrationMoveAwaySteps = 2600,
    .calibrationRunInitialDirectionForward = false,
    .calibrationSecondMoveDirectionForward = false,
    .calibrationLimitSwitchActiveState = HIGH,
    .calibrationDelayBetweenMoves = 500,
    .calibrationLoopDelay = 500,
    .encoderSpeed = 32000,
    .encoderAcceleration = 32000,
    .targetPosition = 0,
    .previousMillis = 0,
    .previousMillisNewZero = 0
  };

  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper = engine.stepperConnectToPin(steppers[i].stepPin);
    if (steppers[i].stepper) {
      steppers[i].stepper->setDirectionPin(steppers[i].dirPin, steppers[i].dirHigh);
      steppers[i].stepper->setAcceleration(steppers[i].acceleration);
      steppers[i].stepper->setSpeedInHz(steppers[i].speed);
      steppers[i].stepper->setCurrentPosition(steppers[i].targetPosition);
      steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition();
      steppers[i].currentSpeedMode = StepperMotor::MEDIUM;
      Serial.print("Silnik ");
      Serial.print(steppers[i].name);
      Serial.println(" zainicjalizowany.");
    } else {
      Serial.print("Błąd inicjalizacji silnika ");
      Serial.println(steppers[i].name);
    }
  }
}

void readPreferences() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    StepperMotor& motor = steppers[i];
    motor.speed = preferences.getInt(motor.speedPrefKey.c_str(), motor.maxSpeed / 10);
    motor.acceleration = preferences.getInt(motor.accelerationPrefKey.c_str(), motor.speed / 2);
    motor.prevSpeed = motor.speed;
    motor.prevAcceleration = motor.acceleration;
    motor.speedModifiedForEncoder = false;
    long pos = preferences.getLong(motor.posPrefKey.c_str(), 0);
    motor.zeroOffset = preferences.getLong(motor.zeroOffsetPrefKey.c_str(), 0);

    if (motor.stepper) {
      motor.stepper->setSpeedInHz(motor.speed);
      motor.stepper->setAcceleration(motor.acceleration);
      motor.stepper->setCurrentPosition(pos);
      motor.targetPosition = pos;
      Serial.print("Preferencje odczytane dla silnika ");
      Serial.println(motor.name);
    }
  }
}

void enforceLimits() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    StepperMotor& motor = steppers[i];
    long pos = motor.stepper->getCurrentPosition();
    if (pos < motor.stepsMin || pos > motor.stepsMax) {
      motor.stepper->forceStop();
      if (pos < motor.stepsMin) {
        motor.stepper->setCurrentPosition(motor.stepsMin);
        motor.targetPosition = motor.stepsMin;
        Serial.print("Silnik ");
        Serial.print(motor.name);
        Serial.println(" przekroczył minimalny zakres i został ustawiony na stepsMin.");
      } else {
        motor.stepper->setCurrentPosition(motor.stepsMax);
        motor.targetPosition = motor.stepsMax;
        Serial.print("Silnik ");
        Serial.print(motor.name);
        Serial.println(" przekroczył maksymalny zakres i został ustawiony na stepsMax.");
      }
      Serial.print("Silnik ");
      Serial.print(motor.name);
      Serial.println(" został zatrzymany z powodu przekroczenia granic.");
    }
  }
}

void safetyCheck() {
  /*
  bool safetyTriggered = false;
  if (digitalRead(safetyButton) == LOW) {
    safetyTriggered = true;
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(steppers[i].alarmPin) == LOW) {
      safetyTriggered = true;
    }
  }
  if (safetyTriggered) {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->forceStop();
      long pos = steppers[i].stepper->getCurrentPosition();
      preferences.putLong(steppers[i].posPrefKey.c_str(), pos);
      preferences.putLong(steppers[i].emergencyPosPrefKey.c_str(), pos);
      steppers[i].targetPosition = pos;
    }
    Serial.println("Silniki zatrzymane z powodu alarmu bezpieczeństwa. Proszę odblokować lub zresetować system.");
    safetyLocked = true;
  }
  */
}

void unlockSafety() {
  /*
  if (safetyLocked) {
    safetyLocked = false;
    Serial.println("System odblokowany. Możesz kontynuować pracę.");
  } else {
    Serial.println("System nie jest zablokowany.");
  }
  */
}

void resetESP32() {
  Serial.println("Restartowanie ESP32...");
  delay(1000);
  ESP.restart();
}

void processEncoderAndMoveSteppers() {
  checkStopInLoop();
  pinAstateCurrent = digitalRead(encoderPinA);
  static long stepsForSteppers[NUM_STEPPERS] = { 0 };
  static unsigned long lastEncoderEventTime = 0;
  static unsigned long prevEncoderEventTime = 0;
  static unsigned long deltaT = 1000;

  if (pinAStateLast == LOW && pinAstateCurrent == HIGH) {
    int stepIncrement = (digitalRead(encoderPinB) == HIGH) ? -1 : 1;
    prevEncoderEventTime = lastEncoderEventTime;
    lastEncoderEventTime = millis();
    if (prevEncoderEventTime != 0) {
      deltaT = lastEncoderEventTime - prevEncoderEventTime;
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      stepsForSteppers[i] += stepIncrement * steppers[i].stepperResolution;
    }
  }
  pinAStateLast = pinAstateCurrent;

  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (stepsForSteppers[i] != 0) {
      int axisPin = (i == 0 ? XaxisPin : i == 1 ? YaxisPin
                                                : ZaxisPin);
      if (digitalRead(axisPin) == HIGH) {
        float multiplier = 1.0;
        if (digitalRead(multiplier1Pin) == HIGH) multiplier = 0.1;
        else if (digitalRead(multiplier10Pin) == HIGH) multiplier = 0.01;

        long movement = stepsForSteppers[i] * multiplier;
        long newTargetPosition = steppers[i].targetPosition + movement;

        if (newTargetPosition >= steppers[i].stepsMin && newTargetPosition <= steppers[i].stepsMax) {
          // histereza...
          StepperMotor::SpeedMode newMode = steppers[i].currentSpeedMode;
          const unsigned long veryFastThreshold = 30;
          const unsigned long fastThreshold = 50;
          const unsigned long mediumFastThreshold = 100;
          const unsigned long mediumThreshold = 150;
          const unsigned long slowThreshold = 200;
          switch (steppers[i].currentSpeedMode) {
            case StepperMotor::VERY_FAST:
              if (deltaT > veryFastThreshold) newMode = StepperMotor::FAST;
              break;
            case StepperMotor::FAST:
              if (deltaT > fastThreshold) newMode = StepperMotor::MEDIUM_FAST;
              else if (deltaT <= veryFastThreshold) newMode = StepperMotor::VERY_FAST;
              break;
            case StepperMotor::MEDIUM_FAST:
              if (deltaT > mediumFastThreshold) newMode = StepperMotor::MEDIUM;
              else if (deltaT <= fastThreshold) newMode = StepperMotor::FAST;
              break;
            case StepperMotor::MEDIUM:
              if (deltaT > mediumThreshold) newMode = StepperMotor::SLOW;
              else if (deltaT <= mediumFastThreshold) newMode = StepperMotor::MEDIUM_FAST;
              break;
            case StepperMotor::SLOW:
              if (deltaT > slowThreshold) newMode = StepperMotor::VERY_SLOW;
              else if (deltaT <= mediumThreshold) newMode = StepperMotor::MEDIUM;
              break;
            case StepperMotor::VERY_SLOW:
              if (deltaT <= slowThreshold) newMode = StepperMotor::SLOW;
              break;
            default:
              newMode = StepperMotor::MEDIUM;
          }
          if (newMode != steppers[i].currentSpeedMode) {
            steppers[i].currentSpeedMode = newMode;
            steppers[i].speedModifiedForEncoder = true;
          }

          int newSpeed, newAcceleration;
          switch (steppers[i].currentSpeedMode) {
            case StepperMotor::VERY_FAST:
              newSpeed = steppers[i].encoderSpeed;
              newAcceleration = steppers[i].encoderAcceleration;
              break;
            case StepperMotor::FAST:
              newSpeed = steppers[i].encoderSpeed / 2;
              newAcceleration = steppers[i].encoderAcceleration / 2;
              break;
            case StepperMotor::MEDIUM_FAST:
              newSpeed = steppers[i].encoderSpeed / 6;
              newAcceleration = steppers[i].encoderAcceleration / 4;
              break;
            case StepperMotor::MEDIUM:
              newSpeed = steppers[i].encoderSpeed / 8;
              newAcceleration = steppers[i].encoderAcceleration / 6;
              break;
            case StepperMotor::SLOW:
              newSpeed = steppers[i].encoderSpeed / 10;
              newAcceleration = steppers[i].encoderAcceleration / 8;
              break;
            case StepperMotor::VERY_SLOW:
              newSpeed = steppers[i].encoderSpeed / 12;
              newAcceleration = steppers[i].encoderAcceleration / 10;
              break;
            default:
              newSpeed = steppers[i].encoderSpeed;
              newAcceleration = steppers[i].encoderAcceleration;
          }

          if (steppers[i].speedModifiedForEncoder) {
            steppers[i].prevSpeed = steppers[i].speed;
            steppers[i].prevAcceleration = steppers[i].acceleration;
            steppers[i].speedModifiedForEncoder = false;
          }
          if (steppers[i].speed != newSpeed || steppers[i].acceleration != newAcceleration) {
            steppers[i].speed = newSpeed;
            steppers[i].acceleration = newAcceleration;
            steppers[i].stepper->setSpeedInHz(newSpeed);
            steppers[i].stepper->setAcceleration(newAcceleration);
          }

          steppers[i].targetPosition = newTargetPosition;
          steppers[i].stepper->move(movement);
        }
        stepsForSteppers[i] = 0;
      }
    }
    // przywracanie oryginalnych prędkości po zakończeniu ruchu
    if (steppers[i].speedModifiedForEncoder && !steppers[i].stepper->isRunning()) {
      steppers[i].speed = steppers[i].prevSpeed;
      steppers[i].acceleration = steppers[i].prevAcceleration;
      steppers[i].stepper->setSpeedInHz(steppers[i].speed);
      steppers[i].stepper->setAcceleration(steppers[i].acceleration);
      steppers[i].speedModifiedForEncoder = false;
    }
  }
}

void powP10(int times = 500) {
  for (int i = 0; i < times; ++i) {
    steppers[0].previousMillisNewZero = 0;
    Serial.print("P10: ");
    printPositionZero(steppers[0]);
    delay(intervalNewZero);
  }
}

void printLimits(StepperMotor& motor) {
  float minPos = (float)motor.stepsMin / motor.stepperResolution;
  float maxPos = (float)motor.stepsMax / motor.stepperResolution;
  Serial.print("Silnik ");
  Serial.print(motor.name);
  Serial.print(" — min: ");
  Serial.print(minPos, 3);
  Serial.print("  max: ");
  Serial.println(maxPos, 3);
}

void limitsAll() {
  Serial.println("=== Limity wszystkich silników ===");
  printLimits(steppers[0]);
  printLimits(steppers[1]);
  printLimits(steppers[2]);
  Serial.println("==================================");
}



void processSerialCommand() {
  String command = Serial.readStringUntil('\n');
  command.trim();
  if (command == "STOP1") {
    stopStepperMotor(steppers[0]);
    return;
  } else if (command == "STOP2") {
    stopStepperMotor(steppers[1]);
    return;
  } else if (command == "STOP3") {
    stopStepperMotor(steppers[2]);
    return;
  } else if (command == "STOPALL") {
    stopAllSteppers();
    return;
  } else if (command.startsWith("speed1")) {
    Serial.print("speed1: ");
    float speed = command.substring(6).toFloat();
    changeSpeed(steppers[0], speed);
  } else if (command.startsWith("speed2")) {
    Serial.print("speed2: ");
    float speed = command.substring(6).toFloat();
    changeSpeed(steppers[1], speed);
  } else if (command.startsWith("speed3")) {
    Serial.print("speed3: ");
    float speed = command.substring(6).toFloat();
    changeSpeed(steppers[2], speed);
  } else if (command.startsWith("acceleration1")) {
    Serial.print("acceleration1: ");
    int acc = command.substring(13).toInt();
    changeAcceleration(steppers[0], acc);
  } else if (command.startsWith("acceleration2")) {
    Serial.print("acceleration2: ");
    int acc = command.substring(13).toInt();
    changeAcceleration(steppers[1], acc);
  } else if (command.startsWith("acceleration3")) {
    Serial.print("acceleration3: ");
    int acc = command.substring(13).toInt();
    changeAcceleration(steppers[2], acc);
  } else if (command == "H") {
    Serial.print("H: ");
    calibration(steppers[0]);
  } else if (command == "J") {
    Serial.print("J: ");
    calibration(steppers[1]);
  } else if (command == "G") {
    Serial.print("G: ");
    calibration(steppers[2]);
  } else if (command == "H1") {
    Serial.print("H1: ");
    returnToZero(steppers[0]);
  } else if (command == "J1") {
    Serial.print("J1: ");
    returnToZero(steppers[1]);
  } else if (command == "G1") {
    Serial.print("G1: ");
    returnToZero(steppers[2]);
  } else if (command == "H2") {
    Serial.print("H2: ");
    returnToOffsetZero(steppers[0]);
  } else if (command == "J2") {
    Serial.print("J2: ");
    returnToOffsetZero(steppers[1]);
  } else if (command == "G2") {
    Serial.print("G2: ");
    returnToOffsetZero(steppers[2]);
  } else if (command == "H3") {
    Serial.print("H3: ");
    returnToEmergencyPosition(steppers[0]);
  } else if (command == "J3") {
    Serial.print("J3: ");
    returnToEmergencyPosition(steppers[1]);
  } else if (command == "G3") {
    Serial.print("G3: ");
    returnToEmergencyPosition(steppers[2]);
  } else if (command == "C") {
    Serial.print("C: ");
    calibrationAll();
  } else if (command == "R") {
    Serial.print("R: ");
    returnToZeroAll();
  } else if (command == "T") {
    Serial.print("T: ");
    returnToOffsetZeroAll();
  } else if (command == "Y") {
    Serial.print("Y: ");
    returnToEmergencyPositionAll();
  } else if (command == "A") {
    Serial.print("A: ");
    setNewZero(steppers[0]);
  } else if (command == "S") {
    Serial.print("S: ");
    setNewZero(steppers[1]);
  } else if (command == "D") {
    Serial.print("D: ");
    setNewZero(steppers[2]);
  } else if (command == "P") {
    positionAll();
  } else if (command == "P1") {
    Serial.print("P1: ");
    printPosition(steppers[0]);
  } else if (command == "P2") {
    Serial.print("P2: ");
    printPosition(steppers[1]);
  } else if (command == "P3") {
    Serial.print("P3: ");
    printPosition(steppers[2]);
  } else if (command == "P0") {
    positionAllZero();
  } else if (command == "P10") {
    Serial.print("P10: ");
    printPositionZero(steppers[0]);
  } else if (command == "P20") {
    Serial.print("P20: ");
    printPositionZero(steppers[1]);
  } else if (command == "P30") {
    Serial.print("P30: ");
    printPositionZero(steppers[2]);
  } else if (command == "EP") {
    positionAllEmergency();
  } else if (command == "EP1") {
    Serial.print("EP1: ");
    printEmergencyPosition(steppers[0]);
  } else if (command == "EP2") {
    Serial.print("EP2: ");
    printEmergencyPosition(steppers[1]);
  } else if (command == "EP3") {
    Serial.print("EP3: ");
    printEmergencyPosition(steppers[2]);
  } else if (command == "L") {
    limitsAll();
  } else if (command == "L1") {
    Serial.print("L1: ");
    printLimits(steppers[0]);
  } else if (command == "L2") {
    Serial.print("L2: ");
    printLimits(steppers[1]);
  } else if (command == "L3") {
    Serial.print("L3: ");
    printLimits(steppers[2]);

    else if (command == "F") {
      Serial.print("F: ");
      processEncoderActive = true;
      Serial.println("Jog activate");
    }
    else if (command == "F1") {
      Serial.print("F1: ");
      processEncoderActive = false;
      Serial.println("Jog deactivate");
    }
    else if (command == "X") {
      Serial.print("X: ");
      checkSafety();
    }
    else if (command.equalsIgnoreCase("Odblokuj")) {
      unlockSafety();
    }
    else if (command.equalsIgnoreCase("reset")) {
      resetESP32();
    }
    else if (command == "baza1") {
      setCurrentPositionToCalibration(steppers[0]);
    }
    else if (command == "baza2") {
      setCurrentPositionToCalibration(steppers[1]);
    }
    else if (command == "baza3") {
      setCurrentPositionToCalibration(steppers[2]);
    }
    else if (command.startsWith("getSpeed1")) {
      Serial.print("getSpeed1: ");
      getSpeed(steppers[0]);
    }
    else if (command.startsWith("getSpeed2")) {
      Serial.print("getSpeed2: ");
      getSpeed(steppers[1]);
    }
    else if (command.startsWith("getSpeed3")) {
      Serial.print("getSpeed3: ");
      getSpeed(steppers[2]);
    }
    else if (command.startsWith("getMaxSpeed1")) {
      Serial.print("maxSpeedStepper1: ");
      getMaxSpeed(steppers[0]);
    }
    else if (command.startsWith("getMaxSpeed2")) {
      Serial.print("maxSpeedStepper2: ");
      getMaxSpeed(steppers[1]);
    }
    else if (command.startsWith("getMaxSpeed3")) {
      Serial.print("maxSpeedStepper3: ");
      getMaxSpeed(steppers[2]);
    }
    else if (command.startsWith("move1Steps")) {
      int steps = command.substring(11).toInt();
      moveStepperSteps(steppers[0], steps);
    }
    else if (command.startsWith("move2Steps")) {
      int steps = command.substring(11).toInt();
      moveStepperSteps(steppers[1], steps);
    }
    else if (command.startsWith("move3Steps")) {
      int steps = command.substring(11).toInt();
      moveStepperSteps(steppers[2], steps);
    }
    else if (command.startsWith("LIVE")) {
      float deg = command.substring(4).toFloat();
      Serial.print("Starting LIVE for ");
      Serial.print(deg, 2);
      Serial.println(" degrees");
      startLive(deg);
    }
    else if (command == "P10R") {
      powP10(500);
    }
    else if (command.startsWith("liveDelayOffset")) {
      int v = command.substring(16).toInt();
      liveDelayOffset = v;
      Serial.print("Ustawiono liveDelayOffset = ");
      Serial.println(liveDelayOffset);
    }
    else if (command.startsWith("liveDelayAbsolute")) {
      int v = command.substring(18).toInt();
      liveDelayAbsolute = v;
      Serial.print("Ustawiono liveDelayAbsolute = ");
      Serial.println(liveDelayAbsolute);
    }
    else {
      controlStepperMotor(command);
    }
  }

  void controlStepperMotor(String command) {
    if (command.length() < 3) {
      Serial.println("Niepoprawny format komendy.");
      return;
    }
    checkStopInLoop();
    char motorId = command.charAt(0);
    float value = command.substring(2).toFloat();
    Serial.println(String(motorId) + ": " + String(value));
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      checkStopInLoop();
      if (steppers[i].name.charAt(0) == motorId) {
        long steps = value * steppers[i].stepperResolution;
        long newTargetPosition = steppers[i].targetPosition + steps;
        if (newTargetPosition >= steppers[i].stepsMin && newTargetPosition <= steppers[i].stepsMax) {
          steppers[i].targetPosition = newTargetPosition;
          steppers[i].stepper->move(steps);
          while (steppers[i].stepper->isRunning()) {
            checkStopInLoop();  // <-- tu zatrzymaj silnik, jeśli padnie STOP
            delay(1);           // krótki oddech, żeby nie zablokować całkowicie CPU
          }
        } else {
          Serial.print("Ruch poza zakresem dla silnika ");
          Serial.println(steppers[i].name);
        }
        break;
      }
    }
  }

  void changeSpeed(StepperMotor & motor, float newSpeed) {
    int speedInHz = motor.stepperResolution * newSpeed;
    if (speedInHz > motor.maxSpeed) {
      speedInHz = motor.maxSpeed;
      Serial.println("maxSpeedStepper: " + String(motor.maxSpeed / motor.stepperResolution));
    } else {
      Serial.println(newSpeed);
    }
    motor.speed = speedInHz;
    motor.acceleration = speedInHz / 2;
    motor.stepper->setSpeedInHz(motor.speed);
    motor.stepper->setAcceleration(motor.acceleration);
    preferences.putInt(motor.speedPrefKey.c_str(), motor.speed);
    preferences.putInt(motor.accelerationPrefKey.c_str(), motor.acceleration);
  }

  void changeAcceleration(StepperMotor & motor, int newAcceleration) {
    motor.acceleration = newAcceleration;
    motor.stepper->setAcceleration(motor.acceleration);
    preferences.putInt(motor.accelerationPrefKey.c_str(), motor.acceleration);
    Serial.println(newAcceleration);
  }

  void calibration(StepperMotor & motor) {
    calibrationInProgress = true;
    int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
    int calibrationAcceleration = calibrationSpeed / 2;
    motor.stepper->setSpeedInHz(calibrationSpeed);
    motor.stepper->setAcceleration(calibrationAcceleration);
    motor.stepper->applySpeedAcceleration();
    /*
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) {
    safetyCheck();
    return;
  }
  */
    motor.stepper->moveTo(motor.calibrationPosition);
    while (motor.stepper->isRunning()) {
      checkStopInLoop();

      /*
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
    */
      if (digitalRead(motor.calibrationPin) == motor.calibrationLimitSwitchActiveState) {
        motor.stepper->forceStop();
        break;
      }
      delay(motor.calibrationLoopDelay);
    }
    motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
    motor.targetPosition = motor.calibrationPosition;
    delay(motor.calibrationDelayBetweenMoves);
    motor.stepper->move(motor.calibrationMoveAwaySteps);
    while (motor.stepper->isRunning()) {
      checkStopInLoop();
      /*
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
    */
      delay(motor.calibrationLoopDelay);
    }
    motor.targetPosition += motor.calibrationMoveAwaySteps;
    motor.stepper->setSpeedInHz(calibrationSpeed / 2);
    motor.stepper->setAcceleration(calibrationAcceleration / 4);
    motor.stepper->applySpeedAcceleration();
    if (motor.calibrationSecondMoveDirectionForward) {
      motor.stepper->runForward();
    } else {
      motor.stepper->runBackward();
    }
    while (digitalRead(motor.calibrationPin) != motor.calibrationLimitSwitchActiveState) {
      checkStopInLoop();
      /*
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
    */
      delay(motor.calibrationLoopDelay);
    }
    motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
    motor.targetPosition = motor.calibrationPosition;
    motor.stepper->setSpeedInHz(motor.speed);
    motor.stepper->setAcceleration(motor.acceleration);
    motor.stepper->applySpeedAcceleration();
    printPosition(motor);
    calibrationInProgress = false;
  }

  void calibrationAll() {
    calibrationInProgress = true;
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      StepperMotor& motor = steppers[i];
      int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
      int calibrationAcceleration = calibrationSpeed / 2;
      motor.stepper->setSpeedInHz(calibrationSpeed);
      motor.stepper->setAcceleration(calibrationAcceleration);
      motor.stepper->applySpeedAcceleration();
    }
    /*
  if (digitalRead(safetyButton) == LOW) {
    safetyCheck();
    return;
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(steppers[i].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
  }
  */
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->moveTo(steppers[i].calibrationPosition);
    }
    bool allRunning;
    do {
      checkStopInLoop();
      allRunning = false;
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        if (steppers[i].stepper->isRunning()) {
          allRunning = true;
          /*
        if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
          safetyCheck();
          return;
        }
        */
          if (digitalRead(steppers[i].calibrationPin) == steppers[i].calibrationLimitSwitchActiveState) {
            steppers[i].stepper->forceStop();
          }
        }
      }
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        if (steppers[i].stepper->isRunning()) {
          delay(steppers[i].calibrationLoopDelay);
        }
      }
    } while (allRunning);
    { checkStopInLoop(); }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->forceStopAndNewPosition(steppers[i].calibrationPosition);
      steppers[i].targetPosition = steppers[i].calibrationPosition;
    }
    delay(steppers[0].calibrationDelayBetweenMoves);
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->move(steppers[i].calibrationMoveAwaySteps);
      steppers[i].targetPosition += steppers[i].calibrationMoveAwaySteps;
    }
    bool anyRunning;
    do {
      checkStopInLoop();
      anyRunning = false;
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        if (steppers[i].stepper->isRunning()) {
          /*
        anyRunning = true;
        if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
          safetyCheck();
          return;
        }
        */
        }
      }
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        if (steppers[i].stepper->isRunning()) {
          delay(steppers[i].calibrationLoopDelay);
        }
      }
    } while (anyRunning);
    { checkStopInLoop(); }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->setSpeedInHz(steppers[i].calibrationSpeedFactor * steppers[i].stepperResolution / 2);
      steppers[i].stepper->setAcceleration(steppers[i].calibrationSpeedFactor * steppers[i].stepperResolution / 8);
      steppers[i].stepper->applySpeedAcceleration();
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (steppers[i].calibrationSecondMoveDirectionForward) {
        steppers[i].stepper->runForward();
      } else {
        steppers[i].stepper->runBackward();
      }
    }
    bool calibrationComplete[NUM_STEPPERS] = { false };
    while (true) {
      checkStopInLoop();
      bool allComplete = true;
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        if (!calibrationComplete[i]) {
          if (digitalRead(steppers[i].calibrationPin) == steppers[i].calibrationLimitSwitchActiveState) {
            steppers[i].stepper->forceStopAndNewPosition(steppers[i].calibrationPosition);
            steppers[i].targetPosition = steppers[i].calibrationPosition;
            calibrationComplete[i] = true;
          } /*
        if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
          safetyCheck();
          return;
        }
        */
          if (!calibrationComplete[i]) {
            allComplete = false;
            delay(steppers[i].calibrationLoopDelay);
          }
        }
      }
      if (allComplete) break;
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->setSpeedInHz(steppers[i].speed);
      steppers[i].stepper->setAcceleration(steppers[i].acceleration);
      steppers[i].stepper->applySpeedAcceleration();
    }
    positionAll();
    calibrationInProgress = false;
  }

  void returnToZero(StepperMotor & motor) {
    /*
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW) {
    safetyCheck();
    return;
  }
  */
    motor.stepper->moveTo(0);
    motor.targetPosition = 0;
  }

  void returnToZeroAll() {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      /*
    if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
    */
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->moveTo(0);
      steppers[i].targetPosition = 0;
    }
    while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) {
      checkStopInLoop();
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        /*
      if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
        safetyCheck();
        return;
      }
      */
      }
    }
    delay(500);
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      printPosition(steppers[i]);
    }
  }

  void returnToOffsetZero(StepperMotor & motor) {
    checkStopInLoop();
    /*
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW) {
    safetyCheck();
    return;
  }
  */
    motor.stepper->moveTo(motor.zeroOffset);
    motor.targetPosition = motor.zeroOffset;
  }

  void returnToOffsetZeroAll() {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      /*
    if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
    */
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      steppers[i].stepper->moveTo(steppers[i].zeroOffset);
      steppers[i].targetPosition = steppers[i].zeroOffset;
    }
    while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) {
      checkStopInLoop();
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        /*
      if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
        safetyCheck();
        return;
      }
      */
      }
    }
    delay(500);
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      printPositionZero(steppers[i]);
    }
  }

  void returnToEmergencyPosition(StepperMotor & motor) {
    checkStopInLoop();
    /*
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW) {
    safetyCheck();
    return;
  }
  */
    long emergencyPos = preferences.getLong(motor.emergencyPosPrefKey.c_str(), 0);
    motor.stepper->moveTo(emergencyPos);
    motor.targetPosition = emergencyPos;
    Serial.print("Silnik ");
    Serial.print(motor.name);
    Serial.print(" porusza się do pozycji awaryjnej: ");
    Serial.println(emergencyPos);
  }

  void returnToEmergencyPositionAll() {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      /*
    if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
    */
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      long emergencyPos = preferences.getLong(steppers[i].emergencyPosPrefKey.c_str(), 0);
      steppers[i].stepper->moveTo(emergencyPos);
      steppers[i].targetPosition = emergencyPos;
      Serial.print("Silnik ");
      Serial.print(steppers[i].name);
      Serial.print(" porusza się do pozycji awaryjnej: ");
      Serial.println(emergencyPos);
    }
    while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) {
      checkStopInLoop();
      for (int i = 0; i < NUM_STEPPERS; ++i) {
        /*
      if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
        safetyCheck();
        return;
      }
      */
      }
    }
    delay(500);
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      printEmergencyPosition(steppers[i]);
    }
  }

  void setNewZero(StepperMotor & motor) {
    motor.zeroOffset = motor.stepper->getCurrentPosition();
    preferences.putLong(motor.zeroOffsetPrefKey.c_str(), motor.zeroOffset);
    Serial.print("Ustawiono nowe zero dla silnika ");
    Serial.println(motor.name);
  }

  void positionAll() {
    Serial.print("P1: ");
    printPosition(steppers[0]);
    Serial.print("P2: ");
    printPosition(steppers[1]);
    Serial.print("P3: ");
    printPosition(steppers[2]);
  }

  void printPosition(StepperMotor & motor) {
    unsigned long currentMillis = millis();
    if (currentMillis - motor.previousMillis >= interval) {
      motor.previousMillis = currentMillis;
      float position = (float)motor.stepper->getCurrentPosition() / motor.stepperResolution;
      Serial.println(position, 3);
      preferences.putLong(motor.posPrefKey.c_str(), motor.stepper->getCurrentPosition());
    }
  }

  void positionAllZero() {
    Serial.print("P10: ");
    printPositionZero(steppers[0]);
    Serial.print("P20: ");
    printPositionZero(steppers[1]);
    Serial.print("P30: ");
    printPositionZero(steppers[2]);
  }

  void printPositionZero(StepperMotor & motor) {
    unsigned long currentMillis = millis();
    if (currentMillis - motor.previousMillisNewZero >= intervalNewZero) {
      motor.previousMillisNewZero = currentMillis;
      float position = (float)(motor.stepper->getCurrentPosition() - motor.zeroOffset) / motor.stepperResolution;
      Serial.println(position, 3);
    }
  }

  void positionAllEmergency() {
    Serial.print("EP1: ");
    printEmergencyPosition(steppers[0]);
    Serial.print("EP2: ");
    printEmergencyPosition(steppers[1]);
    Serial.print("EP3: ");
    printEmergencyPosition(steppers[2]);
  }

  void printEmergencyPosition(StepperMotor & motor) {
    unsigned long currentMillis = millis();
    if (currentMillis - motor.previousMillis >= interval) {
      motor.previousMillis = currentMillis;
      long emergencyPos = preferences.getLong(motor.emergencyPosPrefKey.c_str(), 0);
      float position = (float)emergencyPos / motor.stepperResolution;
      Serial.println(position, 3);
    }
  }

  void getSpeed(StepperMotor & motor) {
    float speed = (float)motor.speed / motor.stepperResolution;
    Serial.println(speed);
  }

  void getMaxSpeed(StepperMotor & motor) {
    float maxSpeed = (float)motor.maxSpeed / motor.stepperResolution;
    Serial.println(maxSpeed);
  }

  void moveStepperSteps(StepperMotor & motor, int steps) {
    motor.stepper->move(steps);
    motor.targetPosition += steps;
    Serial.print("Silnik ");
    Serial.print(motor.name);
    Serial.print(" porusza się o ");
    Serial.print(steps);
    Serial.println(" kroków.");
  }

  void setCurrentPositionToCalibration(StepperMotor & motor) {
    motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
    motor.targetPosition = motor.calibrationPosition;
    Serial.print("Pozycja silnika ");
    Serial.print(motor.name);
    Serial.print(" została ustawiona na pozycję kalibracyjną: ");
    Serial.println(motor.calibrationPosition);
    preferences.putLong(motor.posPrefKey.c_str(), motor.calibrationPosition);
  }

  void checkSafety() {
    /*
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    int alarmState = digitalRead(steppers[i].alarmPin);
    Serial.print("Stan alarmPin");
    Serial.print(i + 1);
    Serial.print(": ");
    Serial.println(alarmState == LOW ? "LOW" : "HIGH");
  }
  int safetyButtonState = digitalRead(safetyButton);
  Serial.print("Stan safetyButton: ");
  Serial.println(safetyButtonState == LOW ? "LOW" : "HIGH");
  */
  }

  void updateTargetPositions() {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (!steppers[i].stepper->isRunning()) {
        steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition();
        Serial.print("Stepper ");
        Serial.print(steppers[i].name);
        Serial.print(" targetPosition updated to: ");
        Serial.println(steppers[i].targetPosition);
      }
    }
  }

  void stopStepperMotor(StepperMotor & motor) {
    if (motor.stepper) {
      motor.stepper->forceStop();
      motor.targetPosition = motor.stepper->getCurrentPosition();
      Serial.print("Silnik ");
      Serial.print(motor.name);
      Serial.println(" zatrzymany.");
    }
  }

  void stopAllSteppers() {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (steppers[i].stepper) {
        steppers[i].stepper->forceStop();
        steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition();
      }
    }
    Serial.println("Wszystkie silniki zatrzymane.");
  }

  void checkStopInLoop() {
    if (Serial.available() > 0) {
      String cmd = Serial.readStringUntil('\n');
      cmd.trim();
      if (cmd == "STOP1") {
        stopStepperMotor(steppers[0]);
        return;
      } else if (cmd == "STOP2") {
        stopStepperMotor(steppers[1]);
        return;
      } else if (cmd == "STOP3") {
        stopStepperMotor(steppers[2]);
        return;
      } else if (cmd == "STOPALL") {
        stopAllSteppers();
        return;
      }
      // jeśli to nie STOP, oddajemy komendę do normalnego parsera
      processSerialCommand();
    }
    checkLimitSwitchConnectivity();
  }