#include <QTRSensors.h>

// Description: Line sensing robot with object avoidance
// Written By: Michael Lindley
// Date: 17/11/2019

//Libraries
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include <Wire.h>
#include <LiquidCrystal.h>

LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
////////
#define NUM_SENSORS             8  // number of sensors used
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
#define EMITTER_PIN             2  // emitter is controlled by digital pin 2
// Pins defined for Ultrasonic Sensor HC-SR04
// Sensor 1 - Middle
#define Gnd_PIN 30                       //Ground PIN
#define Echo_PIN 32                      //Signal Out PIN
#define Trig_PIN 34                      //Return Signal
#define Vcc_PIN 36                       //5vdc supply
#define Max_Dist1 200                    //Setting Max Distance uS can sense for
// Sensor 2 - LH Side
#define Gnd2_PIN 22                      //Ground PIN
#define Echo2_PIN 24                     //Signal Out PIN
#define Trig2_PIN 26                     //Return Signal
#define Vcc2_PIN 28                      //5vdc supply
#define Max_Dist2 200                    //Setting Max Distance uS can sense for
// Sensor 3 - RH Side
#define Gnd3_PIN 38                      //Ground PIN
#define Echo3_PIN 40                     //Signal Out PIN
#define Trig3_PIN 42                     //Return Signal
#define Vcc3_PIN 44                       //5vdc supply
#define Max_Dist3 200                    //Setting Max Distance uS can sense for

// sensors 0 through 5 are connected to analog inputs 0 through 5, respectively
QTRSensorsAnalog qtra((unsigned char[]) {15, 14, 13, 12, 11, 10, 9, 8},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(3);

int dark = 800;
int Program = 0;
int Distance1;                          //Ultrasonic sensor 1
int Distance2;                          //Ultrasonic sensor 2
int Distance3;                          //Ultrasonic sensor 3

NewPing sonar1(Trig_PIN, Echo_PIN, Max_Dist1); // NewPing setup of pins and maximum distance.
NewPing sonar2(Trig2_PIN, Echo2_PIN, Max_Dist2); // NewPing setup of pins and maximum distance.
NewPing sonar3(Trig3_PIN, Echo3_PIN, Max_Dist3); // NewPing setup of pins and maximum distance.

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  ////////////
  lcd.begin(16, 2); // start the library
  lcd.setCursor(0,0);
  lcd.print("UltraSense");
  delay(1000); // delay for 1000 miliseconds
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Duration: ");
  lcd.setCursor(0,1);
  lcd.print("Distance: ");

//////////////////
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");
  AFMS.begin();  // create with the default frequency 1.6KHz
  // Set the speed to start, from 0 (off) to 255 (max speed)
  myMotor1->run(RELEASE);
  myMotor3->run(RELEASE);
  /////////////////////////////////////////
  delay(500);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode

   Serial.println("Calibrating");
  for (int i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  {
    qtra.calibrate();       // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
  }
  digitalWrite(13, LOW);     // turn off Arduino's LED to indicate we are through with calibration

  // print the calibration minimum values measured when emitters were on
  Serial.begin(9600);
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMinimumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  // print the calibration maximum values measured when emitters were on
  for (int i = 0; i < NUM_SENSORS; i++)
  {
    Serial.print(qtra.calibratedMaximumOn[i]);
    Serial.print(' ');
  }
  Serial.println();
  //Serial.println();
  delay(1000);
}
{
//Setting Pins to I/Os - Sensor 1
  pinMode(Gnd_PIN, OUTPUT);             //Setting the ground pin to an output
  digitalWrite(Gnd_PIN, LOW);           //Setting the ground pin low, 0v
  pinMode(Vcc_PIN, OUTPUT);             //Setting the supply voltage to an output
  digitalWrite(Vcc_PIN, HIGH);          //Setting the supply voltage to 5v
  pinMode(echoPin, INPUT);              //Setting ECHO pin to an input
  pinMode(trigPin, OUTPUT);             //Setting TRIGGER pin to an output
//Setting Pins to I/Os - Sensor 2
  pinMode(Gnd2_PIN, OUTPUT);             //Setting the ground pin to an output
  digitalWrite(Gnd2_PIN, LOW);           //Setting the ground pin low, 0v
  pinMode(Vcc2_PIN, OUTPUT);             //Setting the supply voltage to an output
  digitalWrite(Vcc2_PIN, HIGH);          //Setting the supply voltage to 5v
  pinMode(echoPin2, INPUT);              //Setting ECHO pin to an input
  pinMode(trigPin2, OUTPUT);             //Setting TRIGGER pin to an output
//Setting Pins to I/Os - Sensor 3
  pinMode(Gnd3_PIN, OUTPUT);             //Setting the ground pin to an output
  digitalWrite(Gnd3_PIN, LOW);           //Setting the ground pin low, 0v
  pinMode(Vcc3_PIN, OUTPUT);             //Setting the supply voltage to an output
  digitalWrite(Vcc3_PIN, HIGH);          //Setting the supply voltage to 5v
  pinMode(echoPin3, INPUT);              //Setting ECHO pin to an input
  pinMode(trigPin3, OUTPUT);             //Setting TRIGGER pin to an output
}
void loop() {
{
long Distance1, Distance2, Distance3;
//Ultrasonic Sensor Serial Coms to display value
  delay(500);
  unsigned int uS1 = sonar1.ping();       // Send ping, get ping time in microseconds (uS).
  unsigned int uS2 = sonar2.ping();       // Send ping, get ping time in microseconds (uS).
  unsigned int uS3 = sonar3.ping();       // Send ping, get ping time in microseconds (uS).
  Distance1 = (uS1 / US_ROUNDTRIP_CM);
  Distance2 = (uS2 / US_ROUNDTRIP_CM);
  Distance3 = (uS3 / US_ROUNDTRIP_CM);
  Serial.print("Ping: ");
  Serial.print(Distance1);           // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");                 // Serial Display Units in Centimeters
}

int dark = 800;
unsigned int position = qtra.readLine(sensorValues);
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
  Serial.print(sensorValues[i]);
  Serial.print('\t');
}
Serial.println();
////////////////////////////
while (Program == 0){
  if(sensorValues[4] > dark && sensorValues[3] > dark && sensorValues[0] > dark && sensorValues[1] > dark && sensorValues[2] > dark){
  myMotor1->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor1->setSpeed(75);
  myMotor3->setSpeed(25); // was 25
  lcd.setCursor(0,0);
  lcd.print("right 90 Degree ");
  delay(100);
}
    else
    //// LINE FOLLOWER
    if(sensorValues[0] > dark || sensorValues[1] > dark || sensorValues[2] > dark){
      right();
    }
    if(sensorValues[7] > dark || sensorValues[6] > dark || sensorValues[5] > dark){
      left();
    }
    if(sensorValues[4] > dark && sensorValues[3] > dark){
      foward();
    }
  }
if(sensorValues[4] > dark && sensorValues[3] > dark && sensorValues[7] > dark && sensorValues[6] > dark && sensorValues[5] > dark){
  myMotor1->run(BACKWARD);
  myMotor3->run(BACKWARD);
  myMotor1->setSpeed(25);
  myMotor3->setSpeed(75); // was 25
  lcd.setCursor(0,0);
  lcd.print("left 90 Degree  ");
  delay(100);
}
    else{
    //// LINE FOLLOWER
    if(sensorValues[0] > dark || sensorValues[1] > dark || sensorValues[2] > dark){
      right();
    }
    if(sensorValues[7] > dark || sensorValues[6] > dark || sensorValues[5] > dark){
      left();
    }
    if(sensorValues[4] > dark && sensorValues[3] > dark){
      foward();
    }
  }

// If the Ultrasonic Sensor 1 detects an object less than 10cm, a 1 is set in a Data Register to initiate Obstacle Avoidance
while (Program == 0) {
  if (Distance1 <= 10){
  Program = Program +1;                            // Detect obstacle at <=10cm - Set 1 in Data Register
  }  else {
  Program = 0;
}
}
// If 1 in Data Register - Activate Object Avoidance Program.
while (Program == 1){
  if (Distance3 >= 10){
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(25);
    myMotor3->setSpeed(90);
  }  else
    if (Distance3 <= 10){         // Once uS3 detects object, +1 into Program register to move program on
    Program = Program +1 ;
    delay (500);
}
}
while (Program == 2){
  if(sensorValues[4] > dark && sensorValues[3] > dark && sensorValues[7] > dark && sensorValues[6] > dark && sensorValues[5] > dark){
    Program = Program +1;
    delay (100);
} else {
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(40);
    myMotor3->setSpeed(35);
  }
}
// Once line detected turn robot till middle sensors reading high and outer sensors reading low.
while (Program == 3) {
  if(sensorValues[4] > dark && sensorValues[3] > dark && sensorValues[7] > dark && sensorValues[6] > dark && sensorValues[5] > dark){
  Program = Program+1;
  } else {
    myMotor1->run(FORWARD);
    myMotor3->run(BACKWARD);
    myMotor1->setSpeed(0);
    myMotor3->setSpeed(35);
}
}
// Stop Object Avoidance Program - Set 0 in Data Register.
  if(Program == 4){
  Program = 0;
  }  else
  if(Program > 4){
  Program = 0;
  end_if;
}
  // Start Line Following Program.

}