Something you should probably do -first-, is to define your states. Offhand, I don't know what they would necessarily be in your case, but probably something like (lets call this an example):
1. Moving forward (while checking distance)
2. Scanning Left/Right distance
3. Backing up to the left
4. Backing up to the right
So - you would have code that would look something like (this is pseudocode here):
int state=1;
loop () {
switch (state) {
case 1: // Moving forward (while checking distance)
forward();
distanceC = getDistanceCenter();
if (distanceC < 20) state = 2;
break;
case 2: // Scanning Left/Right distance
distanceL=getDistanceLeft();
distanceR=getDistanceRight();
if (distanceL < distanceR) {
// object to the left, so we want to backup to the left
state = 3;
}
else {
// object to the right, so we want to backup to the right
state = 4;
}
break;
case 3; // Backing up to the left
backupToLeft();
state = 1;
break;
case 4; // Backing up to the right
backupToRight();
state = 2;
break;
default: // should never get here!
state = 1;
break;
}
}
int getDistanceCenter() {
moveServoCenter();
return (getDistance());
}
int getDistanceLeft() {
moveServoLeft();
return (getDistance());
}
int getDistanceRight() {
moveServoRight();
return (getDistance());
}
int getDistance() {
// activate PING here, get distance, and return it
return (distance);
}
Ok - I am not going to claim the above is perfect, or even right for your situation - but it should give you an idea on how to proceed. If you look at the link I gave earlier to the electro-tech forums post, you will see that the guy I am having a discussion with on state machines posts a state-diagram detailing the operations of a simple elevator controller with 7 states. My first implementation used only 3 states (well, 4, actually - but it could be simplified to 3), but managed to replicate his version with 7 states; he wanted the 7 state version, which I admit I didn't replicate properly, so I re-did the code, and it seemed to come out cleaner.
What this all means is that first, you -must- determine your states, and draw a flow diagram if you can. Show each state, show what causes the transition between states, and try to simplify things to as few states as seems clear, needed, possible (note, in my example above, I reduced the number of states to 3 for the elevator example, yet it turned out to have a clearer representation with 7 states - so what to take away from that is, if in the coding of a state machine, it seems overly complex - perhaps you need -more- states, not fewer!).
Something -not- to take away from his example, though, is the temptation to use if-then constructs and gotos for the implementation of the state machine. While I admit, it looks very tempting, that may be a path towards frustration, especially if your state machine needs to change mid-project, or in the future. But ultimately, its your call.
Hope this helps. Good luck!
