R3PTAR Program in C#: roam, chase, bite!

Here I describe a C# R3PTAR Program that let it roam, find and chase the IR Beacon… and when it’s close enough give it three rapid bites 🙂 !

You can compile the C# code I propose in this page using Xamarin Studio 5.10.1 (Build 6), with Mono 4.2.1 as active runtime and the Monobrick Firmware Library 1.2.0.39486 to let it work on board the Lego Mindstorms Ev3 with the Monobrick firmware.

The source code is happily shared under the Code Project Open Licence (CPOL) 1.02.

Description summary for the R3PTAR Program

Part 1 – Define the mission with a finite states machine

Part 2 – Chasing the IR Beacon

Part 3 – Bite! Bite! Bite!

Part 4 – Download the R3PTAR C# Program

And don’t forget to check see it in action!

Part 1 – The robot mission defined in a finite state machine

R3PTAR Program finite state machine — Graphical representation of the finite states machine implemented inside the R3PTAR program

Before programming the robot, we need to define what it has to do, i.e. we need to define the mission the R3PTAR has to follow.

The mission itself is a very simple one: roam around to find a victim (improbably wearing the IR Beacon), chase the victim, and give him a bite when he is close enough.

The mission can so be divided into three steps or phases, or better said it can be partitioned into three states, and that can be easily coded in a finite states machine.

Looking at the picture above we have three states:

$S_1$ : Roam; it is the initial state. In this state, the R3PTAR just roams without a goal, looking for a signal of the IR Beacon. $S_1$ has just one transition to the state $S_2$ , that becomes superable only if $d>0$ , where $d$ is the sensed distance from the IR Beacon. Consider that the $d$ will become greater than $0$ only if the IR Beacon is detected.
$S_2$ : Chase; in this state, the R3PTAR follows the IR Beacon. This state is left for $S_1$ if the IR Beacon signal is lost i.e. $d=0$ , or it is left for $S_3$ if the distance from the IR Beacon become smaller than the biting distance $T_b$ .
$S_3$ : Bite; when the R3PTAR arrives in this state, it means that it is at biting distance from the target :-). Just proceed straight at full speed and bite three times! After that it goes back to $S_1$ .

By the way: the R3PTAR bites of course don’t hurt at all!

The code for the mission control thread, which implements the finite states machine discussed above, is the following:

// Mission phases definition
enum MissionPhase
{
	Roam = 0,
	Chase,
	Bite
};

// Mission phases
MissionPhase currentMissionPhase;

/// <summary>
/// Missions the control thread
/// </summary>
void MissionControlThread()
{
	Thread.CurrentThread.IsBackground = true;
    bool bitten = false;
	while (!stopMissionControlThread.WaitOne (missonControlThreadSampleTime))
	{
		// Next phase will be the same if not transitioned
		MissionPhase nextPhase = currentMissionPhase;
		switch (currentMissionPhase)
		{
			case MissionPhase.Roam:
				// Set main motor speed
				reptarMainMotor.SetSpeed (50);
                // Set led (led switched off);
				Buttons.LedPattern (0);
                // Oscillation 
                waveEnabled = true;
				// Transition
				if (beaconDistance > 0)
				{
					nextPhase = MissionPhase.Chase;
				} 
				break;

			case MissionPhase.Chase:
				// Set main motor speed
				reptarMainMotor.SetSpeed(30);
				// Chasing
				meanValue = 1.5 * beaconHeading;
                // Set led (green);
				Buttons.LedPattern(1);
                // Oscillation 
                waveEnabled = true;
				// Next phase update
				if (beaconDistance <= 0)
				{
					nextPhase = MissionPhase.Roam;
				}
				else if ((beaconDistance > 0) && (beaconDistance < 15))
				{
					nextPhase = MissionPhase.Bite;
				}
				break;

            case MissionPhase.Bite:                      
                // Set led (red);
                Buttons.LedPattern(2);
                // Oscillation 
                waveEnabled = false;
                biteSetpoint = beaconHeading;
                // Bite
                if (!bitten)
                {
                    bitten = true;
                    GiveThreeBites();
                }
                // Next phase update
                if (beaconDistance <= 0)
                {
                    nextPhase = MissionPhase.Roam;
                    bitten = false;
                }
				break;
		}
		// Mission phase update
		currentMissionPhase = nextPhase;
	}
}

// Mission phases definition

enum MissionPhase

{

Roam = 0,

Chase,

Bite

};

// Mission phases

MissionPhase currentMissionPhase;

/// <summary>

/// Missions the control thread

/// </summary>

void MissionControlThread()

{

Thread.CurrentThread.IsBackground = true;

bool bitten = false;

while (!stopMissionControlThread.WaitOne (missonControlThreadSampleTime))

{

// Next phase will be the same if not transitioned

MissionPhase nextPhase = currentMissionPhase;

switch (currentMissionPhase)

{

case MissionPhase.Roam:

// Set main motor speed

reptarMainMotor.SetSpeed (50);

// Set led (led switched off);

Buttons.LedPattern (0);

// Oscillation

waveEnabled = true;

// Transition

if (beaconDistance > 0)

{

nextPhase = MissionPhase.Chase;

}

break;

case MissionPhase.Chase:

// Set main motor speed

reptarMainMotor.SetSpeed(30);

// Chasing

meanValue = 1.5 * beaconHeading;

// Set led (green);

Buttons.LedPattern(1);

// Oscillation

waveEnabled = true;

// Next phase update

if (beaconDistance <= 0)

{

nextPhase = MissionPhase.Roam;

}

else if ((beaconDistance > 0) && (beaconDistance < 15))

{

nextPhase = MissionPhase.Bite;

}

break;

case MissionPhase.Bite:

// Set led (red);

Buttons.LedPattern(2);

// Oscillation

waveEnabled = false;

biteSetpoint = beaconHeading;

// Bite

if (!bitten)

{

bitten = true;

GiveThreeBites();

}

// Next phase update

if (beaconDistance <= 0)

{

nextPhase = MissionPhase.Roam;

bitten = false;

}

break;

}

// Mission phase update

currentMissionPhase = nextPhase;

}

A brief comment on the above code: a currentMissionPhase variable holds the current phase whose value then causes the correspondent case of the switch statement to be executed; inside each case, first the actions are executed, and then the transitions are evaluated; if and when a transition is found true, the currentMissionPhase is updated with the destination state.

← Back to Summary

Part 2 – Chasing the IR Beacon

Chasing the IR Beacon is quite simple, and you can get this goal by coding these two functionalities:

detecting distance and heading from the Ev3 IR Beacon using the Ev3 IR sensor and,
adjusting the mean value of the sine wave driving the R3PTAR to steer toward the Ev3 IR Beacon.

The sine wave mean value is updated directly in the mission control state machine. The algorithm is just a proportional controller having the measured following error as input and the commanded mean value as output. Given that the controller is just the proportional term of a PID controller, you will always get a non zero following error even if it will be neglectable in most cases.

To detect heading and distance of the IR Beacon use the following code in the sensor update section of your code:

/// <summary>
/// IO update thread
/// </summary>
void IOUpdateThread()
{
	Thread.CurrentThread.IsBackground = true;

	while (!stopIOUpdate.WaitOne (ioUpdateSamplingTime)) 
	{
		// Current snake angle and feeding to the oscillating Thread
		currentSnakeAngle = (sbyte) reptarAngleMotor.GetTachoCount();
		reptarAngleRegulator.InputSignal = (sbyte) currentSnakeAngle;

        if (waveEnabled)
        {
            reptarAngleRegulator.SetPoint = (sbyte)wave;
        }
        else
        {
            reptarAngleRegulator.SetPoint = biteSetpoint;
        }
		reptarAngleMotor.SetPower (reptarAngleRegulator.OutputSignal);

		// Updates the beacon heading only when the head is centered
        if (((currentSnakeAngle > -3) && (currentSnakeAngle < 3)) || !waveEnabled)
		{
			BeaconLocation beaconLocation = irSensor.ReadBeaconLocation ();
			beaconHeading = (sbyte) beaconLocation.Location;
			beaconDistance = (sbyte) beaconLocation.Distance;
		}
	}
}

/// <summary>

/// IO update thread

/// </summary>

void IOUpdateThread()

{

Thread.CurrentThread.IsBackground = true;

while (!stopIOUpdate.WaitOne (ioUpdateSamplingTime))

{

// Current snake angle and feeding to the oscillating Thread

currentSnakeAngle = (sbyte) reptarAngleMotor.GetTachoCount();

reptarAngleRegulator.InputSignal = (sbyte) currentSnakeAngle;

if (waveEnabled)

{

reptarAngleRegulator.SetPoint = (sbyte)wave;

}

else

{

reptarAngleRegulator.SetPoint = biteSetpoint;

}

reptarAngleMotor.SetPower (reptarAngleRegulator.OutputSignal);

// Updates the beacon heading only when the head is centered

if (((currentSnakeAngle > -3) && (currentSnakeAngle < 3)) || !waveEnabled)

{

BeaconLocation beaconLocation = irSensor.ReadBeaconLocation ();

beaconHeading = (sbyte) beaconLocation.Location;

beaconDistance = (sbyte) beaconLocation.Distance;

}

As you can notice, the IR Beacon distance and heading are updated only when the robot has his neck pointing straight forward, so that it can take correct readings even if paying a bit in terms of responsiveness.

← Back to Summary

Part 3 – Bite! Bite! Bite!

Giving three bytes is just a matter of slinging rapidly the R3PTAR head forward, while commanding the robot to advance at full speed!

/// <summary>
/// Gives three bites!
/// </summary>
void GiveThreeBites()
{
    reptarMainMotor.SetSpeed(100);
    reptarNeckMotor.SpeedProfileTime(100, 100, 100, 100, true);
    Thread.Sleep(300);
    reptarNeckMotor.SpeedProfileTime((sbyte)-50, 100, 100, 100, true);
    Thread.Sleep(300);
    reptarNeckMotor.SpeedProfileTime(100, 100, 100, 100, true);
    Thread.Sleep(300);
    reptarNeckMotor.SpeedProfileTime((sbyte)-50, 100, 100, 100, true);
    Thread.Sleep(300);
    reptarNeckMotor.SpeedProfileTime(100, 100, 100, 100, true);
    Thread.Sleep(300);
    reptarNeckMotor.SpeedProfileTime((sbyte)-50, 100, 100, 100, true);
    reptarMainMotor.Off();
}

/// <summary>

/// Gives three bites!

/// </summary>

void GiveThreeBites()

{

reptarMainMotor.SetSpeed(100);

reptarNeckMotor.SpeedProfileTime(100, 100, 100, 100, true);

Thread.Sleep(300);

reptarNeckMotor.SpeedProfileTime((sbyte)-50, 100, 100, 100, true);

Thread.Sleep(300);

reptarNeckMotor.SpeedProfileTime(100, 100, 100, 100, true);

Thread.Sleep(300);

reptarNeckMotor.SpeedProfileTime((sbyte)-50, 100, 100, 100, true);

Thread.Sleep(300);

reptarNeckMotor.SpeedProfileTime(100, 100, 100, 100, true);

Thread.Sleep(300);

reptarNeckMotor.SpeedProfileTime((sbyte)-50, 100, 100, 100, true);

reptarMainMotor.Off();

}

← Back to Summary

Part 4 – Download the R3PTAR Program

Download the code here.

You can compile the code I propose in this page and in the following pages using Xamarin Studio 5.10.1 (Build 6), with Mono 4.2.1 as active runtime and the Monobrick Firmware Library 1.2.0.39486 to let it work on board the Lego Mindstorms Ev3 with the Monobrick firmware.

The source code is happily shared under the Code Project Open Licence (CPOL) 1.02.

R3PTAR Program: roam, chase, bite!