This updated algorithm is somewhat more robust than the previous post.  I thought that this time I would write the description using pseudo-code.  An important feature to remember for all of our discussions about angles is that 0 (zero) degrees is ALWAYS straight ahead of Rover.

Note: The Forward Motion Counter is initialized to 0 (zero) at the beginning of the program.  This value is used to count the number of times sequentially that the Rover has traveled forward less than a predefined number seconds.

Start of CAv code

Start SONAR turret sweeping with low resolution (45 degrees per sample)

Wait enough time for the turret to do a complete 180 degree sweep

Start forward motion timer.

Loop: (Travel forward until obstacle detection occurs)

Tell Rover to go forward until SONAR detects an obstacle within the stopping distance

NOTE: there is an algorithm that scales the stopping distance based on the angle and the specified minimum obstacle distance. See Puck’s blog for details.

Loop End

Tell Rover to Stop

Calculate time Rover has been moving forward

If the forward time > than a predefined threshold

set the Forward Motion Counter to 0 (zero)

Else

increment the Forward Motion Counter

If the Forward Motion Counter < the predefined threshold  (Rover has not gotten stuck)

Set SONAR turret to more sensitive sweep  (9 degrees per sample)

Backup a little

Loop: (Sweep the entire forward field at fine resolution)

Wait for complete sweep

If longest path is > 36 inches – exit loop

Turn Rover 90 degrees and wait for turn to complete

Loop End

Turn Rover to angle with longest path > 36 inches and wait for turn to complete

Else Forward Motion Counter >= the predefined threshold (Rover has gotten stuck )

Set SONAR turret to more sensitive sweep (9 degrees per sample)

Wait for complete sweep

Initialize distance values and loop counter

Loop while distance < 30 inches and loop counter < Threshold:

Increment loop counter

Find angle of longest distance

Wait for complete sweep

Find angle of longest distance – again

If both distances are > 36 inches set angle to average of the two readings

Otherwise set angle to 45 degrees

Turn Rover to the specified angle

Wait for complete sweep

Loop End

Set Forward Motion Counter to 0 for next time  CAv routine is entered

End of test of Forward Motion Counter

End of CAv code