Page 195 - The Unofficial Guide to Lego Mindstorms Robots

184

int motorCommand;

task arbitrate() {
while(true) {
if (cruiseCommand != COMMAND_NONE) motorCommand = cruiseCommand;
if (tagCommand != COMMAND_NON E) motorCommand = tagCommand;
if (avoidCo mmand != COMMAND_NONE) motorCommand = avoidCommand;
if (taggedCommand != COMMAND_NONE) motorCommand = taggedCommand;
motorControl();
}
}

Note that the order is important. The commands in the end of the list w ill overwrite the value of motorCommand and are
thus higher-level be haviors. For example, if both the cruise and tagged behaviors are attempting to control the robot, the
tagged behavior takes precedence by subs uming the lower-level behavior, cruise.

In this implementation, if no behavior asserts control, then motorCommand will be unchanged, and the robot will just
continue doing whatever it d id before. This isn't an issu e, since cruiseCommand is always COMMAND_FORWARD.
However, in a different program, you might want to set motorCommand to a de fault action at the beginning of the while
loop in arbitrate().

Where the rubber meets the road

The arbitrate task hands off the actual dirty work of co ntrolling the motors to a subroutine called motorControl().
All motorControl() has to do is examine the value of mot orCommand and set the motors accordingly. Here it is:

sub mo torControl() {
if (motorCommand == COMMAND_FORWARD)
OnFwd(OUT_A + OUT_C);
else if (mot orC ommand == COMMAND_REVERSE)
OnRev(OUT_A + OUT_C);
else if (motorCommand == COMMAND_LEFT) {
OnRev(OUT_A);
OnFwd(OUT_C);
}
else if (motorCommand == COMMAND_RIGHT) {
OnFwd(OUT_A);
OnRev(OUT_C);
}
else if (motorCommand == COMMAND_STOP)
Off(OUT_A + OUT_C);
else if (motorCommand == COMMAND_FLOAT)
Float(OUT_A + OUT_C);
}

The rel a tionship between arbitrate and motorControl is important. At first glance, you might think it makes sense to
implement motorControl as a separate