Control Software
how to program a robot and not to go mad while doing so
Last year's experiences lead to the decision to completely rebuild the software for Ester to allow easier modifications and tests of different strategies. The robot's control considerably depends on the detection of its position and its surroundings (e.g. I am in front of the goal and I have the ball) and the application of the appropriate solution (shoot). The classic tool for programming that kind of application is FSM (finite state machine), but the usual implementations seem hard to maintain and in general are somewhat clumsy. Can't we do better?
FSM — finite state machines
The basic scheme we used in more or less all of our robots since
Alena looks like this:
while (true) { sendReceive(commandStruct, stateStruct); switch(state) { case INIT: state = init(stateStruct); break; case GOING_TO_SUN: state = goingToSun(stateStruct); break; case SUN_REACHED: state = sunReached(stateStruct); break; case DEPLOYING_SUN_FLAG: state = deployingSunFlag(stateStruct); break; … } }
It is a simple and straight forward solution whose functioning can be
explained very easily. Basically, it is an endless loop in which we
alternate communication with the robot and the processing of the incoming
data.
The communication with the robot stabilized after the initial experiments
with different asynchronous message types over the serial interface to
synchronous communication using to two basic structures. The structure
commandStruct
contains the state we intend the microprocessor
output pins to have and stateStruct
represents the state of the
input devices (A/D converter and digital input).The processing of incoming data determines the robot's internal state. In
the state INIT, for example, the robot waits for the start signal and in
that case begins to measure the time and advances to the state
GOING_TO_SUN. In all the other states it checks if 90s have passed and
if the transitional condition, specified for the given state, is met.
Simple, clear, comprehensible at the first look. So where is the problem? The
FSM, implemented in that way, fits a system which can be analyzed thoroughly and
specified precisely all the needed states and transitions. Experiments show that
a mobile robot is not such a system. There are many activities which seem to go
across all the states. When we try to handle these activities, the number of
states explodes beyond tolerable limits while many states are very similar. So
the code structured like this changes quickly into a mess and becomes hard to
maintain or extend. The main reason the code duplication (i.e.
in the case of Alena, driving to the sun is nearly
identical to driving to another planet etc.).
As you can seen in the mentioned examples even the actual definition of
single states can become very hard problem. As long as there are states with
multiple input edges, you (the programmer) have to guarantee that all the edges
have the same or at least similar preconditions. For example in the state
responsible for the sun's flag placement, it is necessary that the robot is next
to the sun (otherwise the action wouldn't make sense). Due to the amount of code
it is almost an impossible task. The structure of the code does not help, quite
the opposite. In theory it is possible to transition from any state to any other
state (complete graph) and it depends solely on the programmer (and his
discipline) if the proper conditions are met. You can probably guess what
happens when several programmers work simultaneously on the code or when someone
has to continue in a work started by someone else.
Attempt to improve no. 1
The first attempt to regain control of our code was the implementation of
a nested FSM. The motivation was that it is the same if the robot drives to the
sun, the puck or the goal, so that driving to a target can be an independent skill
that can be used in different situations. This is also valid for other
activities.
The main loop will then look like this:
while (true) { sendReceive(commandStruct, stateStruct); switch(state) { case AWAITING_START: controller.stop(); if (stateStruct.startCable) { timeLeft = 90.0; state = PLAYING; } break; case PLAYING: controller.execute(); timeLeft -= stateStruct.dt; if (timeLeft <= 0) { state = GAME_OVER; } break; case GAME_OVER: controller.stop(); if (currentSpeed == 0) { state = AWAITING_START; controller.reset(); } break; } }
As you can see we've achieved starting the robot by starting cable and
stopping after 90s (after the end of match, the control program resets and
prepares for another fight which helps testing). The code for this
activity is isolated from the rest of the control system and its easy to
visualize what and why it does what it does. The control system is completely implemented in
the
controller
object.The necessary FSM-implementation of driving to a target can be
isolated in a similar fashion:
int goTo(goal) { offset = calculateOffsetToGoal(goal); switch (action(offset)) { case TURN: action = &turn; return Command::IN_PROGRESS; case STRAIGHT: action = &straightFW; return Command::IN_PROGRESS; case STOP: action = &stop; return Command::IN_PROGRESS; case IN_PROGRESS: return Command::IN_PROGRESS; case RESULT_OK: return Command::RESULT_OK; } return Command::IN_PROGRESS; }
In this way, we've reached one of our goals — we managed to divide the
code into smaller sections so different people can easily work with it (the
coupling was reduced). But up to a certain point , we still have the problem with
consistency of the transitions. Nested FSMs must be (re)initialized in certain
situations and it is typically expected that its action procedures are called
periodically in every step until they say “enough” themselves and that
they keep complete control of some of the robot's sections (i.e. it means the
goTo(goal)
FSM controls the motors exclusively). However, these
requirements still stay the responsibility of the programmer, even when the code
modularization helps. Dana was programmed this way.What about the competition?
Because we had not been completely satisfied with our code structure,
we were interested how the competition solves these problems. We
managed to take a look into the following projects:
PlayerStage — operating system for robots
This software package offers a variety of abstract interfaces to the
most common platforms of mobile robots. It is built upon three
principles: (a) interface, (b) device, (c) driver. In
general it implements a layer that performs a basic post processing of the input data into a
hardware-independent form. For the sake of simpleness, we can
compare this system to an operating system.
Several application libraries in different languages can be chosen, but
which do not pose fundamental restrictions on the client code's
structure. We did not manage to find any extensive example of usage,
although the project is formed around a number of (mostly American)
scientists who allegedly use that system for the implementation of their
algorithms.
CARMEN — collection of modules for all aspects of steering and navigation
Carnegie Mellon Robot Navigation Toolkit consists of several modules which
communicate via
IPC. IPC
is a package that enables the communication between modules by TCP/IP. The
communication works with sending messages and enables a publish/subscribe
architecture (the module generates data to request another module without
knowing it) and also a query/response architecture (the module generates a query
and awaits a response). The major usage drawback is probably the need to
construct each module as an independent process. That causes unnecessary
inconvenience while testing and debugging. As a free reward, we gain
an collection of single modules which can run on different computers (with
transparent communication).
Tekkotsu — specially for AIBO
Tekkotsu is strongly tied to the robot dog AIBO. Everything needed for its
operation is included and it is intended for extending pre-defined classes for
specific tasks of algorithms
(tutorial,
overview). Adding
a new functionality is done by reimplementing pre-defined virtual functions in
combination with (similarly to IPC) notifications in the form of messages (by
registering some handler functions).
Robocode — simulation of a tank battle in Java
Robocode differs from the program packages mentioned above especially by
its focus on simulated robots. The basic API which can be used for
programming the robot has a synchronous character. More can be seen in
an example:
// A basic robot that moves around in a square public void run() { turnLeft(getHeading()); while(true) { ahead(100); turnRight(90); } }
Apart from this synchronous API, Robocode offers a so-called
AdvancedRobot API which is more low level and lets you peek under
the hood of the whole system. The API function
ahead(int)
can be
implemented using the low-level API like the following:public void ahead(int dist) { setAhead(dist); while(dist– > 0) execute(); setStop(); execute(); } }
The method
execute()
works here like our
sendReceive(commandStruct, stateStruct)
and
setAhead(dist)
sets the power on the motors. The synchronous API
is still our original FSM, but somehow turned inside out.Lessons learned
What have we learned from our colleagues? It is very important to
preprocess the incoming and outgoing data into a hardware-independent
form. Although it does not have to be obvious on first sight, altogether
it is a demanding task (the Player solves essentially only this problem and also
two thirds of Ester's is just this). Consequently, it
simplifies the application code and also the interface for a simulation.
Another common denominator of all projects is the pursuit for the code's
modularization with biggest possible isolation of the modules, with exactly
defined interfaces usually implemented by some form of message passing.
The last (but not the least), the synchronous API of Robocode should be
mentioned because it impressed us by its simplicity and directness. Its design
was simplified presumably by the fact that it is targeted from the beginning at
simulated robots and that it does not have to deal with such "details" as how
motors work or how the encoders tick. Consequently it helps in simulating the
robot and also makes the control easier.
Attempt to improve no. 2 — Ester
By using the nested FSM model we managed to achieve a certain degree of
modularization of our code so that it became cleaner and several people could
work on it together. Alas, we did not manage to solve the fundamental problem
introduced by the FSM-implemented using
switch(state)
construction
— the fact that we can pass between arbitrary states and the
visualization and understanding of these transitions is not easy. Also, it is
still the programmers responsibility to properly initialize the different nested
FSMs. That in turn leads to unnecessary difficulties while extending the code,
but also while only doing maintenance tasks.On thorough examination of the switch-based FSM, we discover that we are
actually using very peculiar way of implementing something, that we programmers
deal with every day (as long as you do not program only in Prolog ).
An example will help again. How would the implementation of a robot driving in
a square looked like using the switch-based implementation?
state = INIT; while (true) { sendReceive(commandStruct, stateStruct); switch(state) { case INIT: if (getAbsHeading() == 0) { state = STRAIGHT; dist = 100; break; } setLeft(); break; case STRAIGHT: if (dist == 0) { state = TURN; angle = 90; break; } dist-@-; setAhead(); break; case TURN: if (angle == 0) { state = STRAIGHT; dist = 100; break; } angle-@-; setLeft(); break; } }
For the sake of simplicity we defined the distance and the angle in the
number of steps. The programmed activity can be described like this: First, it
turns so its absolute orientation is 0 and then it drives 100 ticks straight and
90 ticks to the left. Already in such a simple example we can see something,
that can be fairly easily described, requires a lot of code that in addition to
that does not really resemble the original description. On the other hand the
algorithm coded using the synchronous Robocode API:
// A basic robot that moves around in a square public void run() { turnLeft(getHeading()); while(true) { ahead(100); turnRight(90); } }
The interesting thing is that this code is consequently an FSM as well, only that the compiler
helps us extensively so that the code stays readable. Instead of the
state
variable we have the IP (instruction pointer) here which
points to the currently executed instruction and the stack for function calls
(that in turn implements the nested FSM and more)The fundamental advantage of this solution though is that the majority of the
things the programmer had to care about is now handled by the compiler. For
instance it is automatically guaranteed that we do not enter the initialization
sequence (expressed by the state
INIT
) again while the robot is
moving and that at repeated calls to run()
it is properly executed
(without any work required on the caller's side).Also the already mentioned simplicity and clearness of the code is worth to
emphasize again. It is even more impressive in the situation when it is
necessary to implement a complicated algorithm which contains several variously
nested cycle with more complicated functions of the type
ahead()
or
turnRight()
.The whole control algorithm for Ester at the competition
Eurobot 2004 can be described with such a
simple code:
virtual int main() { mapPalms(); while (true) { try { BallDetect bd; EnemyDetect ed; while (true) { searchBall(); randomMove(); } } catch (BallDetect::done) { shootBall(); } catch (EnemyDetect::done) { randomMove(); } catch (std::runtime_error){ randomMove(); } } return 0; }
Ester first looks for the position of the palms. She does that simultaneously
while driving across the playing field along its longer side, beginning from the
start position. She monitors the distance on the right side returned by Sharp
infrared distance meter. Afterwards it creates two detectors that register to
the system so that they are called in every step and Ester can start searching
for the balls. As long as she does not find a ball in her vicinity, she picks
a new random target on the playing field and continues searching. These two
activities alternate until no ball other ball can be found or Ester hits
a rival. The found ball will be shot to the goal while she is evading the
opponent robot. In both situations she continues searching for another ball
afterwards.
Conclusion
Ester's control system would be unbearably complicated if
being implemented by the switch-based FSMs (in the order of several hundred
states in several tens of FSMs). Thanks to the usage of classical
techniques that every programmer is used to (constructions like
if,
else, while
or function calls) for controlling the robot with a
synchronous API that we put in this year, it brought us an impressive
increase in working efficiency, which therefore enabled us consequently
to work out a reliable control system which, in combination with more
reliable hardware, won Ester the 4th rank.Of course, a current system has its rough edges. As described up to now,
a parallel steering of the robot's movement and the control of more
periphery would be impossible (in the case of Ester a roller for the
balls' manipulation). We solved that by using multiple threads of execution
which lead to enormous complications when accessing commonly used
resources and synchronization in general.
For the implementation of the outlined API, you have to prepare a
lot of support code (especially for the maintenance of multiple threads).
Though, in our opinion, such an investment into overcoming the
initial difficulties will be highly rewarded later.
If you have questions or remakes – contact
us. We will gladly answer.