Building a Netduino Rover – Part 4

See other posts in this series

Basic Object Avoidance

I’ve designed this rover to be autonomous rather than remote controlled. The software is designed around the idea of combining many simple behaviours together to produce what appears to be complex behaviour. You can think of the behaviours as being arranged in a stack, with the (important) collision detection at the top, and the (not quite so important) steering behaviours at the bottom. Each behaviour in the stack is executed in turn, starting at the top. Each behaviour can decide to stop further processing of the stack. After a short delay, the process is repeated starting again from the top of the stack.

The example on this post only has two behaviours, ReverseBehaviour which moves the rover backwards when an obstacle is nearby, and ForwardBehaviour which moves the rover forwards. The rover won’t appear very smart with only two behaviours, but more can be added later. This is a scalable design that is easy to maintain.

Here’s the Rover class showing the behaviour stack and the Motor and RangeSensor classes from part 3.

using NetduinoRover.Behaviours;
using NetduinoRover.Outputs;
using NetduinoRover.Sensors;
using SecretLabs.NETMF.Hardware.Netduino;

namespace NetduinoRover
{
    public class Rover
    {
        private Motor _leftMotor;
        private Motor _rightMotor;
        private RangeSensor _leftSensor;
        private RangeSensor _rightSensor;
        private IBehaviour[] _behaviours;
        private int STEER_CORRECTION = 320;
        
        public Rover()
        {
            // Define outputs
            _leftMotor = new Motor(Pins.GPIO_PIN_D6, Pins.GPIO_PIN_D7, STEER_CORRECTION);
            _rightMotor = new Motor(Pins.GPIO_PIN_D5, Pins.GPIO_PIN_D4, STEER_CORRECTION * -1);

            // Define inputs
            _leftSensor = new RangeSensor(Pins.GPIO_PIN_A5);
            _rightSensor = new RangeSensor(Pins.GPIO_PIN_A4);

            // Define the behaviours you want - the order is very important!
            _behaviours = new IBehaviour[2];
            _behaviours[0] = new ReverseBehaviour(_leftMotor, _rightMotor, _leftSensor, _rightSensor);
            _behaviours[1] = new ForwardBehaviour(_leftMotor, _rightMotor);
        }

        public void Move()
        {
            foreach (var behaviour in _behaviours)
            {
                var hasFired = behaviour.Execute();

                // If this behaviour fired, stop processing other behaviours in the array
                if (hasFired)
                    break;
            }
        }
    }
}

By creating an interface for the different behaviours, the Rover class becomes de-coupled from the actual implementations. This makes it easier to work with an array of behaviours – it also makes it easier to add new behaviours in the future. The interface contains a single method Execute which returns a boolean to show if further behaviours should be executed.

namespace NetduinoRover.Behaviours
{
    interface IBehaviour
    {
        bool Execute();
    }
}

As the name suggests, the ForwardBehaviour instructs the Rover to move forward (at 60% speed). The Execute method returns false to allow other behaviours in the stack to execute.

using NetduinoRover.Outputs;

namespace NetduinoRover.Behaviours
{
    public class ForwardBehaviour : IBehaviour
    {
        private Motor _leftMotor;
        private Motor _rightMotor;

        public ForwardBehaviour(Motor leftMotor, Motor rightMotor)
        {
            _leftMotor = leftMotor;
            _rightMotor = rightMotor;
        }

        public bool Execute()
        {
            _leftMotor.SetSpeed(0.6);
            _rightMotor.SetSpeed(0.6);
            return false;
        }
    }
}

The ReverseBehaviour checks to see if either of the range sensors are close to an obstacle. If they are, the motors are set to reverse and true is returned to prevent further behaviours from running. This allows the ReverseBehaviour to take precedence over other behaviours, since avoiding obstacles is more important than always moving forward.

using Microsoft.SPOT;
using NetduinoRover.Outputs;
using NetduinoRover.Sensors;

namespace NetduinoRover.Behaviours
{
    public class ReverseBehaviour : IBehaviour
    {
        private Motor _leftMotor;
        private Motor _rightMotor;
        private RangeSensor _leftSensor;
        private RangeSensor _rightSensor;

        public ReverseBehaviour(Motor leftMotor, Motor rightMotor, RangeSensor leftSensor, RangeSensor rightSensor)
        {
            _leftMotor = leftMotor;
            _rightMotor = rightMotor;
            _leftSensor = leftSensor;
            _rightSensor = rightSensor;
        }

        public bool Execute()
        {
            // Are we going to hit somthing?
            if (_leftSensor.Read() < 10 || _rightSensor.Read() < 10)
            {
                // Slow reverse
                _leftMotor.SetSpeed(-0.8);
                _rightMotor.SetSpeed(-0.8);
                return true;
            }
            return false;
        }
    }
}

The main program simply creates an instance of the Rover class and calls the Move method repeatedly.

using System.Threading;

namespace NetduinoRover
{
    public class Program
    {
        public static void Main()
        {
            var rover = new Rover();
            
            while (true)
            {
                rover.Move();
                Thread.Sleep(100);
            }
        }
    }
}

Here’s a video contrasting this approach with a different approach (Fuzzy Logic).

Advertisements
This entry was posted in Projects and tagged , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s