     {
         return false;
     }
-    // firePower(_points[0].power);
-    long distX = _points[0].delta[0];
-    long distY = _points[0].delta[1];
-    if (distX <= 0 && distY <= 0)
+    firePower(_points[0].power);
+    if (_points[0].restCount[0] <= 0 && _points[0].restCount[1] <= 0)
         return false;
     long e2 = _points[0].err;
-    if (e2 > -distX)
-    {
-        _points[0].err -= distY;
+    if (e2 > -_points[0].delta[0])
+    {
+        _points[0].err -= _points[0].delta[1];
         stepX(_points[0].dir[0]);
     }
-    if (e2 < distY)
-    {
-        _points[0].err += distX;
+    if (e2 < _points[0].delta[1])
+    {
+        _points[0].err += _points[0].delta[0];
         stepY(_points[0].dir[1]);
     }
     nextWait();

    _points[_sys.plannedIndex].targetCount[1] = abs(_points[_sys.plannedIndex].targetPosition[1] - _prevPoint.targetPosition[1]);
    _points[_sys.plannedIndex].delta[0] = (_points[_sys.plannedIndex].targetCount[0]);
    _points[_sys.plannedIndex].delta[1] = (_points[_sys.plannedIndex].targetCount[1]);
    _points[_sys.plannedIndex].totalCount = _points[_sys.plannedIndex].delta[0] + _points[_sys.plannedIndex].delta[1];
    _points[_sys.plannedIndex].speed = speed;
    if (_sys.plannedIndex > 0)

void stepX(bool dir)
{
    _sys.currentPosition[0] += dir ? 1 : -1;
    _points[0].restCount[0]--;
    _points[0].totalCount--;
    steppers[0].step(dir);
}
void stepY(bool dir)
{
    _sys.currentPosition[1] += dir ? 1 : -1;
    _points[0].restCount[1]--;
    _points[0].totalCount--;
    steppers[1].step(dir);
}
