int path[][4] = {
   {0, 0, 0, 0},   //  0 Позиция для определения объекта X+267 Y+050 Z+150 Q00  
   {880, -924, 20000, -2109},    //   1 Позиция для определения объекта X+267 Y+050 Z+150 Q00  
  {0, -2244, 20600, -925},      //   2 Точка захвата X+267 Y+000 Z+000 Q00
  {-1760, -836, 17800, -2627},  //   3 Промежуточная позиция для кубика из дерева X+267 Y-100 Z+200 Q00
  {-3344, -2684, 15400, -1480}, //   4 Позиция сброса кубика из дерева X+267 Y-212 Z+000 Q00
};

int path2[][4] = {
   {0, 0, 0, 0},   //  0 Позиция для определения объекта X+267 Y+050 Z+150 Q00  
   {880, -924, 20000, -2109},    //   1 Позиция для определения объекта X+267 Y+050 Z+150 Q00  
  {0, -2244, 20600, -925},      //   2 Точка захвата X+267 Y+000 Z+000 Q00
  {1761, -836, 17800, -2627},  //    3 Промежуточная позиция для кубика из акрила X+267 Y+100 Z+200 Q00
  {3345, -2684, 15400, -1480}, //    4 Позиция сброса кубика из акрила X+267 Y+212 Z+000 Q00
};

int nodeAmount = sizeof(path) / 8;
int nodeAmount2 = sizeof(path2) / 8;

// В инструкции будут ссылки на библиотеки
#include <GyverStepper2.h>
#include <ServoSmooth.h>
#include "GyverPlanner2.h"

// Переменные
unsigned long timing;
int Xp = 0;
int Yp = 0;
int Zp = 0 ;
int Qp = 90 ;
int steps1 = 0;
int steps2 = 0;
int steps3 = 0;
int steps4 = 0;
int oldsteps1 = 0;
int oldsteps2 = 0;
int oldsteps3 = 0;
int oldsteps4 = 0;
int currentDir = -1;
int sortMode = 1;
int a1 = 0;
int a2 = 0;
int a3 = 0;
int a0 = 0;
int pos1 = 0;
int pos2 = 0;
int pos3 = 0;
int pos4 = 0;
int sp1;
int sp2;
int sp3;
int sp4;
float ksp;
int wood = 0;
int acril = 0;
int start = 0;
int end = 0;
int nextiteration;
int count1 = 0;
int count2 = 0;
int ntact = 0;


//Инциализая двигателей и создание массива
GStepper2<STEPPER2WIRE> arrSteppers[] = {
  GStepper2<STEPPER2WIRE>(32000, 7, 8, 11),     // Поворот колонны -> engineInt = 1
  GStepper2<STEPPER2WIRE>(16000, 9, 10, 11),    // Наклон плеча -> engineInt = 2
  GStepper2<STEPPER2WIRE>(32000, 5, 6, 11),     // Наклон предплечья -> engineInt = 3
  GStepper2<STEPPER2WIRE>(16000, 3, 4, 11),     // Поворот магнита -> engineInt = 4
  GStepper2<STEPPER2WIRE>(32000, 12, 13, 11),   // Наклон магнита -> engineInt = 5
};

// Инициализая планировщиков позиций
GPlanner2< STEPPER2WIRE, 4 > planner;
GPlanner2< STEPPER2WIRE, 4 > planner2;

// Инициализая захвата
ServoSmooth servoGripper;

const int COUNT_STEPPERS = (sizeof(arrSteppers) / sizeof(arrSteppers[0]));
const int PIN_SERVO_GRIPPER = 2;

int arrCurrentSteps[COUNT_STEPPERS];

int engineInt = 0;
int angleInt  = 0;
 
// Основная функция
void setup() {

  Serial.begin(9600); // Запуск работы serial порта
    Serial.println("Coordinate  XYZ. Example: X+250 Y+250 Z+400 Q90");
    Serial.println("Control command. Example: dir_1");
    Serial.println("Engine control command engine/angeles. Example: 1/30");
    Serial.println("Print Start for starting sorting");
    Serial.println("Print End for ending sorting or back in start position");

// Добавление двигателей в планировщик
  planner.addStepper(0, arrSteppers[0]);  // ось 1
  planner.addStepper(1, arrSteppers[1]);  // ось 2
  planner.addStepper(2, arrSteppers[2]);  // ось 3
  planner.addStepper(3, arrSteppers[4]);  // ось 4

  planner2.addStepper(0, arrSteppers[0]);  // ось 1
  planner2.addStepper(1, arrSteppers[1]);  // ось 2
  planner2.addStepper(2, arrSteppers[2]);  // ось 3
  planner2.addStepper(3, arrSteppers[4]);  // ось 4

// установка скоростей планировщика, запуск и указание стартовой позиции
  planner.setAcceleration(1500);
  planner.setMaxSpeed(1500);

  planner2.setAcceleration(5000);
  planner2.setMaxSpeed(5000);

  planner.setCurrent(path[0]);
  planner.start();

  planner2.setCurrent(path[0]);
  planner2.start();

//присваивание захвату цифрового пина, установка скорости
  servoGripper.attach(PIN_SERVO_GRIPPER, 500, 2400);
  servoGripper.setSpeed(50);
  servoGripper.setAutoDetach(true);
  
}

// Функция для проверки (на сколько шагов нужно повернуть двигатель для достижение 90градусов)
void Colibrate() {
  // ---- 90 градусов (для отладки) -----
  //delay(500);
  /*delay(500);
  arrSteppers[0].setTarget(4000, RELATIVE);*/

  /*arrSteppers[1].setTarget(-4000, RELATIVE);
  delay(1000);*/
  
  //arrSteppers[2].setTarget(16600, RELATIVE);
  //delay(1000);

  //arrSteppers[4].setTarget(-3400, RELATIVE);
}

// Функция, получающая данные от порта
void getCoord() {

  while ((Serial.available() != 0) && (count1 <= 4) && (count2 <= 4))  {
    String strFromESP = Serial.readString();
    strFromESP.trim();

    Serial.println("Output string: " + (String) strFromESP);
    
    //Форматированное чтение полученной строки, запись необходимых значений и их вывод
    if (strFromESP.length() >= 4 && strFromESP[0] == 'X' && strFromESP[6] == 'Y' && strFromESP[12] == 'Z' && strFromESP[18] == 'Q') {
      Xp = ((String) strFromESP.substring(1,6)).toInt();
      Yp = ((String) strFromESP.substring(7,11)).toInt();
      Zp = ((String) strFromESP.substring(13,17)).toInt();
      Qp = ((String) strFromESP.substring(19)).toInt();     // Угол ориентации захвата относительно вертикали °
      Serial.println("X = " + (String)Xp + "; Y = "+ (String)Yp+ "; Z = " + (String)Zp + "; Qp = " + (String)Qp);
     
      float L0 = 230;  // длинны звеньев манипулятора m
      float L1 = 221;
      float L2 = 223;
      float L3 = 166;
     
      a0 = atan2(Yp, Xp)*57.3; // расчеты обратной задачи кинематики по полученным значениям координат
      int Xt = sqrt(pow(Xp, 2) + pow(Yp, 2));  
      float dX = L3*sin(Qp/57.3); 
      float dY = L3*cos(Qp/57.3); 
      int Xn = Xt - dX; 
      int Yn = Zp - L0 + dY; 
      float d = sqrt(pow(Xn, 2) + pow(Yn, 2)); 
      float q1 = atan2(Yn, Xn)*57.3; 
      float sq2 = (pow(d, 2) + pow(L1, 2) - pow(L2, 2)) / ( 2 * L1 * d );
      float q2 = acos(sq2)*57.3;
      float sq3 = (pow(L1, 2) + pow(L2, 2) - pow(d, 2)) / ( 2 * L1 * L2 );
      float q3 = acos(sq3)*57.3;
       a1 = 90 - (q1 + q2); 
       a2 = 180 - q3; 
       a3 = -1 *((180 - q2 - q3)+ (180 - q1 - 90) + Qp - 180); 
      Serial.println("a0 = " + (String)a0 + "; a1 = "+ (String)a1 + "; a2 = " + (String)a2 + "; a3 = " + (String)a3);
       steps1 = (int) ((8000 * 4) / 360) * a0; 
       steps2 = (int) -((4000 * 4) / 360) * a1;
       steps3 = ((int) ((16600 * 4) / 360) * a2) * 100;
       steps4 = (int) -((3400 * 4) / 360) * a3;

      Serial.println("steps1 = " + (String)steps1 + "; steps2 = "+ (String)steps2 + "; steps3 = " + (String)steps3 + "; steps4 = " + (String)steps4);
      Serial.println("for path[]: {" +(String)steps1 + ", " + (String)steps2 + ", " + (String)steps3 + ", " + (String)steps4 + "}," );

    }
    //Форматированное чтение полученной строки, запись необходимых значений и их вывод
    else if (strFromESP.length() == 1 && strFromESP[0] == 'r') {
      Serial.println("Reseted! All current steps is 0.");
    
    //Форматированное чтение номера двигателя и значения угла, запись необходимых значений и их вывод
      for (int i = 0; i < COUNT_STEPPERS; i++) {
        arrCurrentSteps[i] = 0;
      }
    } else if (strFromESP.length() <= 5 && (strFromESP[1] == '/' || strFromESP[1] == '\\')) {
      engineInt = ((String) strFromESP[0]).toInt();
      angleInt  = ((String) strFromESP.substring(2)).toInt();

      Serial.print("Engine: ");
      Serial.println(engineInt);

      Serial.print("Angle: ");
      Serial.println(angleInt);

    // Все по аналогии
    //Форматированное чтение полученной строки, запись необходимых значений и их вывод
    } else if (strFromESP.length() >= 4 && strFromESP[0] == 'd' && strFromESP[1] == 'i' && strFromESP[2] == 'r' && strFromESP[3] == '_') {
       currentDir = ((String) strFromESP.substring(4)).toInt();
      Serial.println("ID dir: " + strFromESP.substring(4) + "; " + currentDir);

    }else if (strFromESP.length() >= 5 && strFromESP[0] == 's' && strFromESP[1] == 'o' && strFromESP[2] == 'r' && strFromESP[3] == 't' && strFromESP[4] == '_') {
       sortMode = ((String) strFromESP.substring(5)).toInt();
      Serial.print("sortMode : ");
      Serial.println(sortMode);

    }else if (strFromESP.length() >= 6 && strFromESP[0] == 'i' && strFromESP[1] == 't' && strFromESP[2] == 's' && strFromESP[3] == 'w' && strFromESP[4] == 'o' && strFromESP[5] == 'o' && strFromESP[6] == 'd') {
       wood = 1;
      Serial.print("Wood? : ");
      Serial.println(wood);

    }
    else if (strFromESP.length() >= 6 && strFromESP[0] == 'i' && strFromESP[1] == 't' && strFromESP[2] == 's' && strFromESP[3] == 'a' && strFromESP[4] == 'c' && strFromESP[5] == 'r' && strFromESP[6] == 'i' && strFromESP[7] == 'l') {
       acril = 1;
      Serial.print("Acril? : ");
      Serial.println(acril);

    }else if (strFromESP.length() >= 5 && strFromESP[0] == 'S' && strFromESP[1] == 't' && strFromESP[2] == 'a' && strFromESP[3] == 'r' && strFromESP[4] == 't') {
       start = 1;
      Serial.print("Start ? : ");
      Serial.println(start);

    }
    else if (strFromESP.length() >= 2 && strFromESP[0] == 'E' && strFromESP[1] == 'n' && strFromESP[2] == 'd') {
       end = 1;
      Serial.print("End ? : ");
      Serial.println(end);

    }
  }
}
// Функции управления захватом
void Gripper_open() {
  servoGripper.setTarget(500);   // отправка сервопривода на позицию 500, которая соответсвует начальному положению захвата
  Serial.print("Open !! Gripper position: "); 
  Serial.println(servoGripper.getTarget()); 
}

void Gripper_close() {
  servoGripper.setTarget(1260); // отправка сервопривода на позицию 500, которая соответсвует сжатому положению захвата
  Serial.print("Close !! Gripper position: "); 
  Serial.println(servoGripper.getTarget()); 
}

// Функция отвечающая за расчет скорости движения звеньев для одновременного достижения цели
void EqualTimeToPos() {
     
      if ((steps1 > 0) || (steps1 < 0)) {   // расчеты производятся в зависимости от того совершается ли движение колонны с переходом через ось координат 
         sp1 = 500;
        sp2 = fabs(((float) (steps2-oldsteps2)/steps1)*sp1);
        sp3 = fabs(((float) (steps3-oldsteps3)/steps1)*sp1);    //расчеты скоростей двигателей относительно скорости колонны
        sp4 = fabs(((float) (steps4-oldsteps4)/steps1)*sp1);
        ksp = (float) (fabs((fabs(steps1)-fabs(oldsteps1))/fabs(steps1)));
        if (ksp == 0) {
          ksp = 1;
        }
        sp2 = sp2*ksp;
        sp3 = sp3*ksp;
        sp4 = sp4*ksp;
        arrSteppers[0].setMaxSpeed(sp1);
        arrSteppers[1].setMaxSpeed(sp2);
        arrSteppers[2].setMaxSpeed(sp3);
        arrSteppers[4].setMaxSpeed(sp4);         //установка скорости двигателей исходя из полученных значений 
        arrSteppers[0].setAcceleration(sp1);
        arrSteppers[1].setAcceleration(sp2);
        arrSteppers[2].setAcceleration(sp3);
        arrSteppers[4].setAcceleration(sp4);
      }
      else {
        sp1 = 500;
        sp2 = fabs(((float) (steps2-oldsteps2)/steps2)*sp1);
        sp3 = fabs(((float) (steps3-oldsteps3)/steps2)*sp1);
        sp4 = fabs(((float) (steps4-oldsteps4)/steps2)*sp1);
        arrSteppers[0].setMaxSpeed(sp1);
        arrSteppers[1].setMaxSpeed(sp2);
        arrSteppers[2].setMaxSpeed(sp3);
        arrSteppers[4].setMaxSpeed(sp4);
        arrSteppers[0].setAcceleration(sp1);
        arrSteppers[1].setAcceleration(sp2);
        arrSteppers[2].setAcceleration(sp3);
        arrSteppers[4].setAcceleration(sp4);
        }
     /*
       Serial.println("Speed 1 joint");
        Serial.println(sp1);
        Serial.println("Speed 2 joint");
        Serial.println(sp2);
        Serial.println("Speed 3 joint");   // вывод для отладки 
        Serial.println(sp3);
        Serial.println("Speed 4 joint");
        Serial.println(sp4);*/
}

// Функция окончания работы и возврата в начальное положение
void End() {
  start = 0;
  sortMode = 1;
  pos1 = arrSteppers[0].getTarget(); 
  pos2 = arrSteppers[1].getTarget(); 
  pos3 = arrSteppers[2].getTarget(); 
  pos4 = arrSteppers[4].getTarget();              // переменные возвращаются в исходное состояния, двигатели отправляются в 0
  arrSteppers[0].setTarget(-1*pos1, RELATIVE); 
  arrSteppers[1].setTarget(-1*pos2, RELATIVE);
  arrSteppers[2].setTarget(-1*pos3, RELATIVE);
  arrSteppers[4].setTarget(-1*pos4, RELATIVE);
}

// Функция начала сортировки
void Start() {
  if (sortMode == 1 ){
    arrSteppers[0].setMaxSpeed(500);
    arrSteppers[1].setMaxSpeed(525);
    arrSteppers[2].setMaxSpeed(11363);
    arrSteppers[4].setMaxSpeed(1198);
    arrSteppers[0].setAcceleration(500);
    arrSteppers[1].setAcceleration(525);  
    arrSteppers[2].setAcceleration(11363);       // установка позиции робота для начала сортировки
    arrSteppers[4].setAcceleration(1198);
    arrSteppers[0].setTarget(880, ABSOLUTE);
    arrSteppers[1].setTarget(-924, ABSOLUTE);
    arrSteppers[2].setTarget(20000, ABSOLUTE);
    arrSteppers[4].setTarget(-2109, ABSOLUTE); 
  }
    sortMode = 0;
    Sorting();     // вызов вспомогательной функции
    
    // условия для выхода на следующую итерацию сортировки
    if ((nextiteration == 1) && (wood == 1)) {
      nextiteration = 0;
      planner.resume();
    }
    if ((nextiteration == 1) && (acril == 1)) {
      nextiteration = 0;
      planner2.resume();
    }
    
}

// Вспомогательная функция сортировки
void Sorting() {
  if (wood == 1 ) {
    Cube_wood();
  }
  else if (acril == 1) {
    Cube_acril();
  }
}

// Функция траектории движения робота при получении строки от наличии куба из дерева
void Cube_wood() { 
  
while (planner.available()) {
    // в буфер планировщика 1 записываются позиции и происходит движение по точкам
    count1 = count1 + 1;
    planner.addTarget(path[1], 0, ABSOLUTE); // {880, -924, 20000, -2109},    // 1 // Позиция для определения объекта X+267 Y+050 Z+150 Q00  
    planner.addTarget(path[1], 0, ABSOLUTE); // {880, -924, 20000, -2109},    // 1 // Позиция для определения объекта X+267 Y+050 Z+150 Q00
    planner.addTarget(path[2], 1, ABSOLUTE); // {0, -2508, 20200, -777},      // 2 // Точка захвата X+267 Y+000 Z-025 Q00
    planner.addTarget(path[3], 0, ABSOLUTE); // {-1760, -836, 17800, -2627},  // 3 // Промежуточная позиция для кубика из дерева X+267 Y-100 Z+200 Q00
    planner.addTarget(path[4], 1, ABSOLUTE); // {-3344, -2904, 15000, -1369}, // 4 // Точка сброса кубика из дерева X+267 Y-212 Z-025 Q00
    planner.addTarget(path[3], 0, ABSOLUTE); // {-1760, -836, 17800, -2627},  // 3 // Промежуточная позиция для кубика из дерева X+267 Y-100 Z+200 Q00
    planner.addTarget(path[1], 1, ABSOLUTE); // {880, -924, 20000, -2109},    // 0 // Позиция для определения объекта X+267 Y+050 Z+150 Q00
    //Serial.println(count1);
    if ( count2 == 6) {
      }
    if ( count1 == 7) {
      ntact = 0; 
      Gripper_close();   // условия включения захвата
      planner.stop();
  }
    if ( count1 == 9) {
      ntact = 0;            // условия включения захвата
      Gripper_open(); 
   }
    if ( count1 == 11) {
      wood = 0;
      acril = 0;         // изменение нужных переменных для перехода на следующую итерацию
      count1 = 4;
      Serial.println("wait next cube");
      nextiteration = 1;   
      while(Serial.available()){ Serial.read();} // холостое чтение (очистка буфера Serial)
   }
  
  }
}

// Функция траектории движения робота при получении строки от наличии куба из акрила
void Cube_acril() { 
while (planner2.available()) {
  // Аналогично предыдущей функции
  count2 = count2 + 1;
    planner2.addTarget(path2[1], 0, ABSOLUTE); // {880, -924, 20000, -2109},    // 1 // Позиция для определения объекта X+267 Y+050 Z+150 Q00  
    planner2.addTarget(path2[1], 0, ABSOLUTE); // {880, -924, 20000, -2109},    // 1 // Позиция для определения объекта X+267 Y+050 Z+150 Q00
    planner2.addTarget(path2[2], 1, ABSOLUTE); // {0, -2508, 20200, -777},      // 2 // Точка захвата X+267 Y+000 Z-025 Q00
    planner2.addTarget(path2[3], 0, ABSOLUTE); // {1760, -836, 17800, -2627},  //  5 // Промежуточная позиция для кубика из акрила X+267 Y+100 Z+200 Q00
    planner2.addTarget(path2[4], 1, ABSOLUTE); // {3344, -2904, 15000, -1369},  // 6 // Позиция сброса кубика из акрила X+267 Y+212 Z-025 Q00
    planner2.addTarget(path2[3], 0, ABSOLUTE); // {1760, -836, 17800, -2627},  //  5 // Промежуточная позиция для кубика из акрила X+267 Y+100 Z+200 Q00
    planner2.addTarget(path2[1], 1, ABSOLUTE); // {880, -924, 20000, -2109},    // 0 // Позиция для определения объекта X+267 Y+050 Z+150 Q00
    //Serial.println(count1);
      
      if ( count2 == 6) {
      }
      if ( count2 == 7) {
      ntact = 0; 
      Gripper_close(); 
      planner2.stop();
      }
      if ( count2 == 9) {
      ntact = 0; 
      Gripper_open();
   }
    if ( count2 == 11) {
      acril= 0;
      count2 = 4;
      Serial.println("wait next cube");
      nextiteration = 1;
      while(Serial.available()){ Serial.read();} // холостое чтение (очистка буфера Serial)
   }
  
  }
} 

// Функция движения шаговых двигателей по 1 (используется для отладки)
void EngineTest() {    

    // Настройка шаговых двигателей
  // (sizeof(arrSteppers) / sizeof(arrSteppers[0])) -> length arrSteppers
  for (int i = 0; i < COUNT_STEPPERS; i++) {
    int sp = (i == 2) ? 8000 : 1000; //5000
    int acc = (i == 2) ? 4000 : 1000; //2500
   arrSteppers[i].setMaxSpeed(sp); 
       // Установка макс. скорости в шагахсек*/
  arrSteppers[i].setAcceleration(acc);
   // Установка ускорения в шагах/сек/сек*/

    arrCurrentSteps[i] = 0;
  }   // по принятой строке 
      int countSteps = 0;

      if (engineInt == 1) countSteps = (int) ((8000 * 4) / 360) * angleInt; 
      else if (engineInt == 2) countSteps = (int) -((4000 * 4) / 360) * angleInt;
      else if (engineInt == 3) countSteps = ((int) ((16600 * 4) / 360) * angleInt) * 100;
      else if (engineInt == 4) countSteps = 0;
      else if (engineInt == 5) countSteps = (int) -((3400 * 4) / 360) * angleInt;

      countSteps = countSteps - arrCurrentSteps[ (engineInt - 1) ];

      arrSteppers[ (engineInt - 1) ].setTarget(countSteps, ABSOLUTE);
      arrCurrentSteps[ (engineInt - 1) ] += countSteps;

      engineInt = 0;
      angleInt  = 0;

      pos1 = arrSteppers[0].getTarget(); 
      pos2 = arrSteppers[1].getTarget(); 
      pos3 = arrSteppers[2].getTarget(); 
      pos4 = arrSteppers[4].getTarget();
      
      Serial.println("pos1");
      Serial.println(pos1);
      Serial.println("pos2");
      Serial.println(pos2);
      Serial.println("pos3");
      Serial.println(pos3);
      Serial.println("pos4");
      Serial.println(pos4);
}

// Функция таймера (используется для задерки на захват)
void Timer() {
  if (millis() - timing > 2000){ // Вместо 2000 подставьте нужное вам значение паузы ( как видно условие выполняется через 2 отрезка времени)
  timing = millis(); 
  ntact=ntact+1;
 
  if ((ntact == 2) && (count1 == 7) && (wood == 1 )) {  
  planner.resume();
  Serial.println("continue");
 }
  if ((ntact == 2) && (count1 == 9) && (wood == 1 )) {  
  planner.resume();
  Serial.println("continue");
 }

 if ((ntact == 2) && (count2 == 7) && (acril == 1 )) {  
  planner2.resume();
  Serial.println("continue");
 }
  if ((ntact == 2) && (count2 == 9) && (acril == 1 )) {  
  planner2.resume();
  Serial.println("continue");
 }
 }

}

//Функция перемещения по декартовым координатам
void XYZ(){
      EqualTimeToPos();
      int countSteps1 =  (int) steps1 - oldsteps1;
      int countSteps2 = (int) steps2 - oldsteps2;
      int countSteps3 = (int) steps3 - oldsteps3;
      int countSteps4 = (int) steps4 - oldsteps4;
      arrSteppers[0].setTarget(steps1, ABSOLUTE);
      arrSteppers[1].setTarget(steps2, ABSOLUTE);
      arrSteppers[2].setTarget(steps3, ABSOLUTE);
      arrSteppers[4].setTarget(steps4, ABSOLUTE);
      oldsteps1 += countSteps1;
      oldsteps2 += countSteps2;
      oldsteps3 += countSteps3;
      oldsteps4 += countSteps4;
}   

//Основная функция цикла
void loop() {
 
  planner.tick();
  planner2.tick();           // тикер движения,если движется возвращает true(подробнее читать в документации библиотеки)
  servoGripper.tick();

  Timer();  

  bool allSteppersOff = true;
  for (int i = 0; i < COUNT_STEPPERS; i++) {       // проверка все ли двигатели стоят
    if (arrSteppers[i].tick()) allSteppersOff = false;
  }

  if (allSteppersOff) {
     
    getCoord();  // получение данных

    if (engineInt > 0 && engineInt <= COUNT_STEPPERS) { 

      EngineTest();
    
    } else {

      // ограничения на движение звеньев
      if ( (Xp > 100) && (a0 < 75 && a0 > -75)  && (a1 <= 95 && a1 > -75)  && (a2 <= 120 && a2 > -75) && (a3 <= 110 && a3 > -110) )  {

      XYZ();
      // команды управления, dir используются если нужно вызвать или проверить код, но не хочется писать под это отдельные переменные и строкив getCoord
      } else if (currentDir == 1) {
        Gripper_close();

      } else if (currentDir == 2) {
        Gripper_open();

      } else if (currentDir == 3) {
        planner.resume();

      } else if (currentDir == 4) {
        planner2.resume();
      
      } else if (end == 1) {
        End();
     
      } else if (start == 1) {
        Start();
     
      } else if (currentDir == 6) {
      
      }
      // обнуление переменных в цикле
      Xp = 0;
      Yp = 0;
      currentDir = -1;
      end = 0;
    }
    
  }

}