     s.speed = speed;
     s.targetPosition[0] = x;
     s.targetPosition[1] = y;
-    if (_sys.statesIndex > STATES_COUNT - 1)
-    {
-        for (int i = 0; i < STATES_COUNT; i++)
-        {
-            _sys.states[i] = _sys.states[i + 1];
-        }
-    }
-    _sys.states[_sys.statesIndex] = s;
+    if (_sys.statesLength >= STATES_COUNT)
+    {
+        shiftState();
+    }
+    _sys.states[_sys.statesLength] = s;
     _sys.statesIndex++;
+    _sys.statesLength++;
 }
 void finishCommand()
 {
-    if (commandsLength() <= 1)
+    if (commandsLength() < 2)
     {
         Serial.print("ok:");
         Serial.println(commandsLength());

void shiftState()
{
    _sys.state = _sys.states[0];
    for (int i = 0; i < _sys.statesIndex - 1; i++)
    {
        _sys.states[i] = _sys.states[i + 1];
    }
    _sys.statesIndex--;
    if (_sys.statesIndex < 0)
    {
        if (!_sys.isFinish)
        {

    shiftPlanned();
    calcPlanned();
    unlockTimer();
    if (_sys.plannedLength > 0)
    {
        firePower(_sys.points[0].power);
        _sys.minTime = fmax(25, 100.0 / (_sys.points[0].speed / 100.0));
        _sys.maxTime = fmin(_sys.defaultTime, _sys.defaultTime / (_sys.points[0].exitSpeed / 50.0));
        Serial.print("motion finish:");
        Serial.print(_sys.points[0].speed);
        Serial.print(" - ");
        Serial.print(_sys.minTime);
        Serial.print(" - ");
        Serial.println(_sys.maxTime);
    }
}
void waitingPushPoint()
{
    while (true)
    {
        if (_sys.plannedLength < 2)
        {
            break;
        }
        delay(1);
        // #ifdef DEBUG
        //         String str = "waiting:";
        //         str += _sys.statesIndex;
