     long maxTime = 2250;
     long minTime = 25;
     PlannerPoint points[POINTS_COUNT];
-    MotionState states[STATES_COUNT];
-    MotionState state;
-    CommandState command;
-    String commands[COMMANDS_COUNT];
+    PlannerPoint prevPoint;
     uint8_t power = 0;
     uint8_t defaultSpeed = 1;
     uint8_t speed = 100;

    }
    lineTo(floorf(x3 * 100) / 100, floorf(y3 * 100) / 100);
}
void quadTo(long x1, long y1, long x2, long y2)
{
#ifdef DEBUG
    String str = "quad:";

    finishCommand();
}

void addMotion(long x, long y, uint8_t power, uint8_t speed, uint8_t acceleration)
{
    if (_sys.plannedLength >= POINTS_COUNT)
    {
        waitingPushPoint();
    }
    PlannerPoint prevPoint = _sys.prevPoint;
    PlannerPoint nextPoint;
    nextPoint.targetPosition[0] = x;
    nextPoint.targetPosition[1] = y;
    nextPoint.dir[0] = prevPoint.targetPosition[0] < nextPoint.targetPosition[0];
    nextPoint.dir[1] = prevPoint.targetPosition[1] < nextPoint.targetPosition[1];
    nextPoint.targetCount[0] = abs(nextPoint.targetPosition[0] - prevPoint.targetPosition[0]);
    nextPoint.targetCount[1] = abs(nextPoint.targetPosition[1] - prevPoint.targetPosition[1]);
    nextPoint.delta[0] = (nextPoint.targetCount[0]);
    nextPoint.delta[1] = (nextPoint.targetCount[1]);
    nextPoint.totalCount = nextPoint.delta[0] + nextPoint.delta[1];
    nextPoint.enterSpeed = _sys.plannedIndex > 0 ? prevPoint.exitSpeed : _sys.defaultSpeed;
    nextPoint.speed = fmax(1, speed);
    prevPoint.exitSpeed = fmax(1, sqrt(nextPoint.speed));
    nextPoint.acceleration = acceleration;
    long dist = sqrt(nextPoint.delta[0] * nextPoint.delta[0] + nextPoint.delta[1] * nextPoint.delta[1]) / 2;
    nextPoint.accelCount = nextPoint.totalCount - min(dist, _sys.defaultAccelCount);
    nextPoint.decelCount = min(dist, _sys.defaultDecelCount);
    nextPoint.power = power;
    nextPoint.err = (nextPoint.delta[0] > nextPoint.delta[1] ? nextPoint.delta[0] : -nextPoint.delta[1]) / 2;
    _sys.points[_sys.plannedIndex] = nextPoint;
    _sys.prevPoint = nextPoint;
    _sys.plannedIndex++;
    _sys.plannedLength++;
}
void finishCommand()
{
    if (commandsLength() < POINTS_COUNT)
    {
        Serial.print("ok:");
        Serial.println(commandsLength());
