     finishCommand();
 }
 
-void addMotion(long x, long y, uint8_t power, uint8_t speed, uint8_t acceleration)
-{
-    if (_sys.plannedLength >= POINTS_COUNT)
-    {
-        waitingPushPoint();
-    }
+void addMotion(long x, long y, uint8_t power, uint16_t speed, uint16_t acceleration)
+{
     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++;
+
+    _sys.points[plannedIndex].targetPosition[0] = x;
+    _sys.points[plannedIndex].targetPosition[1] = y;
+    _sys.points[plannedIndex].dir[0] = prevPoint.targetPosition[0] < _sys.points[plannedIndex].targetPosition[0];
+    _sys.points[plannedIndex].dir[1] = prevPoint.targetPosition[1] < _sys.points[plannedIndex].targetPosition[1];
+    _sys.points[plannedIndex].targetCount[0] = abs(_sys.points[plannedIndex].targetPosition[0] - prevPoint.targetPosition[0]);
+    _sys.points[plannedIndex].targetCount[1] = abs(_sys.points[plannedIndex].targetPosition[1] - prevPoint.targetPosition[1]);
+    _sys.points[plannedIndex].delta[0] = (_sys.points[plannedIndex].targetCount[0]);
+    _sys.points[plannedIndex].delta[1] = (_sys.points[plannedIndex].targetCount[1]);
+    _sys.points[plannedIndex].totalCount = _sys.points[plannedIndex].delta[0] + _sys.points[plannedIndex].delta[1];
+    _sys.points[plannedIndex].speed = speed;
+    if (plannedIndex > 1)
+    {
+        _sys.points[plannedIndex - 1].exitSpeed = _sys.points[plannedIndex].speed;
+    }
+    _sys.points[plannedIndex].acceleration = acceleration;
+    long dist = sqrt(_sys.points[plannedIndex].delta[0] * _sys.points[plannedIndex].delta[0] + _sys.points[plannedIndex].delta[1] * _sys.points[plannedIndex].delta[1]) / 2;
+    _sys.points[plannedIndex].accelCount = _sys.points[plannedIndex].totalCount - min(dist, _sys.defaultAccelCount);
+    _sys.points[plannedIndex].decelCount = min(dist, _sys.defaultDecelCount);
+    _sys.points[plannedIndex].power = power;
+    _sys.points[plannedIndex].err = (_sys.points[plannedIndex].delta[0] > _sys.points[plannedIndex].delta[1] ? _sys.points[plannedIndex].delta[0] : -_sys.points[plannedIndex].delta[1]) / 2;
+
+    _sys.prevPoint = _sys.points[plannedIndex];
+    plannedIndex += 1;
+    plannedLength += 1;
 }
 void finishCommand()
 {
-    if (commandsLength() < POINTS_COUNT)
-    {
-        Serial.print("ok:");
-        Serial.println(commandsLength());
-    }
+    Serial.print("ok:");
+    if (commandsLength() < 1)
+    {
+        closePower();
+    }
+    Serial.println(commandsLength());
 }
 int8_t commandsLength()
 {
-    return _sys.plannedLength;
+    return plannedLength;
 }
 void setPower(uint8_t power)
 {
-    _sys.power = fmaxf(0.0, fminf(100.0, power));
-}
-void setSpeed(uint8_t speed)
-{
-    _sys.speed = fmaxf(0.0, fminf(100.0, speed));
-}
-void setAccel(uint8_t accel)
-{
-    _sys.acceleration = fmaxf(0.0, fminf(100.0, accel));
+    _sys.power = power;
+}
+void setSpeed(uint16_t speed)
+{
+    _sys.speed = speed;
+}
+void setAccel(uint16_t accel)
+{
+    _sys.acceleration = accel;
 }
 void firePower(uint8_t power)
 {
     pinMode(_sys.powerPin, OUTPUT);
-    analogWrite(_sys.powerPin, fmaxf(0.0, fminf(255.0, power)));
+    analogWrite(_sys.powerPin, power);
 }
 void openPower()
 {
-    pinMode(_sys.powerPin, OUTPUT);
-    analogWrite(_sys.powerPin, _sys.power * 2.55);
+    firePower(_sys.power);
 }
 void closePower()
 {
-    analogWrite(_sys.powerPin, 0);
+    firePower(0);
 }
 /**
  * motion control in Timer


void shiftPlanned()
{
    if (_sys.plannedLength > 0)
    {
        for (int i = 0; i < _sys.plannedLength - 1; i++)
        {
            _sys.points[i] = _sys.points[i + 1];
        }
        _sys.plannedIndex--;
        _sys.plannedLength--;
    }
    if (_sys.plannedLength <= 0)
    {
        finishCommand();
    }
}
void lockTimer()
{
    noInterrupts();
}
void unlockTimer()
{
    interrupts();
}
void setRun()
{

}
void nextWait()
{
    if (tt % 2 == 0)
    {
        if (_sys.points[0].totalCount > _sys.points[0].accelCount)
        {
            _sys.isRunningState = MOTION_ACCELETING;
        }
        else if (_sys.points[0].totalCount > _sys.points[0].decelCount)
        {
            _sys.isRunningState = MOTION_NORMAL;
        }
        else
        {
            _sys.isRunningState = MOTION_DECELETING;
        }
        switch (_sys.isRunningState)
        {
        case MOTION_ACCELETING:
        {
            if (_sys.currentSpeed < _sys.maxSpeed)
            {
                _sys.currentSpeed += _sys.acceleration;
                if (_sys.currentSpeed > _sys.maxSpeed)
                {
                    _sys.currentSpeed = _sys.maxSpeed;
                }
            }
            break;
        }
        case MOTION_DECELETING:
        {
            if (_sys.currentSpeed > _sys.minSpeed)
            {
                _sys.currentSpeed -= _sys.acceleration;
                if (_sys.currentSpeed < _sys.minSpeed)
                {
                    _sys.currentSpeed = _sys.minSpeed;
                }
            }
            break;
        }
