SHARE
TWEET

Untitled

a guest Apr 18th, 2019 84 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // I have adhered to the Duke Community Standard in completing this assignment
  2. // NetID: nrm27
  3. #include <Servo.h>
  4. #include <Ping.h>
  5. #define Rx 17
  6. #define Tx 16
  7.  
  8. // Instantiate sensor and servos
  9. Ping ping = Ping(3,74,29);
  10. Servo servoLeft;
  11. Servo servoRight;
  12.  
  13. // Declare global variables
  14. int red = 45;
  15. int green = 46;
  16. int blue = 44;
  17. String paddles = "";
  18. String quaffles = "";
  19. int score = 0;
  20. int qTotal = 0;
  21. int halted = 0;
  22. bool seeker= false;
  23. bool p1 = false;
  24. bool finished = false;
  25.  
  26. void setup() {
  27.   Serial.begin(9600);
  28.   servoLeft.attach(12);
  29.   servoRight.attach(11);
  30.  
  31.   pinMode(red, OUTPUT);
  32.   pinMode(green, OUTPUT);
  33.   pinMode(blue, OUTPUT);
  34.   digitalWrite(blue, HIGH);
  35.  
  36.   Serial.begin(9600);
  37.   Serial2.begin(9600);
  38.   Serial3.begin(9600);
  39.  
  40.   // Start up display
  41.   delay(100);
  42.   Serial3.write(17);
  43.   delay(5);
  44.   Serial3.write(12);
  45.   delay(3000);
  46.   Serial3.write(18);
  47. }
  48.  
  49. void loop() {
  50.   char outgoing = " ";
  51.   boolean left = readSensor(53);
  52.   boolean middle = readSensor(51);
  53.   boolean right = readSensor(49);
  54.   digitalWrite(red, HIGH);
  55.   digitalWrite(green, HIGH);  
  56.  
  57.   if (!left && middle && !right) { // Go straight, middle sensor is on
  58.     setServos(1.2, 1.2);
  59.     halted = 0;
  60.     delay(50);
  61.   }
  62.   else if ((left && middle && !right) || (left && !middle && !right)) { // Turn left, middle/right sensors off
  63.     setServos(0, 0.1);
  64.     halted = 0;
  65.     delay(75);
  66.   }
  67.   else if ((!left && middle && right)||(!left && !middle && right)) { // Turn right, middle/left sensors off
  68.     setServos(0.1, 0);
  69.     halted = 0;
  70.     delay(75);
  71.   }
  72.   else if (left && right && middle && halted == 0) { // Reached a mark
  73.    
  74.     delay(100);
  75.     setServos(0, 0);
  76.     ping.fire();
  77.    
  78.     if(ping.inches() < 5.0) { // If quaffle detected, record that it was seen and flash green LED
  79.       digitalWrite(green, LOW);
  80.       quaffles += "s";
  81.     }
  82.     else { // If quaffle not detected, flash red and record that it wasn't seen
  83.       digitalWrite(red, LOW);
  84.       quaffles += "q";
  85.     }
  86.    
  87.     delay(250);
  88.     digitalWrite(red, HIGH);
  89.     digitalWrite(green, HIGH);  
  90.     halted = 1;
  91.     setServos(1,1);
  92.     qTotal++; // Keep track of how many marks have been seen (qTotal = total quaffles seen)
  93.     delay(200);
  94.  
  95.    
  96.     if(qTotal == 5) { // Back up once 5 marks have been seen to allow seeker to drive by
  97.       setServos(-0.3, -0.3);
  98.         delay(800);
  99.         setServos(0, 0);
  100.     }
  101.     while(qTotal == 5) { // Reached end of line, wait for partner information
  102.       digitalWrite(blue, LOW);
  103.       setServos(0, 0);
  104.        
  105.       if(Serial2.available()) { // If partner has sent characters, record characters in order
  106.         char incoming = Serial2.read();
  107.         if(incoming == 'r' || incoming == 'g') {
  108.           paddles += incoming;
  109.         }
  110.         delay(50);
  111.       }
  112.  
  113.       if(paddles.length() >= 5) { // Enter conditional if all data from partner has been received
  114.         Serial.println("received data");
  115.         digitalWrite(red, HIGH);
  116.         delay(1000);
  117.         digitalWrite(red, LOW);
  118.         delay(1500);
  119.        
  120.         for(int i = 0; i < paddles.length(); i++) {
  121.           if(quaffles.charAt(i) == 's' && paddles.charAt(i) == 'r') {
  122.             score += 10; // Add 10 points each time there is a matching quaffle and paddle
  123.           }
  124.           char outgoing = quaffles.charAt(i);
  125.           Serial2.print(outgoing);
  126.           delay(300);
  127.         }
  128.         digitalWrite(blue, HIGH);
  129.         digitalWrite(red, HIGH);
  130.  
  131.         // Determine score to send other 2 teams
  132.         char outgoing = 'w';
  133.         if(score == 0)
  134.           outgoing = 'f';
  135.         if(score == 10)
  136.           outgoing = 'a';
  137.         if(score == 20)
  138.           outgoing = 'b';
  139.         if(score == 30)
  140.           outgoing = 'c';
  141.         if(score == 40)
  142.           outgoing = 'd';
  143.         if(score == 50)
  144.           outgoing = 'e';
  145.  
  146.         delay(100);
  147.         Serial.print("Score before ");
  148.         Serial.println(score);
  149.                
  150.         while (!finished) { // Wait for data from other 2 teams to calculate final score
  151.           Serial2.print(outgoing);
  152.           delay(100);
  153.          
  154.           if (Serial2.available()) {
  155.             char incoming = Serial2.read();
  156.             delay(50);
  157.             Serial.print("p1: ");
  158.             Serial.print(p1);
  159.             Serial.print(" seeker: ");
  160.             Serial.println(seeker);
  161.            
  162.             if (!p1) { // Received data from other pair
  163.               if (incoming == 'A') {
  164.                 score += 10;
  165.                 p1 = true;
  166.               }
  167.               else if (incoming == 'B') {
  168.                 score += 20;
  169.                 p1 = true;
  170.               }
  171.               else if (incoming == 'C') {
  172.                 score += 30;
  173.                 p1 = true;
  174.               }
  175.               else if (incoming == 'D') {
  176.                 score += 40;
  177.                 p1 = true;
  178.               }
  179.               else if (incoming == 'E') {
  180.                 score += 50;
  181.                  p1 = true;
  182.               }
  183.               else if (incoming == 'F') {
  184.                 score += 0;
  185.                 p1 = true;
  186.               }
  187.             } // Received data from seeker
  188.             if (!seeker) {
  189.               if (incoming == 'x') {
  190.                 score += 0;
  191.                 seeker = true;
  192.               }  
  193.               else if (incoming == 'z') {
  194.                 score += 150;
  195.                 seeker = true;
  196.               }
  197.             }
  198.  
  199.             if (p1 && seeker) { // Once seeker and p1 have been seen, exit
  200.               finished = true;
  201.             }
  202.           }
  203.         }
  204.  
  205.         Serial.print("Score after ");
  206.         Serial.println(score);
  207.  
  208.        
  209.              
  210.         while(true) { // Entire routine is finished, display final score and blink green
  211.           Serial2.print(outgoing);
  212.           digitalWrite(green, LOW);
  213.           delay(250);
  214.           digitalWrite(green, HIGH);
  215.           delay(250);
  216.           Serial3.write(12);
  217.           Serial3.write(17);
  218.           delay(5);
  219.           Serial3.print("Total Points:");
  220.           Serial3.write(13);
  221.           Serial3.print(score);
  222.           delay(50);
  223.         }
  224.       }
  225.     }
  226.   }
  227. }
  228.  
  229. boolean readSensor(int pin) { // Helper method to determine if QTI sensor sees black
  230.   pinMode(pin, OUTPUT);
  231.   digitalWrite(pin, HIGH);
  232.   delayMicroseconds(230);
  233.   pinMode(pin, INPUT);
  234.   digitalWrite(pin, LOW);
  235.   long time  = micros();
  236.   while (digitalRead(pin));
  237.   time = micros() - time;
  238.   return time > 250;
  239. }
  240.  
  241. void setServos(double left, double right) { // Helper method which makes setting servos simpler
  242.   servoLeft.writeMicroseconds(1500 - (left * 200));
  243.   servoRight.writeMicroseconds(1500 + (right * 200));
  244. }
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