#include <Servo.h>

Servo myservo1; // Left servo
Servo myservo2;// Right servo
const int ir_sens=A0; // Infrared sensor Sharp on analog input 1
const int pickup_1=A1; // signal of the first detection coil = rechterkant robot = bovenste deel breadboard
const int pickup_2=A2; // signal of the second detection coil = linkerkant robot
boolean col=false;  // no collision at start
int ledPin_1 = 4; // connection of led_1
int ledPin_2 = 5; // connection of led_2
int pos; //position measured using the coils
int neg_pos;
int tresholdsensor=200; // approximately 1 Volt
int treshold = 200;
int hallo = 0;
int hallo2 = 0; 
  
void setup() 
{
  Serial.begin(9600);
  pinMode(ledPin_1, OUTPUT);// possibility to add lights to navigate in the dark
  pinMode(ledPin_2, OUTPUT);
  myservo1.attach(9);  //Remember the servo is hacked, so 0:full speed forward, 180:full speed backward
  myservo2.attach(10);
}

void loop() 
{
  // put your main code here, to run repeatedly:
 // hallo=analogRead(ir_sens);
  col=irRead(ir_sens); // determine if there is collision by reading IR-sensor
  if (col == true)
  {
     stop_and_blink(); //blink leds to get attention to remove object
  }
  else
  {
     pos=(analogRead(pickup_1)-0.8*analogRead(pickup_2)); // could be transformed to a range of 0 to 5V which is easier to debug in case of errror, but this saves memory and speed
     
     hallo=analogRead(pickup_1);
     hallo2=analogRead(pickup_2)*0.8;  // To compensate the fact that coil 2 detects easier (both coils have a slightly different L-value because of fabricage error)
//     Serial.println("Sensor links:");
//     Serial.println(hallo2, DEC);
//     Serial.println("Sensor rechts");    //debug lines
//     Serial.println(hallo, DEC);
//     Serial.println(" ");
     
     if(hallo<treshold && hallo2<treshold)  //When the robot hasn't found the wire yet, or when the wire is exactly between the detecting coils, the robot needs to move forward
     {
       //Serial.println("Vooruit");  //debug line
       move_forward();
     }
     
     else
     {
     
       if (pos > 0 && pos > treshold)  //When the right detection coil has a larger detection amplitude (pos>0) and it is higher than the treshold value, the robot needs to turn right
       {
            //Serial.println("rechts");  //debug line
            move_right(pos); 
           
       }
     
       else if(pos < 0 && -pos > treshold ) //When the left detection coil has a larger detection amplitude (pos<0), the robot needs to turn left
       {
            //Serial.println("links");  //debug line
            move_left(-pos); 
         
       }
     }
  } 
  

}

float irRead(int irSense)     //Code for the object detection, so it doesn't ride against objects placed in the followed path
{ 
  boolean collision=false;

float averaging = 0;

averaging = analogRead(irSense);

if(averaging >= tresholdsensor)
{
   collision = true; 
}  

else
{
   collision = false; 
}  
  float averaging1 = 0;             //  Holds value to average readings
  float distance[] = {0,0,0}; 

  // Get a sampling of 3 readings from sensor
  for (int i=0; i<3; i++) 
  {
    distance [i] = analogRead(irSense)/(1024.0f)*5.0f; //in unit voltage
    if (distance[i]>0.4) // start averaging at 30cm 
    {
       averaging1 = averaging1 + distance[i];
       
       if (i>0 && distance[i-1]<=distance[i])  //since we are calculatng in voltages, higher voltage=smaller distance (neergaande flank)
       {
          if(distance[i]> 1) //1 volt corresponds to 12 cm
          {
            collision = true; 
          }
          else 
          {
            collision = false;
          }
      
       }
    }
     
  }   
    
    delay(55);      // Wait 55 ms between each read
                    // According to datasheet time between each read
                    //  is -38ms +/- 10ms. Waiting 55 ms assures each
    return (collision);               //  read is from a different sample
  }    

void stop_and_blink()
  {
    myservo1.write(85); // stopping the servomotors, is due to hacking not exactly on 90, so the stop values were tested in advance
    myservo2.write(89);
//    digitalWrite(ledPin_1, HIGH);
//    digitalWrite(ledPin_2, HIGH);
//    delay(250);
//    digitalWrite(ledPin_1, LOW);
//    digitalWrite(ledPin_2, LOW);
//    delay(250);
delay(2000); //wait at least 2 seconds after you have again clear view


  }   
 
void move_forward()
{
  // LED implementation is possible in here
  //Serial.println("rechtdoor geweest");  //Debug line
  myservo1.write(0); //Since the servos are placed differently, forward is 0 for servo1 and 180 for servo2
  myservo2.write(180); 
  delay(30);
}	

void move_left(int ind)
{ 
  //Serial.println("Ga links");  //debug line
  //Serial.println(ind, DEC);
  
    while(ind>treshold)    // calculates how long the turning needs to happen 
    {
      //Serial.println("links geweest");
      myservo1.write(85);   // 85 is standing still
      myservo2.write(110); //soft turn to the left, because if he robot goes too fast it hasn't the time to detect
      ind = 0.8*analogRead(pickup_2);                     // Again compensation for better detection of the wire, calculates wether the robot may go out the while loop or not 
      delay(30);
    }

} 
void move_right(int ind)
{
 // Serial.println("Ga rechts");  //debug line
 // Serial.println(ind,DEC);
  
  while(ind>treshold)    // calculates how long the turning needs to happen
    {
      //Serial.println("rechts geweest");
      
      myservo1.write(60); //soft turn to the right, because if he robot goes too fast it hasn't the time to detect
      myservo2.write(89); // 85 is standing still
      ind = analogRead(pickup_1);             // calculates the value that decides if the robot can go out of the while-loop 
      delay(30);
    }  
}