 }
 void finishCommand()
 {
-    if (commandsLength() < 2)
+    if (commandsLength() < STATES_COUNT)
     {
         Serial.print("ok:");
         Serial.println(commandsLength());

 * */
void shiftPlanned()
{
    if (_sys.plannedIndex > 0)
    {
        for (int i = 0; i < _sys.plannedIndex; i++)
        {
            _sys.points[i] = _sys.points[i + 1];
        }
        _sys.plannedIndex--;
        _sys.plannedLength--;
    }
}
void lockTimer()
{

#define CURVE_SECTION 0.5
#define NUM_STEPS 100
#define NUM_AXIS 2
// #define DEBUG true
enum MOTION_STATE
{
    MOTION_ACCELETING = 0,
