Advertisement
Erfinator

Rover Code, WIP (work in progress)

Dec 20th, 2014
150
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.53 KB | None | 0 0
  1. #include <Servo.h>
  2. const int pingPin = 7;
  3. const int minsafedistance = 5;
  4. int rightDistance, leftDistance;
  5. int leftMotor2 = 3;
  6. int leftMotor7 = 4;
  7. int rightMotor10 = 5;
  8. int rightMotor15 = 6;
  9. long duration, distance;
  10. Servo servo;
  11.  
  12.  
  13. // SETUP
  14.  
  15. void setup()
  16. {
  17. servo.attach(8);
  18. pinMode(leftMotor2, OUTPUT);
  19. pinMode(leftMotor7, OUTPUT);
  20. pinMode(rightMotor10, OUTPUT);
  21. pinMode(rightMotor15, OUTPUT);
  22. }
  23.  
  24.  
  25. // LOOP
  26.  
  27. void loop()
  28. {
  29. lookForward();
  30. int fwdDistance = ping();
  31. if(fwdDistance>minsafedistance)
  32. {
  33. GoForward();
  34. }
  35. else
  36. {
  37. // this section is incomplete
  38. lookRight();
  39. rightDistance = ping();
  40. delay(100);
  41. lookLeft();
  42. leftDistance = ping();
  43. delay(100);
  44. }
  45. }
  46.  
  47.  
  48. // FUNCTIONS
  49.  
  50. long ping()
  51. {
  52. pinMode(pingPin, OUTPUT);
  53. digitalWrite(pingPin, LOW);
  54. delayMicroseconds(2);
  55. digitalWrite(pingPin, HIGH);
  56. delayMicroseconds(5);
  57. digitalWrite(pingPin, LOW);
  58.  
  59. pinMode(pingPin, INPUT);
  60. duration = pulseIn(pingPin, HIGH);
  61. distance = (duration/2) /29.3866996;
  62. return distance;
  63. }
  64.  
  65. //---------------------------------------------------------
  66.  
  67. void GoForward()
  68. {
  69. digitalWrite(leftMotor7, LOW);
  70. digitalWrite(leftMotor2, HIGH);
  71. digitalWrite(rightMotor15, LOW);
  72. digitalWrite(rightMotor10, HIGH);
  73. }
  74.  
  75. //---------------------------------------------------------
  76.  
  77. void GoBackward()
  78. {
  79. digitalWrite(leftMotor2, LOW);
  80. digitalWrite(leftMotor7, HIGH);
  81. digitalWrite(rightMotor10, LOW);
  82. digitalWrite(rightMotor15, HIGH);
  83. }
  84.  
  85. //---------------------------------------------------------
  86.  
  87. void GoLeft()
  88. {
  89. digitalWrite(leftMotor2, LOW);
  90. digitalWrite(leftMotor7, HIGH);
  91. digitalWrite(rightMotor15, LOW);
  92. digitalWrite(rightMotor10, HIGH);
  93. }
  94.  
  95. //---------------------------------------------------------
  96.  
  97. void GoRight()
  98. {
  99. digitalWrite(leftMotor7, LOW);
  100. digitalWrite(leftMotor2, HIGH);
  101. digitalWrite(rightMotor10, LOW);
  102. digitalWrite(rightMotor15, HIGH);
  103. }
  104.  
  105. //---------------------------------------------------------
  106.  
  107. void Stop()
  108. {
  109. digitalWrite(leftMotor7, LOW);
  110. digitalWrite(leftMotor2, LOW);
  111. digitalWrite(rightMotor10, LOW);
  112. digitalWrite(rightMotor15, LOW);
  113. }
  114.  
  115. //---------------------------------------------------------
  116.  
  117. void lookRight()
  118. {
  119. servo.write(180);
  120. delay(2000);
  121. }
  122.  
  123. //---------------------------------------------------------
  124.  
  125. void lookLeft()
  126. {
  127. servo.write(0);
  128. delay(2000);
  129. }
  130.  
  131. //---------------------------------------------------------
  132.  
  133. void lookForward()
  134. {
  135. servo.write(90);
  136. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement