 struct SystemStatus
 {
     bool running = false;
-    long currentPosition[NUM_AXIS];
+    long currentPosition[NUM_AXIS] = {0, 0};
     long targetPosition[NUM_AXIS];
     long startPosition[NUM_AXIS];
+    long lastPosition[NUM_AXIS] = {0, 0};
     long timerCount = 0;
     long time = 4250;
     long defaultTime = 4250;

    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)
    {
        waitingPushPoint();
    }
    _sys.states[_sys.statesLength] = s;
    _sys.statesIndex++;
    _sys.statesLength++;
}
void finishCommand()
{
    if (commandsLength() < STATES_COUNT)
    {
        Serial.print("ok:");
        Serial.println(commandsLength());

    }
    tt++;
}
/**
 * Motion Parse
 * */

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 curveTo(long x0, long y0, long x1, long y1, long x2, long y2, long x3, long y3)
{
    double subdiv_step = 1.0 / (NUM_STEPS + 1);
    double subdiv_step2 = subdiv_step * subdiv_step;
    double subdiv_step3 = subdiv_step * subdiv_step * subdiv_step;
