#include "Config.h"

int Davach[5];

void Znachennya_Davach()
  {
  Davach[1]=digitalRead(D1);
  Davach[2]=digitalRead(D2);
  Davach[3]=digitalRead(D3);
  Davach[4]=digitalRead(D4);
  Davach[5]=digitalRead(D5);
  }

//Задаємо швидкість двигунам
void Set_speed (int Speed_L, int Speed_R)
  {
      analogWrite(Pin_Speed_L,Speed_L);
      analogWrite(Pin_Speed_R,Speed_R);
  }



//Процедури руху робота
void Forward() //Рух вперед
{
  digitalWrite(Dir1L, HIGH);
  digitalWrite(Dir2L, LOW);

  digitalWrite(Dir1R, HIGH);
  digitalWrite(Dir2R, LOW);
}

void Left() // Рух вліво
{
  digitalWrite(Dir1L, LOW);
  digitalWrite(Dir2L, HIGH);

  digitalWrite(Dir1R, HIGH);
  digitalWrite(Dir2R, LOW);
}

void Right() // Рух вправо
{
  digitalWrite(Dir1L, HIGH);
  digitalWrite(Dir2L, LOW);

  digitalWrite(Dir1R, LOW);
  digitalWrite(Dir2R, HIGH);
}

void Reverse() // Рух назад
{
  digitalWrite(Dir1L, LOW);
  digitalWrite(Dir2L, HIGH);

  digitalWrite(Dir1R, LOW);
  digitalWrite(Dir2R, HIGH);
}

void Stop() // Повна зупинка
{
  digitalWrite(Dir1L, LOW);
  digitalWrite(Dir2L, LOW);

  digitalWrite(Dir1R, LOW);
  digitalWrite(Dir2R, LOW);
}


void Pryznachennya_pins() //Ідентифікація пінів
{

pinMode(Dir1L, OUTPUT);
pinMode(Dir2L, OUTPUT);

pinMode(Dir1R, OUTPUT);
pinMode(Dir2R, OUTPUT);

pinMode(Pin_Speed_L, OUTPUT); // давач швидкості лівого двигуна
pinMode(Pin_Speed_R, OUTPUT); // давач швидкості правого двигуна
}



void setup() {
  
  Pryznachennya_pins();

  Forward();  // Рух вперед - час 1 секунди
  delay(1000);

  Stop();  // Зупинка - 1 секунда
  delay(1000);

  Reverse(); // Рух назад - час 1 секунди
  delay(1000);

  Stop();  // Зупинка - 1 секунда
  delay(1000);


  Right();
  delay(1000);

  Stop();  // Зупинка - 1 секунда
  delay(1000);

  Left();
  delay(1000);

  Stop();  // Зупинка - 1 секунда
 
pinMode(D1, INPUT);
pinMode(D2, INPUT);
pinMode(D3, INPUT);
pinMode(D4, INPUT);
pinMode(D5, INPUT);

Serial.begin(9600);



  // put your setup code here, to run once:

}


void Ruh_robot()
{
  Znachennya_Davach();
    if(Davach[3]==HIGH) // Центральний давач бачить чорний трек (повністю або частоково)
      {
        if (Davach[2]==LOW && Davach[4]==LOW)
              {Forward();
              Set_speed(Speed5, Speed5);              
               }

      else if (Davach[4]==HIGH && Davach[5]==LOW)
        {Right();
        Set_speed(Speed4, Speed4-Speed4*0.75);
        }

      else if (Davach[2]==HIGH && Davach[1]==LOW)
          {Left();
          Set_speed(Speed4-Speed4*0.75, Speed4);
          }
      } 
    else if (Davach[3]==LOW) // Центральний давач на білому фоні (повністю поза треком)
      {
        if (Davach[4]==HIGH && Davach[5]==LOW)
          {Right();
          Set_speed(Speed3, Speed3-Speed3*0.75);
           }

        if (Davach[2]==HIGH && Davach[1]==LOW)
          {Left();
          Set_speed(Speed3-Speed3*0.75, Speed3);
           }
      }
        if (Davach[4]==HIGH && Davach[5]==HIGH)  
           {Right();
           Set_speed(Speed2, Speed2-Speed2*0.75);
           }

        if (Davach[2]==HIGH && Davach[1]==HIGH)  
           {Left();
           Set_speed(Speed2-Speed2*0.75, Speed2);
           }

        if (Davach[4]==LOW && Davach[5]==HIGH)  
           {Right();
           Set_speed(Speed1, Speed1-Speed1*0.75);
           }

        if (Davach[2]==LOW && Davach[1]==HIGH)  
           {Left();
           Set_speed(Speed1-Speed1*0.75, Speed1);
           }

      else 
      
      {
        Reverse();
        Set_speed(Speed1, Speed1);
      }

      }    



void loop() {

    Ruh_robot();

}