         point.decelCount = min(dist, 200);
         point.power = lastState.power;
         point.err = (point.delta[0] > point.delta[1] ? point.delta[0] : -point.delta[1]) / 2;
+        _sys.plannedIndex++;
         _sys.points[_sys.plannedIndex] = point;
-        _sys.plannedIndex++;
     }
 }
 /**

                steppers[i].disableOutputs();
            }
        }
    }
    break;
    }
}

void moveTo(long x, long y)

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