LEGO MINDSTORMS Robots

}
task messageWatcher() {
while (true) {
message = Message();
if (message == FORWARD_MESSAGE) {
OnFwd(OUT_C);
ClearMessage();
}
else if (message == SPIN_MESSAGE) {
OnRev(OUT_C);
ClearMessage();
}
else if (message == STOP_MESSAGE) {
Off(OUT_C);
ClearMessage();
}
else if (message == GRAB_MESSAGE) {
start grab;
ClearMessage();
}
else if (message == RELEASE_MESSAGE) {
start release;
ClearMessage();
}
else if (message == HEARTBEAT_MESSAGE) {
missedBeats = missedBeats - 1;
ClearMessage();
}
}
}
task grab() {
until (armLock == 0);
Page 156
stop release;
OnFwd(OUT_A);
until (SENSOR_3 == 100);
armLock = 1;
OnRev(OUT_A);
until (SENSOR_3 != 100);
armLock = 0;
Off(OUT_A);
}
task release() {
until (armLock == 0);
stop grab;
OnRev(OUT_A);
until (SENSOR_3 == 100);
armLock = 1;
OnFwd(OUT_A);
until (SENSOR_3 != 100);
armLock = 0;
Off(OUT_A);