SHARE
TWEET

Untitled

a guest May 26th, 2019 65 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /* Pin Constants (editable) */
  2.  
  3. //Pins for the ultrasonic sensor
  4. const int pingPin = 12;
  5. const int echoPin = 11;
  6.  
  7. //Wheels
  8. const int leftWheelEnable = 9;
  9. const int leftWheelForward = 10;
  10. const int leftWheelBackward = 8;
  11. const int rightWheelEnable = 7;
  12. const int rightWheelForward = 5;
  13. const int rightWheelBackward = 6;
  14.  
  15. //White line detector
  16. const int whiteSensor0 = A0;
  17. const int whiteSensor1 = A1;
  18. const int whiteSensor2 = A2;
  19.  
  20. //Pins for the binary distance sensors on the side
  21. const int leftFrontSensor = 4;
  22. const int leftBackSensor = 2;
  23. const int rightFrontSensor = A3;
  24. const int rightBackSensor = 3;
  25.  
  26. /* Structures */
  27.  
  28. enum State {
  29.   SEARCHING,
  30.   CHASING,
  31.   ON_BORDER,
  32.   AVOIDING
  33. };
  34.  
  35. enum Direction {
  36.   FORWARD,
  37.   BACKWARD
  38. };
  39.  
  40. /* Logic Constants */
  41.  
  42. //When sensor reads over this value, it interprets it as a white line (border)
  43. const int whiteLineThreshold0 = 700;
  44. const int whiteLineThreshold1 = 700;
  45. const int whiteLineThreshold2 = 700;
  46. //When ultrasonic sensor detects object closer than this value then it will interpret it as an enemy;  
  47. const int enemySightDistance = 10;
  48.  
  49. //Globals
  50. State state = SEARCHING;
  51. Direction direction = FORWARD;
  52. unsigned long time;
  53.  
  54. void setup() {
  55.   // put your setup code here, to run once:
  56.   Serial.begin(9600);
  57.  
  58.   pinMode(leftWheelForward, OUTPUT);
  59.   pinMode(leftWheelBackward, OUTPUT);
  60.   pinMode(rightWheelForward, OUTPUT);
  61.   pinMode(rightWheelBackward, OUTPUT);
  62.  
  63.   pinMode(leftWheelEnable, OUTPUT);
  64.   pinMode(rightWheelEnable, OUTPUT);
  65.  
  66.   digitalWrite(leftWheelEnable, HIGH);
  67.   digitalWrite(rightWheelEnable, HIGH);
  68.  
  69.   pinMode(pingPin, OUTPUT);
  70.   pinMode(echoPin, INPUT);
  71.  
  72.   time = millis();
  73. }
  74.  
  75. void loop() {
  76.  
  77.   //Update time
  78.   unsigned long deltaTime = millis() - time;
  79.   time = millis();
  80.  
  81.   int serialState = 0;
  82.   if(Serial.available() > 0) {
  83.      serialState = Serial.read();
  84.   }
  85.  
  86.   //if(serialState == '1') { Do bluetooth (potem sie zrobi)
  87.    
  88.     if(!avoidBorder()) {
  89.       if(!avoidEnemiesFromSide()) {
  90.         searchAndChase();
  91.       }
  92.     }
  93.    
  94.   //}
  95. }
  96.  
  97. void searchAndChase() {
  98.   if(state == SEARCHING) {
  99.       if(enemyInFront()) {
  100.         state = CHASING;
  101.         direction = FORWARD;
  102.         move();
  103.       }
  104.       else {
  105.         rotateLeft();
  106.       }
  107.     }
  108.   else if(state == CHASING) {
  109.     if(!enemyInFront()) {
  110.       state = SEARCHING;
  111.     }
  112.     else {
  113.       move();
  114.     }
  115.   }
  116. }
  117.  
  118.  
  119. bool avoidEnemiesFromSide() {
  120.   if(enemyOnSide(true, true) || enemyOnSide(true, false) || enemyOnSide(false, false) || enemyOnSide(false, true)) {
  121.     direction = BACKWARD;
  122.     state = AVOIDING;
  123.     move();
  124.     return true;
  125.   }
  126.   else {
  127.     if(state = AVOIDING) {
  128.       state = SEARCHING;
  129.     }
  130.     return false;
  131.   }
  132. }
  133.  
  134. bool avoidBorder() {
  135.   if(state == ON_BORDER) {
  136.     if(onBorder()) {
  137.       move();
  138.       return true;
  139.     }
  140.     else {
  141.       state = SEARCHING;
  142.       return false;
  143.     }
  144.   }
  145.   else {
  146.     if(onBorder()) {
  147.       state = ON_BORDER;
  148.       switchDirection();
  149.       move();
  150.       return true;
  151.     }
  152.     else {
  153.       return false;
  154.     }
  155.   }
  156. }
  157.  
  158. void switchDirection() {
  159.   direction = FORWARD ? BACKWARD : FORWARD;
  160. }
  161.  
  162. void move() {
  163.   if(direction == FORWARD) {
  164.     forward();
  165.   }
  166.   else {
  167.     backward();
  168.   }
  169. }
  170.  
  171. void rotateLeft() {
  172.   digitalWrite(leftWheelForward, HIGH);
  173.   digitalWrite(leftWheelBackward, LOW);
  174.   digitalWrite(rightWheelForward, LOW);
  175.   digitalWrite(rightWheelBackward, HIGH);
  176. }
  177.  
  178. void rotateRight() {
  179.   digitalWrite(leftWheelForward, HIGH);
  180.   digitalWrite(leftWheelBackward, LOW);
  181.   digitalWrite(rightWheelForward, HIGH);
  182.   digitalWrite(rightWheelBackward, LOW);
  183. }
  184.  
  185. void forward() {
  186.   digitalWrite(leftWheelForward, HIGH);
  187.   digitalWrite(leftWheelBackward, LOW);
  188.   digitalWrite(rightWheelForward, HIGH);
  189.   digitalWrite(rightWheelBackward, LOW);
  190. }
  191.  
  192. void backward() {
  193.   digitalWrite(leftWheelForward, LOW);
  194.   digitalWrite(leftWheelBackward, HIGH);
  195.   digitalWrite(rightWheelForward, LOW);
  196.   digitalWrite(rightWheelBackward, HIGH);
  197. }
  198.  
  199.  
  200. //Reads distance to the closest object in front of a bot (in centimeters)
  201. long readDistance() {
  202.    digitalWrite(pingPin, LOW);
  203.    delayMicroseconds(2);
  204.    digitalWrite(pingPin, HIGH);
  205.    delayMicroseconds(10);
  206.    digitalWrite(pingPin, LOW);
  207.    long duration = pulseIn(echoPin, HIGH);
  208.    long cm = duration / 29 / 2;
  209.    return cm;
  210. }
  211.  
  212. bool enemyOnSide(bool left, bool front) {
  213.   if(front) {
  214.     if(left) {
  215.       return digitalRead(leftFrontSensor) == LOW;
  216.     }
  217.     else {
  218.       return analogRead(rightFrontSensor) < 300;
  219.     }
  220.   }
  221.   else {
  222.         if(left) {
  223.       return digitalRead(leftBackSensor) == LOW;
  224.     }
  225.     else {
  226.       return digitalRead(rightBackSensor) == LOW;
  227.     }
  228.   }
  229. }
  230.  
  231. bool onBorder() {
  232.   int value0 = analogRead(whiteSensor0);
  233.   int value1 = analogRead(whiteSensor1);
  234.   int value2 = analogRead(whiteSensor2);
  235.   if(value0 < whiteLineThreshold0) {
  236.     return true;
  237.   }
  238.   if(value1 < whiteLineThreshold1) {
  239.     return true;
  240.   }
  241.   if(value2 < whiteLineThreshold2) {
  242.     return true;
  243.   }
  244.   return false;
  245. }
  246.  
  247. bool enemyInFront() {
  248.   return readDistance() < enemySightDistance;
  249. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top