Модбас на прерывании, как реализовать?

Добрый день! Работаю над реализацией одновременной работы шаговых двигателей (библиотека <AccelStepper.h>) и общения устройств по протоколу модбас (библиотеки что пробовал <ModbusMaster.h> <iarduino_Modbus.h> ). Ситуация такая что при выполнении любой из функций по типу ReadHoldingRegister то ардуино посылает посылку задействуя процессор полностью и запрещает прерывание на данный момент из за чего шаговые двигатели стоят около 50 миллисекунд (скорость обмена 9600). Сейчас уже не знаю что делать и как избавиться от остановки шаговых двигателей я могу уменьшить время изменив скорость 115200 но всё равно будут останавливаться шаговые двигатели а мне этого не нужно. Как избавиться от этого? И возможно ли как то посадить модбас на внутренне прерывание по таймеру дабы опрос шёл отдельно без остановки других процессов?

причем тут прерывание? Вы думаете оно в другой вселенной выполняется и потому мешать не будет? - нет!
Контроллер у вас один, а значит пока идет прерывание, моторы все равно будут стоять.

Надо код опроса Модбас переписывать, чтобы не было блокировки. Или моторы посадить на аппаратный таймер.

В общем, без вашего кода (полностью) вряд ли можно что-то посоветовать.

Хорошо, не подскажете как лучше посадить на аппаратный таймер? Не использовать библиотеку <AccelStepper.h> и писать частотник вручную? Или как то по другому?

что за шаговые и через что подключены?

Nema 17, Tb6600

ну тогда, как советовал @MMM
на STEP/PUL драйвера Tb6600 надо вывести таймер/PWM
схема есть?

То есть отказаться от текущей библиотеки и писать частотник ?

Игорь, чтобы обсуждать что-то - надо видеть текущий код и схему. Без этого не вижу смысла продолжать .

1 лайк

Если скорость двигателей невысока, и, можно за время одного шага переслать байт (а может и больше), то так и делать.

я так понял она меняется.

Отправка не занимает много времени. Буфер порта принимает посылку быстро а уж потом сам через прерывания потихоньку отправляет. Приём тоже не сильно затратен. Разве что разбор принятого. Так что надо смотреть как организована программа.
Без текста программы говорить не о чем.

Ну , значит взять за основу минимальную длительность одного шага

Это да.
На “общий” вопрос - “общий” ответ

#include <AccelStepper.h>
#include <math.h>
#include <iarduino_Modbus.h>
#include <Servo.h>
#include <EEPROM.h>
#include <ModbusMaster.h>


#define SLAVE_ID 3          // Адрес подчиненного устройства
#define BAUDRATE 9600      // Скорость обмена
//#define TX_ENABLE_PIN 2     // Пин управления направлением RS485

ModbusMaster slave2, slave3, slave4;


// Определяем пины для шаговых двигателей
#define X_STEP_PIN 4
#define X_DIR_PIN 5
#define Y_STEP_PIN 7
#define Y_DIR_PIN 6

// Определяем пины для концевиков
#define X_MIN_PIN 25
#define X_MAX_PIN 24
#define Y_MIN_PIN 23
#define Y_MAX_PIN 22

// Определяем пины для джойстика
#define JOYSTICK_X_PIN A14
#define JOYSTICK_Y_PIN A15
#define JOYSTICK_BUTTON_PIN 26
#define ENCODER_PIN 2  // PA- (синий провод)
#define ENCODER_PB 3  // PB- (зелёный провод)
// Размер окна медианного фильтра (рекомендуется нечетное число, например 3, 5, 7)
#define FILTER_WINDOW 11

#define PIN_knopka_prav A11  // кнопка правая на понели
#define PIN_knopka_lev 45
#define PIN_potenV A8
#define PIN_potenN A13
#define PIN_SERVO 49

// Адреса устройств модбаса
#define Adr_motor 2
#define Adr_panel 3
#define Adr_rashod 4


#define K_P 0.04
#define K_D 0.05
#define K_I 0.01

Servo myservo;
//ModbusClient modbus(Serial2);

// Создаем объекты для управления шаговыми двигателями
AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);

volatile unsigned long stepCount = 0;  // Счётчик шагов

const unsigned long debounceDelay = 50;
struct Button {
  int pin;
  int state;
  int lastState;
  unsigned long lastDebounceTime;
};
Button button1 = {PIN_knopka_lev, HIGH, HIGH, 0};


// Переменные для хранения текущей позиции
long xPos = 0;
long yPos = 0;

// Переменные для хранения целевых позиций
long targetX = 0;
long targetY = 0;

// Флаги для отслеживания состояния движения
bool isMovingX = false;
bool isMovingY = false;

// Флаги для отслеживания движения до концевика
bool moveToEndstopX = false;
bool moveToEndstopY = false;

// Переменные для хранения позиций
const int maxPositions = 6; // Максимальное количество сохраненных позиций
long savedPositions[maxPositions][2] = {
  1000, 5000,
  5000, 1000,
  100, 500,
  5000, 1000,
  1000, 5000,
}; // Массив для сохранения позиций [X, Y]
int savedCount = 0; // Счетчик сохраненных позиций
String gcode;
String buf;
uint8_t i = 0;
uint16_t Delta = 0;
int16_t Rashod = 0, Er = 0, Integral = 0, Div = 0, Er_late = 0, PID = 0;
uint16_t zad1 = 0, poten = 0, zad2 = 0, buf_S = 0;
uint32_t T1 = 0, T = 0, T2 = 0;
bool state = 0, state_b = 0, kn_lev = 0;
int filteredValue = 0;
int KSpeed = 0;

// Массив для хранения значений фильтра
int filterValues[FILTER_WINDOW];

// Флаг для отслеживания настройки
bool set = false;

// Cкорости
float speedX = 0;
float speedY = 0;

// Переменные для хранения данных с джойстика
int joystickXValue = 0;
int joystickYValue = 0;
bool joystickButtonPressed = false;

// Максимальная скорость (шагов в секунду)
const float maxSpeed = 2000; // Увеличьте, если двигатели поддерживают

// Максимальная и минимальная скорости  в ручном режиме
const float maxHandSpeed = 2000; // Максимальная скорость
const float minHandSpeed = 200;  // Минимальная скорость

// Текущая скорость (по умолчанию максимальная)
float currentSpeed = maxSpeed;

// Скорость перемещения при управлении джойстиком
const int joystickSpeed = 2000;

// Обработчик прерывания: +1 на каждое изменение PA-
void countStep() {
  stepCount++;
}

void calibrate() {
  Serial.println("Начало калибровки...");

  // Калибровка оси X
  Serial.println("Калибровка оси X...");
  while (digitalRead(X_MIN_PIN) != HIGH) { // Пока концевик не сработал
    stepperX.setSpeed(-joystickSpeed);   // Двигаемся влево
    stepperX.run();
  }
  stepperX.stop();                      // Останавливаем двигатель
  stepperX.setCurrentPosition(0);       // Устанавливаем текущую позицию в 0
  xPos = 0;
  Serial.println("Ось X откалибрована.");

  // Калибровка оси Y
  Serial.println("Калибровка оси Y...");
  while (digitalRead(Y_MIN_PIN) != HIGH) { // Пока концевик не сработал
    stepperY.setSpeed(-joystickSpeed);   // Двигаемся вверх
    stepperY.run();
  }
  stepperY.stop();                      // Останавливаем двигатель
  stepperY.setCurrentPosition(0);       // Устанавливаем текущую позицию в 0
  yPos = 0;
  stepCount = 0;
  Serial.println("Ось Y откалибрована.");

  // Перемещение в безопасное положение (например, на 10 мм от концевиков)
  Serial.println("Перемещение в безопасное положение...");
  stepperX.moveTo(100);  // 100 шагов от нулевой позиции
  stepperY.moveTo(300);  // 100 шагов от нулевой позиции
  while (stepperX.isRunning() || stepperY.isRunning()) {
    stepperX.run();
    stepperY.run();
  }
  Serial.println("Калибровка завершена.");
}

void setup() {
  // Настройка скорости и ускорения
  stepperX.setMaxSpeed(maxSpeed);
  stepperX.setAcceleration(500);
  stepperY.setMaxSpeed(maxSpeed);
  stepperY.setAcceleration(500);

  // Настройка пинов концевиков как входов с подтяжкой к VCC
  pinMode(X_MIN_PIN, INPUT_PULLUP);
  pinMode(X_MAX_PIN, INPUT_PULLUP);
  pinMode(Y_MIN_PIN, INPUT_PULLUP);
  pinMode(Y_MAX_PIN, INPUT_PULLUP);

  // Настройка пинов джойстика
  pinMode(JOYSTICK_X_PIN, INPUT);
  pinMode(JOYSTICK_Y_PIN, INPUT);
  pinMode(JOYSTICK_BUTTON_PIN, INPUT_PULLUP);

  pinMode(ENCODER_PIN, INPUT_PULLUP);
  
  // Прерывание на изменение состояния PA- (RISING + FALLING = CHANGE)
  attachInterrupt(digitalPinToInterrupt(ENCODER_PIN), countStep, CHANGE);


  // Инициализация Serial для получения G-code
  Serial.begin(9600);
  Serial2.begin(BAUDRATE);
  slave2.begin(2, Serial2);
  slave3.begin(3, Serial2);
  slave4.begin(4, Serial2);

  
  Serial.println("Modbus RTU Master started");


  pinMode(button1.pin, INPUT);
  pinMode(PIN_knopka_prav, INPUT);
  pinMode(PIN_potenV, INPUT);
  pinMode(PIN_potenN, INPUT);
  myservo.attach(PIN_SERVO);

  //modbus.begin();                // Инициируем работу по протоколу Modbus.
  //modbus.setTypeMB(MODBUS_RTU);  // Указываем тип протокола Modbus: MODBUS_RTU (по умолчанию), или MODBUS_ASCII.
  //modbus.setDelay(20);           // Указываем выдерживать паузу между пакетами в 5 мс.
  //modbus.setTimeout(50);        // Указываем жать ответ от модулей не более 15 мс.

  myservo.write(180);

  calibrate();
}

void loop() {

// Если движение по оси X активно, обновляем двигатель X
  if (isMovingX) {
    stepperX.runSpeed();
  //  if(gcode.startsWith("G28") && !isMovingY){
   //   stepperX.setSpeed(-currentSpeed);
 //   }
    if (moveToEndstopX) {
      if (digitalRead(X_MIN_PIN) == HIGH) {
        stepperX.stop(); // Останавливаем двигатель
        /*Serial.print("X");
        Serial.println(stepperX.currentPosition());*/
        stepperX.setCurrentPosition(0); // Сбрасываем позицию в 0
        moveToEndstopX = false; // Завершаем движение
        isMovingX = false; // Останавливаем движение по оси X
      }
    }
    else{
      if (stepperX.currentPosition() == targetX) {
        isMovingX = false; // Останавливаем движение по оси X
      }
    }
  }

  // Если движение по оси Y активно, обновляем двигатель Y
  if (isMovingY) {
    stepperY.runSpeed();
    if (moveToEndstopY) {
      if (digitalRead(Y_MIN_PIN) == HIGH) {
        stepperY.stop(); // Останавливаем двигатель
        /*Serial.print("Y");
        Serial.println(stepperY.currentPosition());
        Serial.print("Position: ");
        Serial.println(stepCount);*/
        stepperY.setCurrentPosition(0); // Сбрасываем позицию в 0
        stepCount = 0;
        moveToEndstopY = false; // Завершаем движение
        isMovingY = false; // Останавливаем движение по оси X
      }
    }
    else{
      if (stepCount == Delta) {
        isMovingY = false; // Останавливаем движение по оси Y
      }
    }
  }

  // Проверяем, есть ли данные для чтения
  if (!(isMovingY || isMovingX)){
    
    if (Serial.available() > 0) {
      gcode = Serial.readStringUntil('\n');
      //Serial.print(gcode);
    }
    if (gcode == "set" || set){
       set = true;
    }
    else if (gcode == "go"){
      Serial.println(gcode);
      buf = "G1 X" + String(savedPositions[i][0]) + " Y" + String(savedPositions[i][1]);
      Serial.print("X: ");
      Serial.print(savedPositions[i][0]);
      Serial.print(" Y: ");
      Serial.print(savedPositions[i][1]);
      Serial.print(" Encoder: ");
      Serial.println(stepCount);
      processGCode(buf); 
      i++;
      if (i == 6){
        processGCode("G28");
        i = 0;
      }
    }
    else {
      processGCode(gcode);
    }
    
  }

  // Если функция управления джойстиком активна
  if (set) {
    joystickControl();
  }
  
  // Проверка концевиков и остановка двигателей при срабатывании
  checkEndstops();

  //Опрос модбас и работа с ним
  if(millis() - T1 > 1000){ 
    modBus();
    //node.writeSingleRegister(1, 1);
    //node.writeSingleRegister(2, 2);
    //node.writeSingleRegister(3, 3);
    //node.writeSingleRegister(4, 4);
    //node.writeSingleRegister(5, 5);
    T1 = millis();
     //Serial.print(isMovingX);
    // Serial.print(" ");
   // Serial.println(isMovingY);
  }


  handleButton(button1, "Кнопка левая");
}
void processGCode(String gcode) {
  // Обработка G-code
  if (gcode.startsWith("G1")) {
    // G1 - линейное перемещение
    int xIndex = gcode.indexOf('X');
    int yIndex = gcode.indexOf('Y');
    int fIndex = gcode.indexOf('F');

    long newX = targetX; // Текущая целевая позиция по X
    long newY = targetY; // Текущая целевая позиция по Y

    // Если есть команда F, обновляем текущую скорость
    if (fIndex != -1) {
      currentSpeed = gcode.substring(fIndex + 1).toFloat(); // Новая скорость
    }

    if (xIndex != -1) {
      newX = gcode.substring(xIndex + 1).toInt(); // Новая целевая позиция по X
    }
    if (yIndex != -1) {
      newY = gcode.substring(yIndex + 1).toInt(); // Новая целевая позиция по Y
    }

    // Вычисляем разницу в шагах для каждой оси
    long deltaX = abs(newX - stepperX.currentPosition());
    long deltaY = abs(newY - stepperY.currentPosition());

    // Определяем максимальную разницу шагов
    long maxDelta = max(deltaX, deltaY);

    // Если разница шагов не нулевая, синхронизируем скорости
    if (maxDelta > 0) {
      // Вычисляем пропорциональные скорости
      speedX = (deltaX / (float)maxDelta) * currentSpeed ;
      speedY = (deltaY / (float)maxDelta) * currentSpeed ;

      // Устанавливаем скорости
      stepperX.setSpeed(newX > stepperX.currentPosition() ? speedX : -speedX);
      stepperY.setSpeed(newY > stepperY.currentPosition() ? speedY : -speedY);

      // Устанавливаем целевые позиции
      targetX = newX;
      targetY = newY;

      // Провека на езду до концевика 
      if (targetX == 0) moveToEndstopX = true;
      if (targetY == 0) moveToEndstopY = true;

      // Запускаем движение
      isMovingX = true;
      isMovingY = true;

      // Задаём значение для энкодера
      Delta = int(deltaY / 3.2);
      stepCount = 0;
    }
  } else if (gcode.startsWith("G28")) {
    // G28 - возврат в нулевую позицию
    stepperX.setSpeed(-currentSpeed); // Двигаемся влево
    stepperY.setSpeed(-currentSpeed); // Двигаемся вниз
    ///Serial.println("Возврат");
    // Устанавливаем целевые позиции
    targetX = 0;
    targetY = 0;
    moveToEndstopY = true;
    moveToEndstopX = true;
    // Запускаем движение
    isMovingX = true;
    isMovingY = true;
  }
}
void checkEndstops() {
  // Проверка концевиков оси X
  if (digitalRead(X_MIN_PIN) == HIGH) {
    // Если сработал минимальный концевик, останавливаем двигатель и сбрасываем позицию в 0
    if (stepperX.speed() < 0) { // Двигатель движется влево (к минимальному концевику)
      stepperX.stop();
      stepperX.setCurrentPosition(0); // Сбрасываем позицию в 0
      //Serial.println("X_MIN сработал. Позиция X сброшена в 0.");
    }
  } else if (digitalRead(X_MAX_PIN) == HIGH) {
    // Если сработал максимальный концевик, останавливаем двигатель
    if (stepperX.speed() > 0) { // Двигатель движется вправо (к максимальному концевику)
      stepperX.stop();
      //Serial.println("X_MAX сработал. Движение по X остановлено.");
    }
  }

  // Проверка концевиков оси Y
  if (digitalRead(Y_MIN_PIN) == HIGH) {
    // Если сработал минимальный концевик, останавливаем двигатель и сбрасываем позицию в 0
    if (stepperY.speed() < 0) { // Двигатель движется вниз (к минимальному концевику)
      stepperY.stop();
      stepperY.setCurrentPosition(0); // Сбрасываем позицию в 0
      //Serial.println("Y_MIN сработал. Позиция Y сброшена в 0.");
    }
  } else if (digitalRead(Y_MAX_PIN) == HIGH) {
    // Если сработал максимальный концевик, останавливаем двигатель
    if (stepperY.speed() > 0) { // Двигатель движется вверх (к максимальному концевику)
      stepperY.stop();
      //Serial.println("Y_MAX сработал. Движение по Y остановлено.");
    }
  }
}
void joystickControl() {
  // Чтение значений с джойстика
  int joystickX = analogRead(JOYSTICK_X_PIN);
  int joystickY = analogRead(JOYSTICK_Y_PIN);
  bool buttonPressed = digitalRead(JOYSTICK_BUTTON_PIN) == LOW;

  // Если джойстик отклонен максимально, используем максимальную скорость
  if (joystickX > 900 || joystickX < 100) {
    speedX = (joystickX > 900) ? maxHandSpeed : -maxHandSpeed;
  } else if (joystickX > 400 && joystickX < 650) {
    speedX = 0; // Центр джойстика - остановка
  } else {
    speedX = (joystickX > 600) ? minHandSpeed : -minHandSpeed; // Маленькая скорость
  }

  if (joystickY > 900 || joystickY < 100) {
    speedY = (joystickY > 900) ? maxHandSpeed : -maxHandSpeed;
  } else if (joystickY > 450 && joystickY < 650) {
    speedY = 0; // Центр джойстика - остановка
  } else {
    speedY = (joystickY > 600) ? minHandSpeed : -minHandSpeed; // Маленькая скорость
  }

  // Устанавливаем скорости
  stepperX.setSpeed(speedX);
  stepperY.setSpeed(speedY);

  // Обновляем двигатели
  stepperX.runSpeed();
  stepperY.runSpeed();

  // Если кнопка нажата, сохраняем текущую позицию
  if (buttonPressed) {
    saveCurrentPosition();
    delay(500); // Задержка для антидребезга
  }
}

void saveCurrentPosition() {
  // Проверяем, есть ли место в массиве
  if (savedCount < maxPositions) {
    // Сохраняем текущие позиции
    savedPositions[savedCount][0] = stepperX.currentPosition();
    savedPositions[savedCount][1] = stepperY.currentPosition();
    savedCount++;

    // Вывод в Serial для отладки
    Serial.print("Сохранена позиция: X=");
    Serial.print(savedPositions[savedCount - 1][0]);
    //modbus.holdingRegisterWrite(Adr_panel, 20 + savedCount , savedPositions[savedCount - 1][0]);
    Serial.print(", Y=");
    Serial.println(savedPositions[savedCount - 1][1]);
    //modbus.holdingRegisterWrite(Adr_panel, 26 + savedCount, savedPositions[savedCount - 1][1]);

    // Если сохранено 6 позиций, завершаем функцию
    if (savedCount == maxPositions) {
      set = false; // Завершаем функцию
      gcode = "";
      savedCount = 0;
      Serial.println("Сохранено 6 позиций. Функция завершена.");
    }
  }
}

void modBus(){
  //Serial.println(modbus.coilRead(Adr_panel, 3));
 // Serial.println (gcode);
  if (0)//(readCoil(3, 3) )
  {
   
    gcode = "set";
    //rabota s regitrami v menu
  }
  else{
    ////////////////////////////////////////////// Воздух
    gcode = "";
    set = 0;
    if (button1.state){

      //Rashod = modbus.holdingRegisterRead(Adr_rashod, 0);
      zad1 = analogRead(PIN_potenV);
      Serial.println(zad1);
      int filteredValue = medianFilter(zad1);
      filteredValue = map(filteredValue, 0, 1023, 0, 20);
      //writeSingleRegister(3, 1, filteredValue * 25);

      Er = Rashod  - filteredValue * 25;
      Integral = Integral + Er;
      Div = Er_late - Er;
      Er_late = Er;
      PID = K_P * Er + K_D * Div + K_I * Integral;

      if (PID <= 0){ PID = 0;}
      if (PID >= 180) {PID = 180;} 
      
      myservo.write(PID);
    }
    //////////////////////////////////////////// Мотор
    buf_S = analogRead(PIN_knopka_prav);
    
    if(buf_S < 1023) {state = 0;}
    else {state = 1;}
    if(state != state_b){
      if (state){
        //modbus.holdingRegisterWrite(Adr_motor, 120, 1);  // ВКЛ мотор
          Serial.println(1);
      }
      else{
        //modbus.holdingRegisterWrite(Adr_motor, 120, 0);  // ВЫКЛ мотор
          Serial.println(0);
      }
        state_b = state;
    }
    //poten = analogRead(PIN_potenN);
    zad2 = 500 + map(poten, 0, 1023, 0, 40 ) * 50 ;
    
    //writeSingleRegister(3, 2, zad2);
   // if(millis() - T1 > 1000){ 
      //modbus.holdingRegisterWrite(Adr_motor, 137, zad2);
     // T1 = millis();
    //}
    //if(readCoil(3, 1)){
   //   gcode = "go";
   // }
    //else{
   //   gcode = "";
   // }
    gcode = "go";
    //KSpeed = readHoldingRegister (3, 3) / 10;
  }
}
/*void modBus(){
  //Serial.println(modbus.coilRead(Adr_panel, 3));

  if (modbus.coilRead(Adr_panel, 3)){
    gcode = "set";
    //rabota s regitrami v menu
  }
  else{
    ////////////////////////////////////////////// Воздух
    gcode = "";
    set = 0;
    if (button1.state){

      Rashod = modbus.holdingRegisterRead(Adr_rashod, 0);
      zad1 = analogRead(PIN_potenV);
      Serial.println(zad1);
      int filteredValue = medianFilter(zad1);
      filteredValue = map(filteredValue, 0, 1023, 0, 20);
      modbus.holdingRegisterWrite(Adr_panel, 1, filteredValue * 25);
    
      Er = Rashod  - filteredValue * 25;
      Integral = Integral + Er;
      Div = Er_late - Er;
      Er_late = Er;
      PID = K_P * Er + K_D * Div + K_I * Integral;

      if (PID <= 0){ PID = 0;}
      if (PID >= 180) {PID = 180;} 
      
      myservo.write(PID);
    }
    //////////////////////////////////////////// Мотор
    buf_S = analogRead(PIN_knopka_prav);
    
    if(buf_S < 1023) {state = 0;}
    else {state = 1;}
    if(state != state_b){
      if (state){
        modbus.holdingRegisterWrite(Adr_motor, 120, 1);  // ВКЛ мотор
          Serial.println(1);
      }
      else{
        modbus.holdingRegisterWrite(Adr_motor, 120, 0);  // ВЫКЛ мотор
          Serial.println(0);
      }
        state_b = state;
    }
    //poten = analogRead(PIN_potenN);
    zad2 = 500 + map(poten, 0, 1023, 0, 40 ) * 50 ;
    modbus.holdingRegisterWrite(Adr_panel, 2, zad2);
    if(millis() - T1 > 1000){ 
      modbus.holdingRegisterWrite(Adr_motor, 137, zad2);
      T1 = millis();
    }
    if(modbus.coilRead(Adr_panel, 1)){
      gcode = "go";
    }
    else{
      gcode = "";
    }
    
    KSpeed = modbus.holdingRegisterRead(Adr_panel, 3)/10;
  }
}*/

int medianFilter(int newValue) {
  static byte index = 0;
  static int sortedValues[FILTER_WINDOW];
  
  // Добавляем новое значение в массив
  filterValues[index] = newValue;
  
  // Копируем значения для сортировки
  for (byte i = 0; i < FILTER_WINDOW; i++) {
    sortedValues[i] = filterValues[i];
  }
  
  // Сортируем массив (пузырьковая сортировка)
  for (byte i = 0; i < FILTER_WINDOW - 1; i++) {
    for (byte j = 0; j < FILTER_WINDOW - i - 1; j++) {
      if (sortedValues[j] > sortedValues[j + 1]) {
        int temp = sortedValues[j];
        sortedValues[j] = sortedValues[j + 1];
        sortedValues[j + 1] = temp;
      }
    }
  }
  
  // Увеличиваем индекс для следующего значения
  index = (index + 1) % FILTER_WINDOW;
  
  // Возвращаем медианное значение (середина отсортированного массива)
  return sortedValues[FILTER_WINDOW / 2];
}

void handleButton(Button &btn, const char* btnName) {
  int reading = digitalRead(btn.pin);
  if (reading != btn.lastState) {
    btn.lastDebounceTime = millis();
  }
  if ((millis() - btn.lastDebounceTime) > debounceDelay) {
    if (reading != btn.state) {
      btn.state = reading;
    }
  }
  btn.lastState = reading;
}

bool readCoil(uint8_t slaveAddress, uint16_t startCoil) {
  ModbusMaster* slave = getSlaveInstance(slaveAddress);
  
  bool result = slave->readCoils(startCoil, 1);
  uint8_t coilValue = (slave->getResponseBuffer(0/16) >> (0 % 16)) & 0x01;
  return coilValue;
}

ModbusMaster* getSlaveInstance(uint8_t slaveAddress) {
  switch(slaveAddress) {
    case 2: return &slave2;
    case 3: return &slave3;
    case 4: return &slave4;
    default:
      Serial.print("Invalid slave address: ");
      Serial.println(slaveAddress);
      return nullptr;
  }
}
uint8_t readHoldingRegister(uint8_t slaveAddress, uint16_t startRegister) {
  ModbusMaster* slave = getSlaveInstance(slaveAddress);

  uint8_t result = slave->readHoldingRegisters(startRegister, 1);
  return result;

}

/**
 * Запись одного holding register на указанное устройство
 * @param slaveAddress - адрес устройства (2, 3 или 4)
 * @param registerAddress - адрес регистра для записи
 * @param value - значение для записи
 * @return true если успешно, false если ошибка
 */
void writeSingleRegister(uint8_t slaveAddress, uint16_t registerAddress, uint16_t value) {
  ModbusMaster* slave = getSlaveInstance(slaveAddress);
   slave->writeSingleRegister(registerAddress, value);
}

код в процессе работы и устранения багов так что строчки с модбас просто в коментах

Можешь объяснить что делают эти строки?

Инициализация слейвоф модбаса, настройка на работу по встроенному сериал юарт порту.
slave2 это класс begin команда 2 это адрес.

ссылка на библиотеку

А это что? Зачем они нужны? Они могут работать вместе на одном порту? И к тому же эта библиотека очень старая и не оптимизированная.

да они работают на одном порту и без каких проблем, для меня библиотека данная более интереснее чем от irarduino

Тут ещё и схема, и полное описание нужно ИМХО

Возможно вы не правильно используете термины. Можете показать в коде, где происходит запрет прерываний на 50 мс?
Если прерывания не запрещаются, а просто долго выполняется кусок кода, то можно использовать управлением шаговым двигателем на прерываниях таймера. Или самостоятельно написать или использовать библиотеки (например GyverStepper).