Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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"); }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement