Lego Mindstorms EV3 Components: Infrared Sensor - Part 3

This is the last part of our articles on the mindstorm's infrared sensor. After looking at the abilities of the remote sensor to recognize objects and commands from the remote control we will now have a closer a look on the functionality of finding an infrared signal sent by the remote control constantly.

In february the lejos api had a new release - 0.6.0-ALPHA, so while using the 0.5.0-ALPHA-release in our last articles we will by now use the 0.6.0-release.

The robot

We will use the robot from the last two articles on the infrared sensor again, and of course we will use the remote control. If you haven't checked the infrared articles already ( Part 1/Part 2) you should do so.

The parcours

There is the robot and there is the remote control. In this part we want to tell the robot: Hold a distance of about 20-35cm to the remote control!
Which means that if we move the remote control nearer to the robot, it should move backwards and if we move the remote control away from the robot it should follow. We will only use the toggling button this time. If it is toggled and sending a signal, the robot should care about holding the right distance to the remote control. The program will exit if the signal ends (the button is untoggled).

The infrared sensor API

As announced in our last part on the infrared sensor we will use the SeekMode this time to receive the relative position and distance of the remote control.

What we will need to instantiate therefore is: the infrared sensor and a RangeFinderAdaptor for the infrared sensor.

As soon as we instantiated a RangeFinderAdapter we can use its getRanges()-method which returns all required information to let the robot find the remote control and its position.

We will use a thread which will check those informations continuously so that the robot will always react to changes of the remote controls position.

Let us directly jump to the finished code and see how it works in detail.

The code

We have two classes this time: The Main-class and the InfraredSignalCheckerThread-class. First, let's look at the main-class:

public class Main {

    protected final static double PILOT_SPEED = 60.0;
    protected final static int PILOT_ACCELERATION = 500;
    protected final static double WHEEL_DIAMETER = 30.0f;
    protected final static double DISTANCE_BETWEEN_WHEELS = 170.0;

    public static void main(String[] args) throws InterruptedException {

        final DifferentialPilot pilot = new DifferentialPilot(WHEEL_DIAMETER, DISTANCE_BETWEEN_WHEELS, Motor.C, Motor.B);
        final EV3IRSensor infraredSensor = new EV3IRSensor(SensorPort.S2);
        final RangeFinderAdaptor rangeFinderAdaptor = new RangeFinderAdaptor(infraredSensor.getSeekMode());
        final InfraredSignalCheckerThread checkerThread = new InfraredSignalCheckerThread(rangeFinderAdaptor, pilot);

        configurePilot(pilot);
        checkerThread.start();

        Button.waitForAnyPress();
    }

    private static void configurePilot(final DifferentialPilot pilot) {
        pilot.setAcceleration(PILOT_ACCELERATION);
        pilot.setRotateSpeed(PILOT_SPEED);
        pilot.setTravelSpeed(PILOT_SPEED);
    }
}

First, we instantiate the pilot and the infraredSensor as always. Then we instantiate the RangeFinderAdapter which gets the infrared-sensor's SeekMode as a parameter. The RangeFinderAdapter will return all informations on the remote control's position. We then instantiate a InfraredSignalCheckerThread which gets the adaptor to check the remote control's position continuosly and the pilot to let the robot react on the remote control's position.

After the instantiations we set the pilot's speeds and start the thread. So now let's have a look at our Thread-class:

public class InfraredSignalCheckerThread extends Thread {

    private RangeFinderAdaptor adaptor;
    private DifferentialPilot pilot;
    private boolean isMoving = false;
    private  boolean isBearing = false;

    public InfraredSignalCheckerThread(final RangeFinderAdaptor adaptor, final DifferentialPilot pilot){
        this.adaptor = adaptor;
        this.pilot = pilot;
    }

    @Override
    public void run() {
        while(true){
            final float bearing = adaptor.getRanges()[0];
            final float distance = adaptor.getRanges()[1];
            if(distance < 129.0f && distance > 127.0f && bearing < 1.0f){
                System.exit(0);
            }
            doBearing(bearing);
            doMoving(distance);
        }
    }

    private void doBearing(final float bearing) {
        if(bearingIsOk(bearing)){
            isBearing = false;
        }
        else {
            if(bearing > 5.0f){
                pilot.rotateRight();
                isBearing = true;
            } else {
                pilot.rotateLeft();
                isBearing = true;
            }
        }
    }

    private void doMoving(final float distance) {
        if(!isBearing){
            if(!isMoving){
                pilot.quickStop();
            }
            if(distanceIsOk(distance)){
                pilot.quickStop();
                isMoving = false;
            }
            else {
                if(distance < 25.0f){
                    pilot.backward();
                } else {
                    pilot.forward();
                }
                isMoving = true;
            }
        }
    }

    private boolean bearingIsOk(final float bearing) {
        if(bearing < 5.0f && bearing > -5.0f){
            return true;
        }
        return false;
    }

    private boolean distanceIsOk(final float distance) {
        if(distance >20.0f && distance < 35.0f){
            return true;
        }
        return false;
    }
}

The constructor is easy as it only saves the adaptor and the pilot into private variables so that we can use them in the run-method. If you have a look at the private variables you will see that there are two boolean attributes isBearing and isMoving. We need those variables because we want to robot to first react on direction changes of the remote control (which is done by rotating), and we don't want the robot to react on distance changes (which is done by moving forwards/backwards) until the bearing has finished.

Okay, let's have a look at the run-method which is called when we start the thread in the main-class.
We need two information on our remote control: The relative bearing to the robot and the distance to the robot. As said earlier, the getRanges()-Method returns those information. Here is what the api says:

/**
     * return a sample provider for the IR sensor operating in seek mode
     * The provider returns the bearing and distance to one or more IR beacons.
     * Up to four
     * beacons (on different channels 0-3) can be detected. Each beacon has an
     * associated two byte value (so the beacon on channel 0 will have values
     * in locations 0 and 1 in the array. The first location contains the relative
     * bearing to the beacon, the second the distance.

     * The bearing values range from -12 to +12 (with values increasing clockwise
     * when looking from behind the sensor. A bearing of 0 indicates the beacon is
     * directly in front of the sensor. Distance values (0-100) are in cm and if no
     * beacon is detected a bearing of 0 and a distance of 255 is returned.

So, we only care for the first two floats of the array because we only have one remote control. And we want the bearing-information of it ([0]) and the distance-information ([1]).

After writing those informations into float variables we check, if the information are valid: The api says, if no signal is found the value of bearing will be 0 and the distance will be 255. So first, we check this case and exit the program if no signal is found (I found out, that if no signal is sent, the distance value is 128.0 and not 255 as written in the api).

If the values are valid (a signal was received we call the methods doBearing and doMoving). Those methods will make the robot first react on direction changes of the remote control, and then react on distance changes afterwards. Let's have a closer look at these two methods, first the doBearing-Method:

private void doBearing(final float bearing) {
        if(bearingIsOk(bearing)){
            isBearing = false;
        }
        else {
            if(bearing > 5.0f){
                pilot.rotateRight();
                isBearing = true;
            } else {
                pilot.rotateLeft();
                isBearing = true;
            }
        }
    }

private boolean bearingIsOk(final float bearing) {
        if(bearing < 5.0f && bearing > -5.0f){
            return true;
        }
        return false;
    }

First, we check if the bearing is okay, which means that the remote control is more or less directly in front of the robot. As said in the api that value can be anything between -12 and 12 (I found out, that this value can be anything between -25 and 25). So we tell the program, if the value is between -5 and 5 the bearing is okay and the reaction of the robot has finished. If the bearing is not okay, we tell the robot to rotate right of the bearing is greater than 5, or rotate left if the value is lower than -5. While rotating, we tell the program, that the robot is reacting to the bearing, by setting the isBearing-attribute to true. By the rotation the bearing value will approach to 0. When that's the case we tell the program, that the robot has finished its reaction and set the isBearing variable to false.

And here's the doMoving-Method:

private void doMoving(final float distance) {
        if(!isBearing){
            if(!isMoving){
                pilot.quickStop();
            }
            if(distanceIsOk(distance)){
                pilot.quickStop();
                isMoving = false;
            }
            else {
                if(distance < 20.0f){
                    pilot.backward();
                } else {
                    pilot.forward();
                }
                isMoving = true;
            }
        }
    }

private boolean distanceIsOk(final float distance) {
        if(distance >20.0f && distance < 35.0f){
            return true;
        }
        return false;
    }

When the doBearing-method has finished, the doMoving-method will start. But we only want to react on the remote control's distance if the robot has finished it's reaction to the bearing. So we first check, if the robot is not bearing any longer. The second "if" that follows now is a bit tricky: We check, if the robot is moving as a result from the doMoving-Method or as a result from the doBearing-method. If it NOT moving as a result of the doMoving-method, it should stop because then we have the case, that the robot is still rotating as a reacting to the bearing (we didn't tell the program to stop rotating any time in the doBearing-method). So after that, we check if the remote control's distance is okay (which means the remote control is in a distance of 20 to 35cm) to the robot. If that is the case the robot should stop and we mark the isMoving-attribute as false. If the distance is NOT okay we tell the robot to move backwards of the distance is less then 25 (because it should "flee" from the remote control) or move forwards if the distance is greater than 35 (because it should "follow" the remote control).

The result

And here is the result (watch on youtube for better quality):