Prinzipiell finde ich bei Deinem Aufbau die Warteschleifen problematisch -- sowohl die blockierenden TWI-Schleifen als auch die wait() in der Hauptschleife.

Also: entweder so umcoden, daß Dein Prog *ohne* Warteschleifen auskommt. Oder: die Berechnung der Drehzahl in die ISR verlagern. Falls nämlich wegen des Rumgammelns in den Wartern mehr als 1 Periode der Drehzahlmessung vergeht, hast du ein Problem.

Ein weiteres Problem bereitet der Überlauf von TCNT1, ich sehe nicht, wo der Fall EndTime-StartTime < 0 behandelt wird.

Hast du mal überlegt, den InputCapture von Timer1 zu verwenden? Für Zeitmessung passt das vielleicht besser.