Page 127 -

243_MasterPieces_02b.qxd 4/18/03 6:59 PM Page 99

The Learning Brick Sorter • Masterpiece 2 99

if (dest == pos) return;

// set direction according to the difference between
// the current position and the required destination
if (dest > pos)
{
dir = 1;
OnRev(OUT_A);
}
else
{
dir = -1;
OnFwd(OUT_A);
}

// move until destination is reached
while (dest != pos)
{
while (SENSOR_3 == 1); // wait for sensor open
while (SENSOR_3 == 0); // wait for sensor closed again
pos += dir;
}
Off(OUT_A);
}

Another simple routine that configures the sensors and initializes the global variables
and the knowledge base:

void SystemSetup()
{
int i;
SetSensor(SENSOR_1, SENSOR_LIGHT);
SetSensor(SENSOR_2, SENSOR_TOUCH);
SetSensor(SENSOR_3, SENSOR_TOUCH);
pos = -1; // initial position is pick-up station
top_range = -1; // means no range is yet defined

// clear the knowledge base
for (i=0;i<BINS*RANGES;i++) kb[i]=KB_UNKNOWN;
}