     long targetPosition[NUM_AXIS];
     long startPosition[NUM_AXIS];
     long timerCount = 0;
+    long time = 6250;
+    long maxTime = 6250;
+    long minTime = 125;
     PlannerPoint points[POINTS_COUNT];
     MotionState states[STATES_COUNT];
     MotionState state;

 * */
void shiftPlanned()
{
    for (int i = 0; i < _sys.plannedIndex; i++)
    {
        _sys.points[i] = _sys.points[i + 1];
    }
    _sys.plannedIndex--;
}
void lockTimer()
{
    cli();
}
void unlockTimer()
{
    sei();
}
void setRun()
{

}
void waitingPushPoint()
{
    while (true)
    {
        if (_sys.statesIndex <= 5)
        {
            break;
        }
        delay(1);
        // #ifdef DEBUG
        //         String str = "waiting:";
        //         str += _sys.statesIndex;
        //         Serial.println(str);
        // #endif
    }
}
void initTimer()
{
    lockTimer(); // disable all interrupts
    TCCR1A = 0;
    TCCR1B = 0;
    TCNT1 = 0;

    OCR1A = _sys.time;      // compare match register 16MHz/256/1Hz
    TCCR1B |= (1 << WGM12); // CTC mode
    TCCR1B |= (1 << CS11);
    // TCCR1B |= (1 << CS10);   // 64 prescaler
    TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt

    unlockTimer(); // enable all interrupts
}
// float t = 625;
// float dir = 1;
int tt = 0;
// ISR(TIMER1_COMPA_vect)
// {
//     steppers[0].step(1);
//     if (tt % 2 == 0)
//     {
//         tt = 0;
//         OCR1A = t;
//         t += dir;
//         if (t > 625)
//         {

//             dir = -1;
//         }
//         else if (t < 20)
//         {
//             dir = 0;
//         }
//     }
//     tt++;
// }
ISR(TIMER1_COMPA_vect)
{
    run();
}
void run()
{
