FreeRover is an autonomous robot. This means it acts by itself without human intervention. This posts handles the way its behaviour is implemented.

First I’ll discuss what I want FreeRover to do. Then I’ll describe its sensors and actuators that are used to make it behave. Then I’ll describe the design paradigma I used to implement behaviour. After that I’ll discuss some implementation details.

FreeRover must wander around on its own avoiding obstacles on its way. If it gets into trouble (get stuck) it should be able to free itself. I also want it not to be too repetitive.

In this post I’ll simplify FreeRover a bit. This helps in understanding the behaviour of FreeRover. I will assume that there are only 3 sensors, one for obstacle detection, one to detect if it got stuck and one to find out how far it has been driving. Also, we will discuss only two actuators, one for driving and one for steering.
In reality, FreeRover is more complicated. To find out if it got stuck FreeRover combines information from a US sensor, a touch sensor, motor power consumption and the motor encoders.

As FreeRover has only one kind of behaviour – it just drives around – I choose a hierarchical control system. In this design paradigma behaviour is modelled into nodes, each node reresenting a different kind of behaviour. Only one node executes at any given time. Complex nodes can be constructed from simpler lower level nodes. In FreeRovers case there is one top level node, representing the “wander around” behaviour. Below that thee are different lower level nodes: “drive straight”, “make a corner”, and “get out of trouble”. The “get out of trouble” node consists of several “make a corner” nodes.
Each node can have input parameters that change the behaviour of the node a bit. There are two kinds of input parameters. The first kind tells the node how to behave while executing the node. Speed and steering angle are examples of this. The seconds kind of input tells a node when to finish executing, for example after driving 40 centimeters. Circumstances can prevent nodes from finishing their task. In this post I’ll only discuss one such circumstance, this is when FreeRover gets stuck. When a node finishes it returns to the parent node if it finished on the stop condition or due to circumstances.  This information is used by the parent node to select new behaviour.
Here is some pseudo code for FreeRovers behaviour.

// Top level node for behaviour
// Drive straight until close to an obstacle
// then turn until there is a free path right in front
// repeat forever
// But when got stuck, try to escape

WanderAround()
  start loop forever
    ReturnParameter=DriveStraight(until close_to_an_obstacle, forward)
    If ReturnParameter=succes then ReturnParameter=DriveCorner(until no_obstacle_close_by, someDirection, forward)
    If ReturnParameter=got_stuck then Escape()
  end loop forever

// Drive in a straight line (forward or backward)
// halt function when stop condition is met or when stuck

DriveStraight(stopCondition, direction)
  setSteeringAngle(noAngle)
  setSpeed(direction)
  loop until stopCondition
    If TestForErrors()=true then return got_stuck
  end loop
  return succes

// Drive in a circle, clockwise or counterclockwise, forward or backward
// halt function when stop condition is met or when stuck

DriveCorner(stopCondition, angle, direction)
  setSteeringAngle(angle)
  setSpeed(direction)
  loop until stopCondition
    If TestForErrors()=true then return got_stuck
  end loop
  return succes

// Try to escape from being stuck
// make a backward clockwise turn and then a forward clockwise turn
// repeat this sequence until a free path straight in front

Escape()
  loop forever
    ReturnParameter=DriveCorner(until 10 cm, toLeft, backwards)
    ReturnParameter=DriveCorner(until no_obstacle_close_by, toRight, forward)
    If returnParameter=succes then return succes
  end loop

It is amazing how only a few nodes like this can result in nice and efficient behaviour. If only I could make a proper video. But usually I work on my robot at night and then poor light conditions prevent my from making a video.

I found out that a bathroom is a good place to test robots. There is not too much rubbish in it to disturb your robot. Also, the floor is smooth and most objects in the bathroom have a hard surface and are easily detected by the US sensor.

Here is the robotC code dealing with behaviour. Please note that there is also a rest behaviour I didn’t discuss in the post.  Also the main task implements the top level node for behaviour.

typedef enumWord
{
  brcSucces   = 0,
  brcTouch    = 1,
  brcSonar    = 2,
  brcStall    = 3,
  brcStuck    = 4
} TbehaviourRC;

typedef enumWord
{
  stopOnProblem   = 0,
  stopOnSonar     = 1,
  stopOnDistance  = 2,
  stopOnButton    = 3
} TstopCondition;

typedef enumWord
{
  STrest=0,
  STdrive=1,
  STturn=2,
  STescape=3
} Tstate;

Tstate state=STrest;

/***************************************************************************************************\
*  Behaviour: goStraight                                                                            *
*  Purpose: Allow the car to go straight                                                            *
\***************************************************************************************************/

TbehaviourRC goStraight(int speed, TstopCondition condition, int stopValue)
{
  float startPoint=getDistance();
  if (speed==0)
    speed=50+random(50);
  setSteer(0);
  setSpeed(speed);
  while (true)
  {
    // check for normal termination
    switch (condition)
    {
    case stopOnSonar:
      if (SensorValue[sonar]<stopValue) return brcSucces;
      break;
    case stopOnDistance:
      if (abs(getDistance()-startPoint)>stopValue) return brcSucces;
      break;
    default:
    }

    // Check for error conditions
    if (hitSomethingNew()==true) return brcTouch;
    if (stall()) return brcStall;
    if (speed>0 && SensorValue[sonar]<10) return brcSonar;
  }
}

/***************************************************************************************************\
*  Behaviour: driveCorner                                                                           *
*  Purpose: Allow the car to drive a corner                                                         *
\***************************************************************************************************/

TbehaviourRC driveCorner(int speed, int steer, TstopCondition condition, int stopValue )
{
  float startPoint=getDistance();
  if (speed==0)
    speed=40+random(20);
  setSpeed(speed);
  if (steer==0)
    steer=100*sgn(random(10)-5);
  setSteer(steer);
  while (true)
  {
    // check for normal termination
    switch (condition)
    {
    case stopOnSonar:
      if (SensorValue[sonar]>stopValue) return brcSucces;
      break;
    case stopOnDistance:
      if (abs(getDistance()-startPoint)>stopValue) return brcSucces;
      break;
    default:
    }

    // Check for error conditions
    if (hitSomethingNew()) return brcTouch;
    if (stall()) return brcStall;
    if (speed>0 && SensorValue[sonar]<10) return brcSonar;
  }
}

/***************************************************************************************************\
*  Behaviour: goEscape                                                                              *
*  Purpose: Try to escape from a trapped situation                                                  *
\***************************************************************************************************/

TbehaviourRC goEscape()
{
  int angle, speed, distance=150,i=0;
  TbehaviourRC result;
  while(true)
  {
	  i++;
	  if (desiredAngle<0)
	    angle=100;
	  else
	    angle=-100;
	  if (desiredSpeed<0)
	    speed=40;
	  else
	    speed=-40;
	  setSteer(angle);
	  setSpeed(0);
	  distance+=10;
 	  wait1Msec(300);
	  result=driveCorner(speed, angle, stopOnDistance, distance);
	  if (result==brcSucces &&  (i % 2 ==0) && SensorValue[sonar]>60) return brcSucces;
	  if (i>20) return brcStuck;
  }
}

/***************************************************************************************************\
*  Behaviour: goRest                                                                                *
*  Purpose: Stop the car                                                                            *
\***************************************************************************************************/
TbehaviourRC goRest(TstopCondition condition, int stopValue)
{
  while(true)
  {
    // check for normal termination
    switch (condition)
    {
    case stopOnSonar:
      if (SensorValue[sonar]<stopValue) return brcSucces;
      break;
    case stopOnButton:
      if (nNxtButtonPressed==stopValue) return brcSucces;
      break;
    default:
    }
  }
}

/***************************************************************************************************\
*  Behaviour: Main, top level behaviour                                                             *
*  Purpose: Allow the car to behave                                                                 *
\***************************************************************************************************/

task main()
{
  TbehaviourRC result;
  init();

  while (true)
  {
    //nxtDisplayCenteredBigTextLine(1, "%d", state);
    //nxtDisplayCenteredBigTextLine(3, "%d", result);

    switch (state)
    {
    case STrest:
        result=goRest(stopOnButton, 3);
        if (result==brcSucces)
          state=STdrive;
        else
          state=STrest;
      break;
    case STdrive:
        result=goStraight(0, stopOnSonar, 70);
        if (result==brcSucces)
          state=STturn;
        else
          state=STescape;
      break;
    case STturn:
        result=driveCorner(0, 0, stopOnSonar, 75);
        if (result==brcSucces)
          state=STdrive;
        else
          state=STescape;
      break;
    case STescape:
        result=goEscape();
        if (result==brcSucces)
          state=STdrive;
        else
          state=STrest;
      break;

    default:
    }

}

  StopAllTasks();
}
Advertisements