Page 166 - The Unofficial Guide to Lego Mindstorms Robots

155

#define FORWARD_MESSAGE 16
#define SPIN_MESSAGE 17
#define STOP_MESSAGE 18
#define GRAB_MESSAGE 19
#define RELEASE_MESSAGE 20
#define HEARTBEAT_MESSAGE 21

#define HEARTBEAT_TIME 20

int message;
int armLock;
int missedBeats;

task main() {
SetSensor (SENSOR_3, SENSOR_LIGHT);
armLock = 0;
missedBeats = 0;
start messageWatcher;
start heartbeatWatcher;
}

task messageW atcher() {
while (true) {
message = Message();
if (messag e == FORWARD_MESSAGE) {
OnFwd(OUT_C);
ClearMessage ();
}
else if (message == SPIN_MESSAGE) {
OnRev(OUT_C);
ClearMessage();
}
else if (message == STOP_MESSAGE) {
Off(OUT_C);
ClearMe ssag e();
}
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);