# A better way to keep your robot straight using a NXT sonar sensor

• ### Question

• So I have robot with a sonar sensor of of the side, it keeps itself straight by checking the distance from the wall right? Then I use the RGB sensor to pick up commands from the ground (green turn left etc...) The issue I am having is that my algorithm for keeping the thing straight is lame. See below. Does someone have a better way to do this? Is there something in the WROX book that I can use an example?

The other problem with this is I have no what to know that I am currently in a turn to the right at 60 degrees or whatever so it's really hard to say - please just adjust and go straight again robot

Thanks for your input.

```private void SonarSensorUpdateHandlerTwo(SonarSensorUpdate update)
{
const int duration = 500;
if(!state.Started)
return;
if(update.Body.Distance < 10)
SpawnIterator(-1d, duration, Turn);
if(update.Body.Distance > 10)
SpawnIterator(-1d, duration, Turn);
}```

• Edited by Wednesday, August 8, 2012 11:16 AM
Wednesday, August 8, 2012 11:15 AM

### Answers

• Hi,

I would suggest simplifying the logic further and getting rid of all if-elses for the distance-to-degree translations. Use a continuous translation function. And for a very simple and smooth obstacle avoidance algo I would also suggest not dealing with degrees of turn, but rather adjusting momentary power to the wheels.

There are many ways to skin this cat, and you are right about 'someone having solved it before'. In fact there are many many variations on the theme: "if you see an obstacle on the left - turn right, and vice versa". But there is no single 'best' way. Each algo has its strengths and issues. The best way to develop a sense of how obstacle avoidance works is to experiment (and its fun too as you mention).

Sunday, August 12, 2012 4:37 PM

### All replies

• Hi,

By 'lame' do you mean to say that its making sharp turns where you expect it to make smooth turns? (looks like it from the code).

I would suggest not using 'atomic' turn operations like you have above, but adjust turn smoothly as a function of distance, i.e. start turning slowly when sonar starts reporting less than 30 sm to target (while still maintaining forward motion). The, once you reach a 'too close' distance, i.e. 10 sm, stop and turn for as long as it takes to clear an obstacle (but no longer).

This can be done in the same sonar update handler.

Thursday, August 9, 2012 9:54 PM
• Yeah, I started using the distance to gauge the amount of turn and it sometimes works out. It sometimes gets weird and I can't really figure out why. Here is what I have now (it's a little sloppy but you get the picture). I have using a counter to say hey we just vered to the right/left but we aren't getting any closer after three updates so adjust again. It's sort of fun to play with it and watch the robot go all funky / sometimes work pretty good but I can't help thinking someone has solved this before?

```  private void SonarSensorUpdateHandler(SonarSensorUpdate update)
{
double degrees = 130;

LogInfo("Current Distance: " + update.Body.Distance.ToString(CultureInfo.InvariantCulture));
window.UpdateCurrentDistance(update.Body.Distance.ToString(CultureInfo.InvariantCulture));
if(state.Started)
{
if(state.NoStateChangeCount >= 3)
{
state.NoStateChangeCount = 0;
state.CurrentState = CurrentState.NotTurning;
state.DegreesOfPreviousTurn = 0;
state.LastTurnDirection = Direction.None;
}
if (update.Body.Distance < 10 && state.CurrentState != CurrentState.HeadingRight)
{

if (update.Body.Distance == 9)
degrees = 90;
if (update.Body.Distance == 8)
degrees = 110;
if (update.Body.Distance == 7)
degrees = 120;
if (update.Body.Distance == 6)
degrees = 130;
if (update.Body.Distance == 5)
degrees = 135;
if (update.Body.Distance == 4)
degrees = 140;
if (update.Body.Distance <= 3)
degrees = 145;
if (state.LastTurnDirection == Direction.Left)
degrees = degrees + state.DegreesOfPreviousTurn;
state.LastTurnDirection = Direction.Right;
LogInfo("Turning Right: " + degrees.ToString(CultureInfo.InvariantCulture) + " degrees");
SpawnIterator(-1d, degrees, Turn);
state.DegreesOfPreviousTurn = degrees;
state.CurrentState = CurrentState.HeadingRight;
state.NoStateChangeCount++;
}

if (update.Body.Distance > 10 && state.CurrentState != CurrentState.HeadingLeft)
{
if (update.Body.Distance == 11)
degrees = 90;
if (update.Body.Distance == 12)
degrees = 110;
if (update.Body.Distance == 13)
degrees = 120;
if (update.Body.Distance == 14)
degrees = 130;
if (update.Body.Distance == 15)
degrees = 135;
if (update.Body.Distance == 16)
degrees = 140;
if (update.Body.Distance >= 17)
degrees = 145;
if (state.LastTurnDirection == Direction.Right)
degrees = degrees + state.DegreesOfPreviousTurn;
state.LastTurnDirection = Direction.Left;
LogInfo("Turning Left: " + degrees.ToString(CultureInfo.InvariantCulture) + " degrees");
SpawnIterator(1d, degrees, Turn);
state.DegreesOfPreviousTurn = degrees;
state.CurrentState = CurrentState.HeadingLeft;
state.NoStateChangeCount++;
}
}
}```

Here is the turn handler

``` private IEnumerator<ITask> Turn(double direction, double degrees)
{
yield return Arbiter.Choice(steeringMotorOperationsPort.SetMotorRotation(new SetMotorRotationRequest
{
TargetPower = direction,
StopAfterDegrees = degrees
}),
x => LogInfo("Rotated Motor"), x => LogError("Couldn't rotate the motor"));
}```

Friday, August 10, 2012 11:47 PM
• Hi,

I would suggest simplifying the logic further and getting rid of all if-elses for the distance-to-degree translations. Use a continuous translation function. And for a very simple and smooth obstacle avoidance algo I would also suggest not dealing with degrees of turn, but rather adjusting momentary power to the wheels.

There are many ways to skin this cat, and you are right about 'someone having solved it before'. In fact there are many many variations on the theme: "if you see an obstacle on the left - turn right, and vice versa". But there is no single 'best' way. Each algo has its strengths and issues. The best way to develop a sense of how obstacle avoidance works is to experiment (and its fun too as you mention).

Sunday, August 12, 2012 4:37 PM
• Actually the problem is that is sometimes wanders off and doesn't seem to get the updates. It's like the sensor stops reading but I can see the output that indeed it was getting readings. Is there are way cancel the previous operation? I think sometimes that is the issue - I was turning right then it didn't finish now it is too close and needs to turn away but it's still trying to turn to the right? Where in there can I say "cancel whatever you where trying to do" - Or how actually.

Thanks!

Thursday, August 16, 2012 5:38 PM
• I've found it easier to use a 'game loop' approach to these types of problems, combined with a 'state machine' containing your objective.  For example, in pseudo-code:

var objective = followLeftWall;
while (true) // loop
{
switch (objective)
case followLeftWall:
if (noWallSeen) setMotorsToGentleLeft;
else if (leftWallSeenWithinAcceptableDistance) setMotorsToStraight;
else if (leftWallTooClose) setMotorsToGentleRight;
else if (leftWallTooFar) setMotorsToGentleLeft;
...
delay(50 ms for 20 hz decisions)
}

This algorithm is too simple because it will cause an oscillation, but it should give you a starting idea.

This is essentially what Greg meant by "not dealing with degrees of turn, but rather adjusting momentary power to the wheels" because this approach readjusts the wheel power 20 times a second rather than starts an asynchronous operation like 'turn' that may need to be interrupted if the observed obstacles or the objective changes while the operation is still in progress.  Tracking and cancelling these time based operations can be much more complex than simply reevaluating your robot's action in a frequent loop.

Monday, August 20, 2012 11:44 PM