Advertisement
Ohmnivore

Bot in a Box object avoidance

Mar 8th, 2013
272
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.40 KB | None | 0 0
  1. #define PWMa 3
  2. #define PWMb 5
  3. #define AIN1 9
  4. #define AIN2 8
  5. #define BIN1 7
  6. #define BIN2 6
  7. #define Buzzer 2
  8. #define pingPin 4
  9. #define PhotoLeft 3
  10. #define PhotoRight 2
  11. #define Battery 1
  12. #define LED 11
  13. #define BumperLeft 10
  14. #define BumperRight 12
  15. long ping;
  16. boolean right;
  17. boolean left;
  18. int left_state;
  19. int right_state;
  20.  
  21. void setup()
  22. {
  23.   analogWrite(PWMa, 255);
  24.   analogWrite(PWMb, 255);
  25.   pinMode(AIN1, OUTPUT);
  26.   pinMode(AIN2, OUTPUT);
  27.   pinMode(BIN1, OUTPUT);
  28.   pinMode(BIN2, OUTPUT);
  29.   pinMode(LED, OUTPUT);
  30.   pinMode(Buzzer, OUTPUT);
  31.   pinMode(BumperLeft, INPUT);
  32.   pinMode(BumperRight, INPUT);
  33.   right = false;
  34.   left = false;
  35.   int left_state = LOW;
  36.   int right_state = LOW;
  37. }
  38. void loop()
  39. {
  40.   left_state = digitalRead(BumperLeft);
  41.   right_state = digitalRead(BumperRight);
  42.   ping = Distance();
  43.   digitalWrite(LED, HIGH);
  44.   Forward();
  45.  
  46.   if (ping < 19)
  47.   {
  48.     digitalWrite(LED, LOW);
  49.     digitalWrite(Buzzer, HIGH);
  50.     Right();
  51.     delay(500);
  52.     digitalWrite(Buzzer, LOW);
  53.   }
  54.  
  55.   if (left_state == HIGH)
  56.   {
  57.     digitalWrite(LED, LOW);
  58.     digitalWrite(Buzzer, HIGH);
  59.     Backward();
  60.     delay(500);
  61.     digitalWrite(Buzzer, LOW);
  62.     Right();
  63.     delay(500);
  64.   }
  65.  
  66.    if (right_state == HIGH)
  67.   {
  68.     digitalWrite(LED, LOW);
  69.     digitalWrite(Buzzer, HIGH);
  70.     Backward();
  71.     delay(500);
  72.     digitalWrite(Buzzer, LOW);
  73.     Left();
  74.     delay(500);
  75.   }
  76. }
  77.  
  78. long Distance()
  79. {
  80.   long duration, cm;
  81.  
  82.   pinMode(pingPin, OUTPUT);
  83.   digitalWrite(pingPin, LOW);
  84.   delayMicroseconds(2);
  85.   digitalWrite(pingPin, HIGH);
  86.   delayMicroseconds(5);
  87.   digitalWrite(pingPin, LOW);
  88.  
  89.   pinMode(pingPin, INPUT);
  90.   duration = pulseIn(pingPin, HIGH);
  91.  
  92.   cm = microsecondsToCentimeters(duration);
  93.  
  94.   delay(100);
  95.   return cm;
  96. }
  97.  
  98. long microsecondsToCentimeters(long microseconds)
  99. {
  100.   return microseconds / 29 / 2;
  101. }
  102.  
  103. void Forward()
  104. {
  105.   digitalWrite(AIN1, HIGH);
  106.   digitalWrite(AIN2, LOW);
  107.   digitalWrite(BIN1, HIGH);
  108.   digitalWrite(BIN2, LOW);
  109. }
  110.  
  111. void Backward()
  112. {
  113.   digitalWrite(AIN1, LOW);
  114.   digitalWrite(AIN2, HIGH);
  115.   digitalWrite(BIN1, LOW);
  116.   digitalWrite(BIN2, HIGH);
  117. }
  118.  
  119. void Right()
  120. {
  121.   digitalWrite(AIN1, HIGH);
  122.   digitalWrite(AIN2, LOW);
  123.   digitalWrite(BIN1, LOW);
  124.   digitalWrite(BIN2, HIGH);
  125. }
  126.  
  127. void Left()
  128. {
  129.   digitalWrite(AIN1, LOW);
  130.   digitalWrite(AIN2, HIGH);
  131.   digitalWrite(BIN1, HIGH);
  132.   digitalWrite(BIN2, LOW);
  133. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement