Advertisement
Bosssu

Race #4(Version 2)

Nov 18th, 2014
280
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.69 KB | None | 0 0
  1. #include <atx.h>
  2. #define BLACK 150
  3. #define LEFT_SENSOR 2
  4. #define RIGHT_SENSOR 3
  5. int state = 1;
  6. int ms;
  7.  
  8. void setup()
  9. {
  10. lcd("pressSW1");
  11. sw1_press();
  12. beep();
  13. }
  14.  
  15. void loop()
  16. {
  17. int left = analogRead(LEFT_SENSOR);
  18. int right = analogRead(RIGHT_SENSOR);
  19. if(left < 200 && right < 200)
  20. {
  21. beep();
  22. switch(state)
  23. {
  24. case 1:
  25. {
  26. go_backward(50);
  27. delay(250);
  28. spin_left(100);
  29. delay(380);
  30.  
  31. break;
  32. }
  33. case 2:
  34. {
  35. go_backward(50);
  36. delay(250);
  37. spin_right(100);
  38. delay(400);
  39.  
  40. break;
  41. }
  42. case 3:
  43. {
  44. go_backward(50);
  45. delay(250);
  46. spin_right(100);
  47. delay(400);
  48.  
  49. break;
  50. }
  51. case 4:
  52. {
  53. go_backward(50);
  54. delay(250);
  55. spin_right(100);
  56. delay(400);
  57.  
  58. break;
  59. }
  60.  
  61. default:
  62. {
  63. motor_stop(ALL);
  64. sw1_press();
  65. }
  66. }
  67. state = state + 1;
  68. }
  69. else if(left < 200)
  70. {
  71. spin_left(20);
  72. }
  73. else if(right < 200)
  74. {
  75. spin_right(20);
  76. }
  77. else
  78. {
  79. go_forward(100);
  80. }
  81. }
  82.  
  83. void spin_left(int t_power)
  84. {
  85. motor(0,t_power);
  86. motor(1,t_power);
  87. motor(2,-t_power);
  88. motor(3,-t_power);
  89. }
  90.  
  91. void spin_right(int t_power)
  92. {
  93. motor(0,-t_power);
  94. motor(1,-t_power);
  95. motor(2,t_power);
  96. motor(3,t_power);
  97. }
  98.  
  99. void go_forward(int f_power)
  100. {
  101. motor(0,f_power);
  102. motor(1,f_power);
  103. motor(2,f_power);
  104. motor(3,f_power);
  105. }
  106.  
  107. void go_backward(int b_power)
  108. {
  109. motor(0,-b_power);
  110. motor(1,-b_power);
  111. motor(2,-b_power);
  112. motor(3,-b_power);
  113. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement