If you're supposed to use stepper motors, I bet it's meant that you should deal with non blocking issues. Your stepper motors should be controlled in a non blocking way. Your sonar likewise.
You should have something like this in your loop:
switch (sonar_mode)
{
case 0: // Wait for like half a second to check the distance again
if micros() > next_event
{
sonar_mode++;
}
break;
case 1:
{
digitalWrite(trig, HIGH);
next_event += 10; // Is 10 microseconds enough? Will case 3 catch it?
up_time = micros();
sonar_mode++;
break;
}
case 2:
{
if (micros() > next_event)
{
digitalWrite(trig, LOW);
sonar_mode++;
}
break;
}
case 3: // This is the mode where you detect the echo
{
if (digitalRead(son) == HIGH)
{
distance = (micros() - up_time) / some_clever_constant;
sonar_mode = 0;
next_event += 500000; // Next ping after half a second
}
// YOU HAVE TO CHECK if it's been too long, too, in case no obstacle returns the echo.
break;
}
}
...and something similar for the stepper motors.