     finishCommand();
 }
 
-void moveTo(long x, long y)
-{
-    waitingPushPoint();
-#ifdef DEBUG
-    String str = "move:";
-    str += x;
-    str += ",";
-    str += y;
-    Serial.println(str);
-#endif
-    _sys.startPosition[0] = x;
-    _sys.startPosition[1] = y;
-    _sys.time = _sys.defaultTime;
-    addMotion(x, y, 0, _sys.speed, _sys.acceleration);
-}
-void lineTo(long x, long y)
-{
-    waitingPushPoint();
-#ifdef DEBUG
-    String str = "line:";
-    str += x;
-    str += ",";
-    str += y;
-    str += ",";
-    str += _sys.speed;
-    Serial.println(str);
-#endif
-    addMotion(x, y, _sys.power, _sys.speed, _sys.acceleration);
-}
-void addMotion(long x, long y, uint8_t power, uint8_t speed, uint8_t accelaration)
-{
-    MotionState s;
-    s.power = power;
-    s.acceleration = accelaration;
-    s.speed = speed;
-    s.targetPosition[0] = x;
-    s.targetPosition[1] = y;
-    if (_sys.statesLength >= STATES_COUNT)
+void addMotion(long x, long y, uint8_t power, uint8_t speed, uint8_t acceleration)
+{
+    if (_sys.plannedLength >= POINTS_COUNT)
     {
         waitingPushPoint();
     }
-    _sys.states[_sys.statesLength] = s;
-    _sys.statesIndex++;
-    _sys.statesLength++;
+    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() < STATES_COUNT)
+    if (commandsLength() < POINTS_COUNT)
     {
         Serial.print("ok:");
         Serial.println(commandsLength());

    str += y2;
    Serial.println(str);
#endif
    long x0 = _sys.states[_sys.statesLength - 1].targetPosition[0];
    long y0 = _sys.states[_sys.statesLength - 1].targetPosition[1];
    int step = NUM_STEPS;
    double fx, fy;
    for (int i = 0; i < step; i++)

        x1 = v[0] * PULSES_PER_MM;
        y1 = v[1] * PULSES_PER_MM;
        moveTo(x1, y1);
        _sys.lastPosition[0] = x1;
        _sys.lastPosition[1] = y1;
        break;
    }
    case 'l':
