Advertisement
Bosssu

Project 7 incomplete2

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