Page 198 - The Unofficial Guide to Lego Mindstorms Robots

187

SendMessage(MESSAGE_ACKNOWLEDGE);
PlaySound(4);
Wait(800);
ClearMessage();
taggedCommand = COMMAND_NONE;
}
}
}

int mot orCommand;

task arbitra te() {
while(true ) {
if (cru iseCommand != 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);
els e 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;
}