Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // I have adhered to the Duke Community Standard in completing this assignment
- // NetID: nrm27
- #include <Servo.h>
- #include <Ping.h>
- #define Rx 17
- #define Tx 16
- // Instantiate sensor and servos
- Ping ping = Ping(3,74,29);
- Servo servoLeft;
- Servo servoRight;
- // Declare global variables
- int red = 45;
- int green = 46;
- int blue = 44;
- String paddles = "";
- String quaffles = "";
- int score = 0;
- int qTotal = 0;
- int halted = 0;
- bool seeker= false;
- bool p1 = false;
- bool finished = false;
- void setup() {
- Serial.begin(9600);
- servoLeft.attach(12);
- servoRight.attach(11);
- pinMode(red, OUTPUT);
- pinMode(green, OUTPUT);
- pinMode(blue, OUTPUT);
- digitalWrite(blue, HIGH);
- Serial.begin(9600);
- Serial2.begin(9600);
- Serial3.begin(9600);
- // Start up display
- delay(100);
- Serial3.write(17);
- delay(5);
- Serial3.write(12);
- delay(3000);
- Serial3.write(18);
- }
- void loop() {
- char outgoing = " ";
- boolean left = readSensor(53);
- boolean middle = readSensor(51);
- boolean right = readSensor(49);
- digitalWrite(red, HIGH);
- digitalWrite(green, HIGH);
- if (!left && middle && !right) { // Go straight, middle sensor is on
- setServos(1.2, 1.2);
- halted = 0;
- delay(50);
- }
- else if ((left && middle && !right) || (left && !middle && !right)) { // Turn left, middle/right sensors off
- setServos(0, 0.1);
- halted = 0;
- delay(75);
- }
- else if ((!left && middle && right)||(!left && !middle && right)) { // Turn right, middle/left sensors off
- setServos(0.1, 0);
- halted = 0;
- delay(75);
- }
- else if (left && right && middle && halted == 0) { // Reached a mark
- delay(100);
- setServos(0, 0);
- ping.fire();
- if(ping.inches() < 5.0) { // If quaffle detected, record that it was seen and flash green LED
- digitalWrite(green, LOW);
- quaffles += "s";
- }
- else { // If quaffle not detected, flash red and record that it wasn't seen
- digitalWrite(red, LOW);
- quaffles += "q";
- }
- delay(250);
- digitalWrite(red, HIGH);
- digitalWrite(green, HIGH);
- halted = 1;
- setServos(1,1);
- qTotal++; // Keep track of how many marks have been seen (qTotal = total quaffles seen)
- delay(200);
- if(qTotal == 5) { // Back up once 5 marks have been seen to allow seeker to drive by
- setServos(-0.3, -0.3);
- delay(800);
- setServos(0, 0);
- }
- while(qTotal == 5) { // Reached end of line, wait for partner information
- digitalWrite(blue, LOW);
- setServos(0, 0);
- if(Serial2.available()) { // If partner has sent characters, record characters in order
- char incoming = Serial2.read();
- if(incoming == 'r' || incoming == 'g') {
- paddles += incoming;
- }
- delay(50);
- }
- if(paddles.length() >= 5) { // Enter conditional if all data from partner has been received
- Serial.println("received data");
- digitalWrite(red, HIGH);
- delay(1000);
- digitalWrite(red, LOW);
- delay(1500);
- for(int i = 0; i < paddles.length(); i++) {
- if(quaffles.charAt(i) == 's' && paddles.charAt(i) == 'r') {
- score += 10; // Add 10 points each time there is a matching quaffle and paddle
- }
- char outgoing = quaffles.charAt(i);
- Serial2.print(outgoing);
- delay(300);
- }
- digitalWrite(blue, HIGH);
- digitalWrite(red, HIGH);
- // Determine score to send other 2 teams
- char outgoing = 'w';
- if(score == 0)
- outgoing = 'f';
- if(score == 10)
- outgoing = 'a';
- if(score == 20)
- outgoing = 'b';
- if(score == 30)
- outgoing = 'c';
- if(score == 40)
- outgoing = 'd';
- if(score == 50)
- outgoing = 'e';
- delay(100);
- Serial.print("Score before ");
- Serial.println(score);
- while (!finished) { // Wait for data from other 2 teams to calculate final score
- Serial2.print(outgoing);
- delay(100);
- if (Serial2.available()) {
- char incoming = Serial2.read();
- delay(50);
- Serial.print("p1: ");
- Serial.print(p1);
- Serial.print(" seeker: ");
- Serial.println(seeker);
- if (!p1) { // Received data from other pair
- if (incoming == 'A') {
- score += 10;
- p1 = true;
- }
- else if (incoming == 'B') {
- score += 20;
- p1 = true;
- }
- else if (incoming == 'C') {
- score += 30;
- p1 = true;
- }
- else if (incoming == 'D') {
- score += 40;
- p1 = true;
- }
- else if (incoming == 'E') {
- score += 50;
- p1 = true;
- }
- else if (incoming == 'F') {
- score += 0;
- p1 = true;
- }
- } // Received data from seeker
- if (!seeker) {
- if (incoming == 'x') {
- score += 0;
- seeker = true;
- }
- else if (incoming == 'z') {
- score += 150;
- seeker = true;
- }
- }
- if (p1 && seeker) { // Once seeker and p1 have been seen, exit
- finished = true;
- }
- }
- }
- Serial.print("Score after ");
- Serial.println(score);
- while(true) { // Entire routine is finished, display final score and blink green
- Serial2.print(outgoing);
- digitalWrite(green, LOW);
- delay(250);
- digitalWrite(green, HIGH);
- delay(250);
- Serial3.write(12);
- Serial3.write(17);
- delay(5);
- Serial3.print("Total Points:");
- Serial3.write(13);
- Serial3.print(score);
- delay(50);
- }
- }
- }
- }
- }
- boolean readSensor(int pin) { // Helper method to determine if QTI sensor sees black
- pinMode(pin, OUTPUT);
- digitalWrite(pin, HIGH);
- delayMicroseconds(230);
- pinMode(pin, INPUT);
- digitalWrite(pin, LOW);
- long time = micros();
- while (digitalRead(pin));
- time = micros() - time;
- return time > 250;
- }
- void setServos(double left, double right) { // Helper method which makes setting servos simpler
- servoLeft.writeMicroseconds(1500 - (left * 200));
- servoRight.writeMicroseconds(1500 + (right * 200));
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement