Проблема с gPlanner2

[Alex 597]

Всем привет. При работе с библиотекой gPlanner 2 возникла проблема. Использую драйвер TMC 2209 и контроллер Arduino Mega 2560 Pro. При попытке переместится в точку устройство перемещается в случайные координаты. Вот код:

#include "GyverPlanner2.h"

Stepper< STEPPER2WIRE > stepper1(2, 4);
Stepper< STEPPER2WIRE > stepper2(3, 5);
GPlanner2< STEPPER2WIRE, 2 > planner;

#include <Servo.h>

#include <charMap.h>
#include <GyverOLED.h>
#include <icons_7x7.h>
#include <icons_8x8.h>
GyverOLED<SSD1306_128x64, OLED_NO_BUFFER> oled;

Servo servo;

// 17 - светодиод
// 7, 8, 9 - кнопки
// 20, 21 - экран
// 6 - сервопривод
// 2, 4 - первый двигатель
// 3, 5 - второй двигатель
// А0 - датчик линии
int path[][2] = {
{0, 0},
{6000, 2500},
{0, 0},
{6000, 2500},
{3000, 1200},
};

//int per[][2] = {
// {900, 0},
// {0, 0},
// {900, 3320},
//};

float mm_s[2] = {2840.0 / 200.0, 3320.0 / 150.0};

int mm_a(float mm, int ax = 0) {
float stepp = mm_s[ax] * mm;
return floor(stepp);
}
int mm_r(float mm, int ax) {
float stepp = mm_s[ax] * mm;
return floor(stepp);
}

void Go(int x0, int y0) {
int g[2] = {mm_a(x0), mm_a(y0, 1)};

oled.clear();
oled.setCursor(0, 2);
oled.print(g[0]);
oled.print(" ");
oled.print(g[1]);

planner.addTarget(g, 1, ABSOLUTE);
while (!planner.ready()) {planner.tick();}
planner.resume();
}

long start0[2] = {0, 0};
long start1[2] = {0, 0};

void Kolibrovka(long x0 = 0, long y0 = 0, bool ser = 0) {
planner.setCurrent(start0);
planner.setSpeed(0, 2000);
while (analogRead(A0) < 400) {planner.tick();}
while (analogRead(A0) > 60) {planner.tick();}
planner.stop();
planner.setSpeed(1, 2000);
while (analogRead(A0) < 400) {planner.tick();}
planner.stop();

start0[0] = planner.getCurrent(0) + 300;
start0[1] = planner.getCurrent(1);

start1[0] = mm_a(x0);
start1[1] = mm_a(y0, 1);

planner.addTarget(start0, 1, ABSOLUTE);
while (!planner.ready()) {planner.tick();}
planner.resume();

planner.setCurrent(start1);
// if (ser) {
// servo.write(180);
// delay(500);
// servo.write(90);
// delay(500);
// }
}

void Finish() {
long f[2] = {0, 0};
planner.addTarget(start1, 1, ABSOLUTE);
while (!planner.ready()) {planner.tick();}
planner.resume();

planner.setCurrent(start0);

planner.addTarget(f, 1, ABSOLUTE);
while (!planner.ready()) {planner.tick();}
planner.resume();
}

void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(17, OUTPUT);
digitalWrite(17, 0);
pinMode(7, INPUT_PULLUP);
pinMode(8, INPUT_PULLUP);
pinMode(9, INPUT_PULLUP);


planner.addStepper(0, stepper1); // ось 0
planner.addStepper(1, stepper2); // ось 1

// устанавливаем ускорение и скорость
planner.setAcceleration(3000);
planner.setMaxSpeed(4000);

oled.init(); // инициализация
oled.clear(); // очистка
oled.setScale(2);
oled.setCursor(0, 3);
oled.print(0);

servo.attach(6);
servo.write(90);

// while (1 == 1) {
// oled.clear();
// oled.setCursor(0, 3);
// oled.print(analogRead(A0));
// delay(300);
// }
int per[][2] = {
{300 + mm_a(150), 0},
{0, 0},
{300, 0},
{mm_a(10), mm_a(150, 1)},
{0, 0},
};

digitalWrite(17, 0);
while(digitalRead(9)) {delay(100);}
digitalWrite(17, 1);

planner.start();

Kolibrovka(5, 152, 1);

oled.clear();
oled.setCursor(0, 4);
oled.print(planner.getCurrent(0));
oled.print(" ");
oled.print(planner.getCurrent(1));


Go(170, 40);


oled.setCursor(0, 4);
oled.print(planner.getCurrent(0));
oled.print(" ");
oled.print(planner.getCurrent(1));

servo.write(180);
delay(500);
servo.write(90);
delay(500000);
// servo.write(90);

Go(10, 20);

// planner.addTarget(per[5], 0, ABSOLUTE);
oled.clear();
oled.setCursor(0, 2);
oled.print(per[3][0]);
oled.print(" ");
oled.print(per[3][1]);

Go(10, 170);

oled.setCursor(0, 4);
oled.print(planner.getCurrent(0));
oled.print(" ");
oled.print(planner.getCurrent(1));

servo.write(90);
delay(500);
// planner.setCurrent(start0);
// planner.addTarget(per[4], 1, ABSOLUTE);


Finish();
digitalWrite(17, 0);
}

void loop() {

}

В чём может быть проблема? Кто-то с таким сталкивался?