Page 216 - The Unofficial Guide to Lego Mindstorms Robots

205

Figure 10-2.
Hank, retrofitted with a light sensor

• avoid() is the highest-level behavior. It is triggered by the bumpe rs and does the standard back-up-and-turn.

Convincing Hank to seek light is surprisingly hard. You can, of course, implement y our own algorithm in
seek_enlightenment(). It's a neat feature of subsumption architecture that yo u can concentrate solely on light-seeking
behavior in seek_enlightenment(). The obs tacle avoidance behavior is alread y programmed in a different behavior and
will subsume the light-seeking behavior as necessary.

Without further ado, here's the entire program, LightSeeker.c:

#include "conio.h"
#include "direct-button.h"
#include "direct-motor.h"
#include "direct-sensor.h"
#include "unistd.h"
#include "sys/tm.h"

#define MAX_TASKS 32

pid_t pid[MAX_TASKS];
int task_index;

#d efine BACK_TIME 500
#define TURN_TIME 800

// Motor commands.
#define COMMAND_NONE -1
#define COMMAND_FORWARD 1