LEGO MINDSTORMS Robots - Page 186

Page 206
#define COMMAND_REVERSE 2
#define COMMAND_LEFT 3
#define COMMAND_RIGHT 4
#define COMMAND_STOP 5
int avoid_command;
int avoid(int argc, char ∗∗argv) {
avoid_command = COMMAND_NONE;
while(1) {
if (SENSOR_1 <0xf000) {
avoid_command = COMMAND_REVERSE;
msleep(BACK_TIME);
avoid_command = COMMAND_RIGHT;
msleep(TURN_TIME);
avoid_command = COMMAND_NONE;
}
if (SENSOR_3 < 0xf000) {
avoid_command = COMMAND_REVERSE;
msleep(BACK_TIME)
avoid_command = COMMAND_LEFT;
msleep(TURN_TIME);
avoid_command = COMMAND_NONE;
}
}
return 0;
}
#define RAW_DARK 0x7c00
#define RAW_LIGHT 0x6000
int process_light(int raw) {
long big = 100 ∗ (long)raw - RAW_LIGHT);
long percent = big / (RAW_DARK - RAW_LIGHT);
return 100 - (int)percent;
}
int seek_command;
#define FUDGE 5
int wait_for_better(int baseline, unsigned long milliseconds) {
int current;
int saved_time = sys_time;
do {
msleep(50);
current = process_light(SENSOR_2);
lcd_int(current ∗ 100 + baseline);
lcd_refresh();
} while (sys_time < (saved_time + milliseconds)
&& current < (baseline + FUDGE));
if (current >= (baseline + FUDGE)) return current;
return -1; // Timed out.
}