Advertisement
Bosssu

Project 7 incomplete

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