Code:
	/*  Servo0  */
	if (i == 1)
			{
				i++;
			}
	if (i == 2)
			{
				i++;
			}
	/*  Servo1  */
	if (i == 3)
			{
				i++;
			}
Das war doch so sicher nicht gedacht. Deine ISR flutscht in einem Rutsch durch. Besser wäre vielleicht ein select..case-Ansatz