Advertisement
Guest User

Untitled

a guest
May 24th, 2012
97
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.24 KB | None | 0 0
  1. void init_epuck()
  2. {
  3. e_start_agendas_processing();
  4. e_init_port();
  5. e_init_motors();
  6. e_init_uart1();
  7. e_init_uart2();
  8. e_init_ad_scan();
  9. // init_IR();
  10. }
  11.  
  12. void slight_left()
  13. {
  14. long j=0;
  15. for (j=0; j<1000;j++)
  16. {
  17. e_set_speed_right(900);
  18. e_set_speed_left(-200);
  19. }
  20. }
  21.  
  22. void slight_right()
  23. {
  24. long j=0;
  25. for (j=0; j<1000;j++)
  26. {
  27. e_set_speed_right(-200);
  28. e_set_speed_left(900);
  29. }
  30. }
  31.  
  32.  
  33. void move(long x, int speed)
  34. {
  35. long i=0;
  36. if (x==0) //no time defined keeps moving constantly
  37. {
  38. e_set_speed_right(speed);
  39. e_set_speed_left(speed);
  40. }
  41. else
  42. {
  43. for (i=0; i<x; i++)
  44. {
  45. e_set_speed_right(speed);
  46. e_set_speed_left(speed);
  47. }
  48. }
  49. }
  50.  
  51. void tracking()
  52. {
  53. int a,b,c,d,e,f,g,h = 0;
  54. int left_light;
  55. int right_light;
  56. a=e_get_ambient_light(0);
  57. b=e_get_ambient_light(1);
  58. c=e_get_ambient_light(2);
  59. d=e_get_ambient_light(3);
  60. e=e_get_ambient_light(4);
  61. f=e_get_ambient_light(5);
  62. g=e_get_ambient_light(6);
  63. h=e_get_ambient_light(7);
  64. left_light = (e+f+g+h)/4; //average value of light to the left
  65. right_light = (a+b+c+d)/4; //average value of light to the right
  66. btcomSendInt(left_light);
  67. btcomSendChar('-');
  68. btcomSendInt(right_light);
  69. btcomSendChar(' ');
  70. if(right_light<left_light) //brighter source on the right
  71. slight_right();
  72. else if (left_light<right_light) //brighter source on the left
  73. slight_left();
  74. else
  75. move(0, 200);
  76. }
  77.  
  78.  
  79. int get_selector()
  80. {
  81. return SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;
  82. }
  83.  
  84. int main (void)
  85. {
  86. init_epuck();
  87. int selector;
  88. int value;
  89. long int j;
  90.  
  91. selector=get_selector();
  92. while(1)
  93. {
  94. if(selector == 0)
  95. {
  96. e_set_led(0,1);
  97. e_set_led(1,1);
  98. e_set_led(2,1);
  99. e_set_led(3,1);
  100. e_set_led(4,1);
  101. e_set_led(5,1);
  102. e_set_led(6,1);
  103. e_set_led(7,1);
  104. }
  105.  
  106. else if(selector == 1)
  107. {
  108. value = (e_get_prox(0)+e_get_prox(7))/2; //proximity of obstacle in front
  109. if(value > 200)
  110. {
  111. e_blink_led();
  112. move(0,0);
  113. }
  114. else
  115. {
  116. tracking();
  117. e_led_clear();
  118. }
  119. for(j=0; j<100000; j++) { asm("nop"); }
  120. }
  121.  
  122.  
  123. else if(selector == 2)
  124. {
  125. value = e_get_ambient_light(0);
  126. btcomSendInt(value);
  127. btcomSendChar(' ');
  128. for(j=0; j<100000; j++) { asm("nop"); }
  129. }
  130. }
  131. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement