Advertisement
Bosssu

Project 9 incomplete

Sep 9th, 2014
374
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.89 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 2
  9. #define RBM 3
  10. void setup()
  11. {
  12. sw1_press();
  13. }
  14. void loop()
  15. {
  16. go_forward(100,500);
  17. turn_right(100,300);
  18. go_forward(100,250);
  19. timed_follow( 3000 );
  20. go_backward(100,050);
  21. turn_right(100,780);
  22. go_backward(100,800);
  23. turn_right(100,780);
  24. turn_left(100,300);
  25. timed_follow( 15000 );
  26. }
  27.  
  28. void timed_follow( long int ms )
  29. {
  30. long int start = millis();
  31. while( millis() - start<ms )
  32. {
  33. int left = analogRead( LEFT_SENSOR );
  34. int right = analogRead( RIGHT_SENSOR );
  35. if( left < BLACK )
  36. {
  37. spin_left(100);
  38. }
  39. else if( right < BLACK )
  40. {
  41. spin_right(100);
  42. }
  43. else
  44. {
  45. motor_forward(30);
  46. }
  47. }
  48. }
  49. void go_forward(int power,int ms)
  50. {
  51. motor(LFM,power);
  52. motor(LBM,power);
  53. motor(RFM,power);
  54. motor(RBM,power);
  55. delay(ms);
  56. motor_stop(ALL);
  57. }
  58. void go_backward(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