#include <dmotor.h>
#include <dsensor.h>
#include <unistd.h>
#define LIGHTSENS SENSOR_2
#define DARK_THRESH 0x21 // black line value
#define BRIGHT_THRESH 0x2b // white value
static wakeup_t dark_found(wakeup_t data) { // black line is found
return LIGHT(LIGHTSENS)<(unsigned short)data;
}
static wakeup_t bright_found(wakeup_t data) { // white is found
return LIGHT(LIGHTSENS)>(unsigned short)data;
}
int main(int argc, char *argv[]) {
ds_active(&LIGHTSENS);
motor_a_speed(MAX_SPEED/3);
motor_c_speed(MAX_SPEED/3);
while(1){
wait_event(bright_found,BRIGHT_THRESH); // white is found
motor_a_dir(rev);
motor_c_dir(fwd);
cputw(LIGHT(LIGHTSENS));
wait_event(dark_found,DARK_THRESH); // black line is found
motor_a_dir(fwd);
motor_c_dir(fwd);
cputw(LIGHT(LIGHTSENS));
}
return 0;
}
|