void init_epuck()
{
e_start_agendas_processing();
e_init_port();
e_init_motors();
e_init_uart1();
e_init_uart2();
e_init_ad_scan();
// init_IR();
}
void slight_left()
{
long j=0;
for (j=0; j<1000;j++)
{
e_set_speed_right(900);
e_set_speed_left(-200);
}
}
void slight_right()
{
long j=0;
for (j=0; j<1000;j++)
{
e_set_speed_right(-200);
e_set_speed_left(900);
}
}
void move(long x, int speed)
{
long i=0;
if (x==0) //no time defined keeps moving constantly
{
e_set_speed_right(speed);
e_set_speed_left(speed);
}
else
{
for (i=0; i<x; i++)
{
e_set_speed_right(speed);
e_set_speed_left(speed);
}
}
}
void tracking()
{
int a,b,c,d,e,f,g,h = 0;
int left_light;
int right_light;
a=e_get_ambient_light(0);
b=e_get_ambient_light(1);
c=e_get_ambient_light(2);
d=e_get_ambient_light(3);
e=e_get_ambient_light(4);
f=e_get_ambient_light(5);
g=e_get_ambient_light(6);
h=e_get_ambient_light(7);
left_light = (e+f+g+h)/4; //average value of light to the left
right_light = (a+b+c+d)/4; //average value of light to the right
btcomSendInt(left_light);
btcomSendChar('-');
btcomSendInt(right_light);
btcomSendChar(' ');
if(right_light<left_light) //brighter source on the right
slight_right();
else if (left_light<right_light) //brighter source on the left
slight_left();
else
move(0, 200);
}
int get_selector()
{
return SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
}
int main (void)
{
init_epuck();
int selector;
int value;
long int j;
selector=get_selector();
while(1)
{
if(selector == 0)
{
e_set_led(0,1);
e_set_led(1,1);
e_set_led(2,1);
e_set_led(3,1);
e_set_led(4,1);
e_set_led(5,1);
e_set_led(6,1);
e_set_led(7,1);
}
else if(selector == 1)
{
value = (e_get_prox(0)+e_get_prox(7))/2; //proximity of obstacle in front
if(value > 200)
{
e_blink_led();
move(0,0);
}
else
{
tracking();
e_led_clear();
}
for(j=0; j<100000; j++) { asm("nop"); }
}
else if(selector == 2)
{
value = e_get_ambient_light(0);
btcomSendInt(value);
btcomSendChar(' ');
for(j=0; j<100000; j++) { asm("nop"); }
}
}
}