Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //We have adhered to the Duke Community Standard in Completing this Assignment
- //Signed: Cameron Jarnot, Sophie Rubin
- long rcTime(int pin, boolean print); //QTI values
- #include <Servo.h> //servos
- #include <Wire.h>
- #include "Adafruit_TCS34725.h" //RGB
- #define Rx 17 //Receiving
- #define Tx 16 //Transmitting
- const int TxPin = 10; //transmit pin
- #include <SoftwareSerial.h> //include communication
- SoftwareSerial mySerial = SoftwareSerial(255, TxPin); //set communication pins
- int paddles = 0; //to count how far along the path it is
- String colors = ""; //to record the paddle colors
- char myScore = 'f'; //set score to zero
- Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X); //initialize RGB
- Servo servoLeft; //turn on left servo
- Servo servoRight; //turn on right servo
- boolean seeker = false; //waiting for seeker score
- boolean p1 = false; //waiting for other pair's score
- boolean finished = false; //waiting to have all scores
- boolean happening = false; //tracking whether or not it has score with chaser
- void setup() {
- pinMode(7, OUTPUT); //non-built in LED
- pinMode(44, OUTPUT); //red
- pinMode(46, OUTPUT); //green
- pinMode(49,OUTPUT); //left QTI
- pinMode(51,OUTPUT); //center QTI
- pinMode(53,OUTPUT); //right QTI
- Serial.begin(9600);
- delay(300);
- servoLeft.attach(12); //connect to left servo
- servoRight.attach(11); //connect to right servo
- pinMode(LED_BUILTIN, OUTPUT);
- Serial.begin(9600);
- Serial2.begin(9600);
- delay(500);
- Serial3.begin(9600);
- delay(100);
- Serial3.write(17); // Turn backlight on
- delay(5); // Required delay
- Serial3.write(18); // Turn backlight off
- }
- void loop() {
- digitalWrite(44, LOW); //red on
- digitalWrite(46, LOW); //green on
- long valueL = rcTime(49, false); //left QTI
- long valueM = rcTime(51, false); //center QTI
- long valueR = rcTime(53, false); //right QTI
- boolean qtL = true; //left QTI sense (true for black)
- boolean qtM = true; //middle QTI sense (true for black)
- boolean qtR = true; //right QTI sense (true for black)
- if(valueL<300){ //if left rcTime less than threshold
- qtL = false; //it sees white
- }
- if(valueM<300){ //if center rcTime less than threshold
- qtM = false; //it sees white
- }
- if(valueR<300){ //if right rcTime less than threshold
- qtR = false; //it sees white
- }
- if(!qtL && qtM && !qtR){ //if only middle black
- servoLeft.writeMicroseconds(1540); //move forward
- servoRight.writeMicroseconds(1460);
- delay(5);
- }
- else if(qtR && !qtL){ //if left white and right black
- servoLeft.writeMicroseconds(1700); //turn right
- servoRight.writeMicroseconds(1500);
- delay(5);
- }
- else if(!qtR && qtL){ //if right white and left black
- servoLeft.writeMicroseconds(1500); //turn left
- servoRight.writeMicroseconds(1300);
- delay(5);
- }
- else if(qtL && qtM && qtR){ //if all black
- servoLeft.writeMicroseconds(1500); //stall, no movement
- servoRight.writeMicroseconds(1500);
- paddles+=1; //track one more has
- uint16_t clear, red, green, blue; //see color from RGB
- delay(400);
- tcs.getRawData(&red, &green, &blue, &clear); //collect RGB data
- if(red>=green){ //if it is more red than green
- char outgoing = 'r'; //add red character to string
- colors += "r"; //track that paddle was red
- //Serial2.println('r');
- digitalWrite(46, HIGH); //blink darker blue
- delay(500);
- digitalWrite(46, LOW);
- delay(500); //wait .5sec
- }
- else if(green>red){ //if it is more green than red
- char outgoing = 'g'; //add green character to string
- colors += "g"; //track that paddle was green
- //Serial2.println('g');
- digitalWrite(44, HIGH); //blink green
- delay(500);
- digitalWrite(44, LOW);
- delay(500); //wait .5sec
- }
- servoLeft.writeMicroseconds(1540); //move forward
- servoRight.writeMicroseconds(1460);
- delay(400); //delay .2sec
- }
- if(paddles == 5) { //if it has sensed all five paddles
- digitalWrite(46, HIGH); //turn off built in LED
- digitalWrite(44, HIGH);
- servoLeft.writeMicroseconds(1500-(0.3*200)); //back up at the end
- servoRight.writeMicroseconds(1500+(0.3*200));
- delay(1200);
- servoLeft.writeMicroseconds(1500); //stop
- servoRight.writeMicroseconds(1500);
- digitalWrite(7, HIGH);
- delay(3000);
- for(int i=0; i<5; i++){ //for each character in colors
- char outgoing = colors.charAt(i); //next letter of colors
- Serial2.print(outgoing); //send char
- delay(500); //wait .5sec
- }
- String chaser = ""; //create string for chaser's letters
- int score = 0; //initialize score
- while(!happening){
- if(Serial2.available()){ //if another robot is sending
- char in = Serial2.read(); //receive letter
- if(in == 's' || in == 'q'){ //if it's the chaser
- chaser += in; //add the letter to chaser string
- }
- }
- if(chaser.length() == 5){ //once all 5 have been received
- delay(50);
- for(int i=0; i<5; i++){ //for each letter in colors and chaser
- if(colors.charAt(i) == 'r' && chaser.charAt(i) == 's'){ //if quaffle and red
- score += 10; //add ten to score
- }
- }
- happening = true; //has score with chaser
- }
- }
- while(!finished){ //while it doesn't have the total
- delay(1);
- if(Serial2.available()){ //if someone is sending
- char seeIn = Serial2.read(); //read the character
- if(! p1){ //if p1 score hasn't been received
- if(seeIn == 'A'){ //if 'A' is sent
- score += 10; //add ten to score
- p1 = true; //has received p1
- }
- if(seeIn == 'B'){ //if 'B' is sent
- score += 20; //add twenty to score
- p1 = true; //has received p1
- }
- if(seeIn == 'C'){ //if 'C' is sent
- score += 30; //add thirty to score
- p1 = true; //has received p1
- }
- if(seeIn == 'D'){ //if 'D' is sent
- score += 40; //add forty to score
- p1 = true; //has received p1
- }
- if(seeIn == 'E'){ //if 'E' is sent
- score += 50; //add fifty to score
- p1 = true; //has received p1
- }
- if(seeIn == 'F'){ //if 'F' is sent
- p1 = true; //do not add to score, has received p1
- }
- }
- if(! seeker){ //if seeker score hasn't been received
- if(seeIn == 'x'){ //if 'x' is received
- seeker = true; //do not add to score, has received seeker
- }
- if(seeIn == 'z'){ //if 'z' is received
- score += 150; //add 150 to score
- seeker = true; //has received seeker
- }
- }
- delay(1);
- if(p1 && seeker){ //if it has all bots' scores
- Serial3.write(12); //clear screen
- delay(5);
- Serial3.print("Total score: ");
- Serial3.println(score); //print total score
- Serial3.print(colors); //print paddles' colors seen
- finished = true; //consider computation complete
- }
- }
- while(finished){ //while computation is complete
- Serial3.write(12); //clear screen
- Serial3.print("Total score: "); //print total score
- Serial3.println(score); //print total score
- Serial3.print(colors); //print paddles' colors seen
- delay(1000);
- }
- }
- }
- }
- long rcTime(int pin, boolean print) { //calculate QTI values
- pinMode(pin, OUTPUT); //send IR signal
- digitalWrite(pin, HIGH);
- delayMicroseconds(230);
- pinMode(pin, INPUT); //receive IR signal
- digitalWrite(pin, LOW); long
- time = micros(); while
- (digitalRead(pin)); time =
- micros() - time; //calculate time constant
- return time;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement