LEGO MINDSTORMS Robots

Page 187
SendMessage(MESSAGE_ACKNOWLEDGE);
PlaySound(4);
Wait(800);
ClearMessage();
taggedCommand = COMMAND_NONE;
}
}
}
int motorCommand;
task arbitrate() {
while(true) {
if (cruiseCommand != COMMAND_NONE) motorCommand = cruiseCommand;
if (tagCommand != COMMAND_NONE) motorCommand = tagCommand;
if (avoidCommand != COMMAND_NONE) motorCommand = avoidCommand;
if (taggedCommand != COMMAND_NONE) motorCommand = taggedCommand;
motorControl();
}
}
sub motorControl() {
if (motorCommand == COMMAND_FORWARD)
OnFwd(OUT_A + OUT_C);
else if (motorCommand == 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);
}
task main() {
initialize();
cruiseCommand = COMMAND_NONE;
tagCommand = COMMAND_NONE;
avoidCommand = COMMAND_NONE;
taggedCommand = COMMAND_NONE;
start cruise;
start tag;
start avoid;
start tagged;
start arbitrate;
}