Page 219 - The Unofficial Guide to Lego Mindstorms Robots

208

int motor_command;

void motor_control() {
motor_a_speed(MAX_SPEED);
motor_c_speed(MAX_SPEED);

switch (motor_command) {
case COMMAND_FORWARD:
motor_a_dir(fwd);
motor_c_dir(fwd);
break;
case COMMAND_REVERSE:
motor_a_dir(rev);
motor_c_dir(rev);
break;
case COMMAND_LEFT:
motor_a_dir(rev);
motor_c_dir(fwd);
break;
case COMMAND_RIGHT:
motor_a_dir(fwd);
motor_c_dir(rev);
break;
case COMMAND_STOP:
motor_a_dir(brake);
motor_c_dir(brake);
break;
default:
break;
}
}

int arbitrate(int argc, char ∗∗argv) {
while(1) {
if (avoid_command != COMMAND_NONE) cputc('a', 0);
else if (seek_command != COMMAND_NONE) cputc('s', 0);
else if (cruise_command != COMMAND_NONE) cputc('c', 0);
else cputc(' ', 0);
lcd_refresh();

if (cruise_command != COMMAND_NONE) motor_command = cruise_command;
if (seek_command != COMMAND_NONE) motor_command = seek_command;
if (avoid_command != COMMAND_NONE) motor_command = avoid_command;
motor_control();
}
}

int stop_task(int argc, char ∗∗argv) {
int i;

msleep(200);
while (!PRESSED(button_state(), BUTTON_RUN));

for (i = 0; i < task_index; i++)