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

    long timerCount = 0;
    long time = 2500;
    long defaultTime = 2500;
    long maxTime = 2500;
    long minTime = 25;
    PlannerPoint points[POINTS_COUNT];
    MotionState states[STATES_COUNT];
