 		currPosACT++;
 
        // check if we have arrived at the SP and if we are going slow enough to stop
-       if( currPosACT == currPosSP && stepIndex <= 1)
+       if( currPosACT == currPosSP ) //&& stepIndex <= 1)
         {
             //no pulses are needed, stop the timer	
  	    TCCR5B &= 0xF8;

long thcCorrectInc = 1;
int FreqZero = 1212;
int incomingByte = 0;
long currPosACT = 0;
long currPosIN = 0;
long currPosSP = 0;
long thcOffset = 0;
boolean timer5ON = false;
int temp = 0;

	TCNT4 = 0;
	TCNT5 = 0;
	
	currPosACT = 1000000;
	currPosIN = 1000000;
	currPosSP = 1000000;
	timer5ON = 0;
	
//NEED TO INITIALIZE THE PINS THAT ARE INTERRUPT DRIVEN
