
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

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:
: Roam; it is the initial state. In this state, the R3PTAR just roams without a goal, looking for a signal of the IR Beacon.
has just one transition to the state
, that becomes superable only if
, where
is the sensed distance from the IR Beacon. Consider that the
will become greater than
only if the IR Beacon is detected.
: Chase; in this state, the R3PTAR follows the IR Beacon. This state is left for
if the IR Beacon signal is lost i.e.
, or it is left for
if the distance from the IR Beacon become smaller than the biting distance
.
: 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
.
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:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 |
// 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.
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:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 |
/// <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.
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!
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
/// <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(); } |
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.