/* 
 * Project Name：Sumo Robot_2
 * Author：Tieying Chu, Qunyi Pan （LCD)
 * Date： 05/10/2022
 * Project description: This project is the code part of the sumo robot, the ultrasonic sensor on this sumo robot is able to identify the incoming object inside the rink and push it out of bounds. And stop and return to the center of the rink when it touches the white boundary.
 * Source: https://www.arduino.cc/en/reference/wire 
 *         https://www.arduinolibraries.info/libraries/liquid-crystal-i2-c  
 *         https://lastminuteengineers.com/l293d-dc-motor-arduino-tutorial/        
 */
 
#include <Wire.h>
#include <LiquidCrystal_I2C.h>


// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);


//LCD Display Notes
//VCC to power
//GND to Ground
//SCL to A5
//SDA to A4

// Ultrasonic
int trig=12;
int echo=13;

//Motor1
int enA = 2;
int MotorPin1 = 3;
int MotorPin2 =4;
//Motor2
int enB = 5;
int MotorPin3 = 6;
int MotorPin4 = 7;

//IR sensors
int color_out1 = 8;
int color_out2 = 9;
int color1;
int color2;

float distance;

unsigned long currentTime;

unsigned long Time;


void setup() {
  // put your setup code here, to run once:
  //Serial Port begin
  Serial.begin(9600);
  
  //Define inputs and outputs
  //Ultrasonic
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  
  // motors
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(MotorPin1, OUTPUT);
  pinMode(MotorPin2, OUTPUT);
  pinMode(MotorPin3, OUTPUT);
  pinMode(MotorPin4, OUTPUT);
  
  // IR sensors setup
  pinMode(color_out1, INPUT);
  pinMode(color_out2, INPUT);

/*
  cli();

  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  
  OCR1A = 15264;

  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS12) | (1 << CS10);
  TIMSK1 |= (1 << OCIE1A);

  sei();
  */

  // LCD initialize
  lcd.init();
  //Serial Port begin
  Serial.begin (9600);
  // initialize the LCD
  lcd.begin(16,2);
  // Turn on the blacklight
  lcd.backlight(); 
  
  lcd.setCursor(0, 0);
  lcd.print("      Game  ");
  lcd.setCursor(0, 1);
  lcd.print("      Start  ");
  

  // initial setting
  halt (0);
  delay(1000);
  currentTime =millis();
}

/*
ISR(TIMER1_COMPA_vect){    //This is the interrupt request
  


}
*/


void loop() {

  // put your main code here, to run repeatedly:
  color1 = digitalRead(color_out1);
  color2 = digitalRead(color_out2);
  
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trig, LOW);
  // Delay LOW pulse 20 microseconds 
  delayMicroseconds(5);
  digitalWrite(trig, HIGH);
  // Delay HIGH pulse 20 microseconds 
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  // Read the time of echo to get High
  // Use the time to calculate the  distance
  distance = pulseIn(echo,HIGH)*340.29/2/10000;
  //distance = sonar.ping_cm();
  
  //print in serial to test
  Serial.println(distance);
  Serial.println(color1);
  Serial.println(color2);
  

  
  // if both side touch the edge
  if(color1==0& color2==0){
    
   // motor action 
   halt(0);
  delay(200);  
   backward(180);
   
   // print  
   lcd.clear();
   lcd.setCursor(0, 0);
   lcd.print("      Both    ");
   lcd.setCursor(0, 1);
   lcd.print("      Edge    "); 
   delay(250);
   }
   
// if left side touch the edge
  else if(color1==0 && color2==1){ 

   // motor action 
   halt(0);
   delay(200);
   turnleft(200);
  delay(20);
   backward(180);
   
   // print
   lcd.clear();
   lcd.setCursor(0, 0);
   lcd.print("      Left    ");
   lcd.setCursor(0, 1);
   lcd.print("      Edge    ");
   delay(250);
    }
    
  // if right side touch the edge
  else if(color2==0 && color1==1){
    

   // motor action
   halt(0);
  delay(200);
  turnright(200);
  delay(20);
   backward(180);
   
   // print
   lcd.clear();
   lcd.setCursor(0, 0);
   lcd.print("      Right   ");
   lcd.setCursor(0, 1);
   lcd.print("      Edge    ");    
   delay(250);
    }

   // if the robot in the rink
  else if(color1 == 1 && color2==1){
  // print the status
  
    // if there is a opponent within 40cm
  if (distance <= 45) { 
    halt(0);
    delay(200);
   // go forward and push the object     
      while(color1==1&& color2==1){
        forward (130,255);
        //Forward(200);
        color1 = digitalRead(color_out1);
        color2 = digitalRead(color_out2);

       while(millis()- currentTime > 2000) {
          
            currentTime =millis();
            
            // print distance 
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Forward");
            lcd.setCursor(0, 1);
            lcd.print("Distance:");
            lcd.print(distance);
            lcd.print("cm          ");
            
        }
        
      }
      halt(0);
      delay(200);
      while(1){
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Victory");
      delay(200);
      }
      

             
          
      }     
       
  else{  
    
  // motors work  
  turnleft(120);
  
  while(millis()- currentTime >2000){
  currentTime =millis();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" Finding ");
  }
  
   }     
  
  }


  }
  



void forward(int value1,int value2) {   //  go forward
  turnright(255);
  
  analogWrite(enA, value1);
  analogWrite(enB, value2);
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, HIGH);
  digitalWrite(MotorPin3, HIGH);
  digitalWrite(MotorPin4, LOW);
  delay(10);
 
}

void backward(int value) {  // go backwards
  analogWrite(enA, value);
  analogWrite(enB, value);
  digitalWrite(MotorPin1, HIGH);
  digitalWrite(MotorPin2, LOW);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, HIGH);

}

void halt(int value) {        // stop 
  analogWrite(enA, value);
  analogWrite(enB, value);
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, LOW);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, LOW);

}
void turnright(int value) {        // turn direction
  analogWrite(enA, value);
  analogWrite(enB, value);
  digitalWrite(MotorPin1, HIGH);
  digitalWrite(MotorPin2, LOW);
  digitalWrite(MotorPin3, HIGH);
  digitalWrite(MotorPin4, LOW);

}

void turnleft(int value) {        // turn direction
  analogWrite(enA, value);
  analogWrite(enB, value);
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, HIGH);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, HIGH);

}

void slowturnleft(int value) {        // turn direction
  analogWrite(enA, value);
  analogWrite(enB, 0);
  digitalWrite(MotorPin1, LOW);
  digitalWrite(MotorPin2, HIGH);
  digitalWrite(MotorPin3, LOW);
  digitalWrite(MotorPin4, HIGH);
}