Not sure what the issue is, but the way the code is written, two unnecessary if tests are going to be executed on each pass through the loop. Why not use something like:
int distance;
distance = ReadDistanceSensor();
if (distance >= 40 && distance <= 120)
{
ForwardNormal();
} else {
if (distance < 40)
{
ForwardSlow();
} else {
ForwardFast();
}
}
As a general rule, anytime you have a situation where one of the conditions prevails more often than the others, place its test first. That way, you avoid the remaining tests. As your code is written, if the first condition is true, the remaining if conditions must be false, which means they are executed unnecessarily. Nested if statements as shown here avoid the redudant if tests when the first condition is true.