Thread.sleep(nsToSleep/17000000, ...);
Должно быть
Thread.sleep(nsToSleep/1000000, nsToSleep%1000000);
Когда вы там переводите наносекунды в миллисекунды.
Кроме того, как указал diciu в комментарии к вашему вопросу, вы должны переместить вычисление для времени начала за пределы цикла.
Я не проверял это, хотя и не уверен, что это все, что вам нужно, чтобы исправить это, но быстрый взгляд на ваш код, кажется, показывает, что это проблемы.