#include <Servo.h>

Servo servo_left;  // Definer venstre servo
Servo servo_right; // Definer høyre servo

char inData[10];   // Allocate some space for the string
char inChar;       // Where to store the character read
byte index = 0;    // Index into array; where to store the character
int value = 1500;
int i = 1;

int analog_sensor_L_read = 0;
int analog_sensor_R_read = 0;
int Speed = 0;
int left_flag = 0;
int right_flag = 0;
int switch_flag = 1;
int Threshold = 200;

int button = 7;          // Button is connected to digital port 7
int digital_sensor = 10; // Digial obstacle sensor is connected to digital port 5

int analog_sensor_L = A1; //analog reflectance sensor is connected to analog port 0
int analog_sensor_R = A0; //analog reflectance sensor is connected to analog port 1

int Left_speed = 0;
int Right_speed = 0;

int forward_highspeed_left   = 1390;  //value when left servo is starting to turn forward with full speed
int forward_lowspeed_left    = 1490;  //value when left servo stopped turning forward
int backward_lowspeed_left   = 1612;  //value when left servo stopped turning backward
int backward_highspeed_left  = 1695;  //value when left servo is starting to turn backward with full speed

int forward_highspeed_right  = 1625;  //value when right servo is starting to turn forward with full speed
int forward_lowspeed_right   = 1552;  //value when right servo stopped turning forward
int backward_lowspeed_right  = 1433;  //value when right servo stopped turning backward
int backward_highspeed_right = 1340;  //value when right servo is starting to turn backward with full speed


void setup() 
{
  servo_left.attach(11);
  servo_right.attach(12);
  
  pinMode(button, INPUT);
  pinMode(digital_sensor, INPUT);
  pinMode(analog_sensor_L, INPUT);
  pinMode(analog_sensor_R, INPUT);
  
  Serial.begin(9600);  
}

///////////////////////////////////////////////////////////////
//  Selve programmet
///////////////////////////////////////////////////////////////
void loop()
{
   // Calibration_LeftServo();                      // Kalibrering av venstre servomotorene
   // Calibration_RightServo();                     // Kalibrering av høyre servomotorene
   // SwitchDetect();                               // Sjekk om bryteren er trykket
   // MotorControl(Left_speed=100, Right_speed=100);// Bestem farten på venstre og høyre motor
   // delay(10000);                                 // Tidsforsinkelse i millisekunder
   // LineFollower (Speed = 100, Threshold = 300);      // Følg en sort linje, sett fart (Speed) og 
                                                    //   terskelnivå (Threshold) for å skille 
                                                    //   mørkt (ca. 700) fra lys område (ca. 25)
   // if(obstacle_detect()){avoid_obstacle();}      // Detekter hindring
   // stop_running();                               // Stopp roboten til programmet restartes
}

///////////////////////////////////////////////////////////////
//  Sett fart på servoene
///////////////////////////////////////////////////////////////
void MotorControl(int L_speed, int R_speed)
{
 set_speed_servo_left(L_speed);
 set_speed_servo_right(R_speed);
}

///////////////////////////////////////////////////////////////
//  unngå hindring
///////////////////////////////////////////////////////////////

void avoid_obstacle(void)
{
  MotorControl(-100, 100); // sving mot venstre
  delay(500);

  MotorControl(80, 100);  // kjør rett fram
  delay(1000);
  
  MotorControl(100, -100); // sving mot høyre
  delay(500);
  
  MotorControl(80, 100);  // kjør rett fram
  delay(1000);
  
  MotorControl(100, -100); // sving mot høyre
  delay(500);
  
  MotorControl(80, 100); // kjør rett fram
  delay(1000);
 
  MotorControl(-100, 100); // sving til venstre
  delay(500);
  
  analog_sensor_L_read = analogRead(analog_sensor_L);
  analog_sensor_R_read = analogRead(analog_sensor_R);
  
  // Let etter svart stripe rett foran
  while((analog_sensor_L_read < Threshold) || (analog_sensor_R_read < Threshold))
  {
    analog_sensor_L_read = analogRead(analog_sensor_L);
    analog_sensor_R_read = analogRead(analog_sensor_R);
    MotorControl(80, 100);                              // Kjør rett fram
  }  
  
    MotorControl(0,0);
}

///////////////////////////////////////////////////////////////
//  Detekter hindring foran roboten
///////////////////////////////////////////////////////////////
boolean obstacle_detect()
{
    if(digitalRead(digital_sensor)==0)
    {
      Serial.println("Digital sensor = true");
      return true;
    }
    else
    {
      Serial.println("Digital sensor = false");
      return false;
    }
}

///////////////////////////////////////////////////////////////
//  Servoen følger en linje
///////////////////////////////////////////////////////////////
void LineFollower (int sp, int th)
{
  analog_sensor_L_read = analogRead(analog_sensor_L);
  analog_sensor_R_read = analogRead(analog_sensor_R);
    
  if(analog_sensor_R_read > th && analog_sensor_L_read > th) // Begge sensorer mørke
  {
    set_speed_servo_left(sp);
    set_speed_servo_right(sp);
    left_flag = 0;
    right_flag = 0;
  }
  // Dersom den svarte stripen er under begge sensorene, resett flaggenen og kjør rett fram
  
  if(analog_sensor_L_read < th && analog_sensor_R_read > th) // Venstre sensor lys, høyre sensor mørk
  {
    set_speed_servo_left(sp);
    set_speed_servo_right(30);
    left_flag = 0;
    right_flag = 1;                                          // Sett høyre flagg som sist var mørk
  }
  // Dersom den svarte stripen bare er under venstre sensor, reset høyre flagg og sett venstre flagg, sving mot venstre
  
  if(analog_sensor_L_read > th && analog_sensor_R_read < th) // Høyre sensor lys, venstre sensor mørk
  {
    set_speed_servo_left(30);
    set_speed_servo_right(sp);
    left_flag = 1;                                           // Sett vesntre flagg som sist var mørk
    right_flag = 0;
  }
  // Dersom den svarte stripen bare er under høyre sensor, reset venstre flagg og sett høyre flagg, sving mot høyre
  
  if(analog_sensor_L_read < th && analog_sensor_R_read < th)
  {
  // Dersom det ikke er noen linje under sensorene bruk flaggene til å styre roboten  
    if(left_flag == 1)
    {
      set_speed_servo_left(-10);
      set_speed_servo_right(sp);
    }
    // Dersom høyre flagg er satt sving til venstre 
    if(right_flag == 1)
    {
      set_speed_servo_left(sp);
      set_speed_servo_right(-10);
    }
    // Dersom høyre flagg er satt, sving til høyre  
  }
delay(10);  
}

///////////////////////////////////////////////////////////////
//  Kalibrer farten til venstre servo
///////////////////////////////////////////////////////////////
void Calibration_LeftServo()
{
   Serial.println("Power ON.");
   inData[0] = 1;
   inData[1] = 5;
   inData[2] = 0;
   inData[3] = 0;
   while(1)
   {
   while(Serial.available() > 0) // Don't read unless
                                 // there you know there is data
   {
       if(index < 9) // One less than the size of the array
       {
           inChar = Serial.read(); // Read a character
           inData[index] = inChar-48; // Store it
           index++; // Increment where to write next
       }
    i = 1;
   }
   if (i==1)
   {
     index = 0;
     value = inData[0]*1000+inData[1]*100+inData[2]*10+inData[3]*1;
     if(value > 2000) value = 2000;
     if(value < 1000) value = 1000;
     Serial.print("Naavaerende verdi er: ");
     Serial.print(value);
     Serial.println(".");
     servo_left.writeMicroseconds(value);
     Serial.println("Skriv inn ny verdi og trykk Enter");
     Serial.println("Verdien skal vaere mellom 1000 og 2000");
     Serial.println();
     i = 0;
   }
   delay(100);
   }
}

///////////////////////////////////////////////////////////////
//  Kalibrer farten til høyre servo
///////////////////////////////////////////////////////////////
void Calibration_RightServo()
{
   Serial.println("Power ON.");
   inData[0] = 1;
   inData[1] = 5;
   inData[2] = 0;
   inData[3] = 0;
   while(1)
   {
   while(Serial.available() > 0) // Don't read unless
                                 // there you know there is data
   {
       if(index < 9) // One less than the size of the array
       {
           inChar = Serial.read(); // Read a character
           inData[index] = inChar-48; // Store it
           index++; // Increment where to write next
       }
    i = 1;
   }
   if (i==1)
   {
     index = 0;
     value = inData[0]*1000+inData[1]*100+inData[2]*10+inData[3]*1;
     if(value > 2000) value = 2000;
     if(value < 1000) value = 1000;
     Serial.print("Naavaerende verdi er: ");
     Serial.print(value);
     Serial.println(".");
     servo_right.writeMicroseconds(value);
     Serial.println("Skriv inn ny verdi og trykk Enter");
     Serial.println("Verdien skal vaere mellom 1000 og 2000");
     Serial.println();
     i = 0;
   }
   delay(100);
   }
}

///////////////////////////////////////////////////////////////
//  Vent med å starte til bryteren er trykket stopp ved nytt trykk
///////////////////////////////////////////////////////////////
boolean SwitchDetect()
{
  if (switch_flag == 1)
  {
    while(digitalRead(button)) // Stopp motorene og vent til knappen trykkes
    {
      servo_right.writeMicroseconds(1500);
      servo_left.writeMicroseconds(1500);
    } 
    switch_flag = 0;
    // Serial.println("Start");
    delay(1000);
  }
  
   if (!digitalRead(button))
   {
     switch_flag = 1;
     delay(1000);
    // Serial.println("Stopp");
   } 
    
}

///////////////////////////////////////////////////////////////
//  Stopp
///////////////////////////////////////////////////////////////

void stop_running()
{
  servo_right.writeMicroseconds(1500);
  servo_left.writeMicroseconds(1500);
  while(true);
}

///////////////////////////////////////////////////////////////
//  Sett normalisert fart til venstre servo
///////////////////////////////////////////////////////////////
void set_speed_servo_left(int speed) 
{
  int var;
  
  if(speed > 100)
  {
    speed = 100;
  }
  
  if(speed < -100)
  {
    speed = -100;
  }
  
  if(speed < 0)
  {
    var = map(speed, -1, -100, backward_lowspeed_left, backward_highspeed_left);
  }
  else if(speed > 0)
  {
    var = map(speed, 1, 100, forward_lowspeed_left, forward_highspeed_left);
  }
        
  if(speed == 0)
  {
    var = 1500;
  }
  
  servo_left.writeMicroseconds(var);
  // Serial.print("Venstre servo satt til: ");
  // Serial.println(var);
}

///////////////////////////////////////////////////////////////
//  Sett normalisert fart til høyre servo
///////////////////////////////////////////////////////////////
void set_speed_servo_right(int speed)
{
  int var;
  
  if(speed > 100)
  {
    speed = 100;
  }
  
  if(speed < -100)
  {
    speed = -100;
  }
   
  if(speed < 0)
  {
    var = map(speed, -1, -100, backward_lowspeed_right, backward_highspeed_right);
  }
  else if(speed > 0)
  {
    var = map(speed, 1, 100, forward_lowspeed_right, forward_highspeed_right);
  }
 
  if(speed == 0)
  {
    var = 1500;
  }
        
  servo_right.writeMicroseconds(var);
  // Serial.print("Hoeyre servo satt til: ");
  // Serial.println(var);
 }