   {
     return;
   }
-  else if (stepCount < desiredStep)
+  else if ((abs(desiredStep - stepCount) < int(180/stepDegree)*19 ) || 
+  ((desiredStep + MAX_STEP_VALUE - stepCount) < 180) )
   {
     driveUp();
   }


void rotateToFaceAngle(int angle)
{
  desiredDegrees = angle % 360;
  // Step = Degrees / steps per degree * gear ratio
  desiredStep = int(desiredDegrees/stepDegree)*19;

{
  digitalWrite(ENABLE,LOW);
  digitalWrite(DIRECTION_PIN, COUNTERCLOCKWISE);
  while(stepCount != desiredStep)
  {
    delayMicroseconds(DELAY);
    digitalWrite(PULSE_PIN, HIGH);
    delayMicroseconds(DELAY);
    digitalWrite(PULSE_PIN, LOW);
    --stepCount;
    if(stepCount < 0)
    {
      stepCount += MAX_STEP_VALUE;
    }
  }
}

