Как запустить G-код с помощью SoftwareSerial?

Я пытаюсь собрать собственный станок с ЧПУ. Поскольку мне нужно управлять большим количеством двигателей одновременно, я хочу управлять им через ведущий Arduino и несколько ведомых устройств с платами управления. Сейчас я пытаюсь настроить один двигатель, отправляя G-код через SoftwareSerial на ведомый. От mySerial ответа нет.

Подскажите, пожалуйста, нужно ли мне интегрировать мой код в библиотеку Grbl?

Вот пример кода подчиненного устройства:

#include <Wire.h>
#include <SoftwareSerial.h>

#define SLAVE_ADDR 9
#define ANSWERESIZE 5

SoftwareSerial mySerial(10, 11); // RX, TX для GRBL

String answer = "Hello";
String command = "";

void setup() {
  // Инициализация Wire для I2C
  Wire.begin(SLAVE_ADDR);
  Wire.onRequest(requestEvent);  // Функция для отправки данных
  Wire.onReceive(receiveEvent);  // Функция для получения данных

  mySerial.begin(115200);  // GRBL работает на 115200 бод по умолчанию
  Serial.begin(9600);  // Для отладки

  Serial.println("Initializing GRBL...");
  delay(1000);  // Ждем загрузки GRBL
  
  // Включаем шаговые двигатели (посылаем команду на активацию)
  mySerial.println("$X");  // Разблокировка GRBL
}

void loop() {
  // Проверяем, есть ли команда для выполнения
  if (command.length() > 0) {
    Serial.println("command: " + command);  // Выводим команду для отладки
    executeCommand(command);  // Передаем команду GRBL для выполнения
    command = "";  // Очищаем команду после выполнения
  }
}

// Функция для обработки полученных данных через I²C
void receiveEvent(int howMany) {
  command = "";  // Очищаем строку перед началом чтения
  while (Wire.available()) {
    char c = Wire.read();  // Читаем байт и конвертируем в символ
    command += c;  // Добавляем символ к строке команды
  }

  Serial.println("Received I²C Data!");  // Уведомляем о получении данных
}

// Функция отправки ответа обратно на мастер
void requestEvent() {
  byte response[ANSWERESIZE];

  for (byte i = 0; i < ANSWERESIZE; i++) {
    response[i] = (byte)answer.charAt(i);
  }

  Wire.write(response, sizeof(response));  // Отправляем ответ
  Serial.println("Sent response to master!");  // Уведомляем о отправке
}

// Функция для передачи команды GRBL
void executeCommand(String command) {
  // Отправляем команду G-кода через Serial (на GRBL)
  Serial.print("Sending to GRBL: ");
  Serial.println(command);

  // Отправляем команду на GRBL через SoftwareSerial
  mySerial.println(command);
  
  // Ждем выполнения команды
  delay(100);  // Настройте задержку в зависимости от сложности движения
}

17:52:18.951 -> Received I²C Data!

17:52:18.951 -> cSent response to master!

17:52:19.000 -> команда: G91 G1 X1600 F1000

17:52:19.018 -> Отправка в GRBL: G91 G1 X1600 F1000

receiveEvent и requestEvent работают нормально, но есть некоторые проблемы в executeCommand или mb в настройках SoftwareSerial.