//Sterownik
//  D3 - PWMA
//  D5 - AIN1
//  D4 - AIN2
//  D6 - BIN1
//  D7 - BIN2
//  D9 - PWMB
//Odleglosc
//  D12 - ECHO
//  D11 - TRIG
//Zblizenie
//  PRZOD :
//    A1 - A0
//    D10 - D0
//  TYL:
//    A0 - A0
//    D2 - D0
#include<Wire.h>

#define PWMA 3
#define AIN1 5
#define AIN2 4
#define BIN1 6
#define BIN2 7
#define PWMB 9
#define ECHO 12
#define TRIG 11
#define xA0 A1
#define xD0 10
#define yA0 A0
#define yD0 2

int CM;
long CZAS;
int TCRT5000x1;
int TCRT5000x2;

void setup() {
  pinMode(PWMA,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(PWMB,OUTPUT);
  pinMode(ECHO,OUTPUT);
  pinMode(TRIG,OUTPUT);
  pinMode(xA0,OUTPUT);
  pinMode(xD0,OUTPUT);
  pinMode(yA0,OUTPUT);
  pinMode(yD0,OUTPUT);
  Serial.begin(9600);
  Serial.println("Debug: ");
  digitalWrite(PWMA,LOW);
  digitalWrite(PWMB,LOW);

}

void loop() {
  lewo();
  for(int i=10;i<500;i+=10)
  {
  pomiar_odleglosci();
  odbicie();
  delay(10);
  }
  prawo();
  for(int i=10;i<500;i+=10)
  {
  pomiar_odleglosci();
  odbicie();
  delay(10);
  }

}

void pomiar_odleglosci()
{
  digitalWrite(TRIG, LOW);        //ustawienie stanu wysokiego na 2 uS - impuls inicjalizujacy - patrz dokumentacja
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);       //ustawienie stanu wysokiego na 10 uS - impuls inicjalizujacy - patrz dokumentacja
  delayMicroseconds(15);
  digitalWrite(TRIG, LOW);
  digitalWrite(ECHO, HIGH);
  CZAS = pulseIn(ECHO, HIGH);
  CM = CZAS / 58;                //szerokość odbitego impulsu w uS podzielone przez 58 to odleglosc w cm - patrz dokumentacja
  Serial.print("Odleglosc[cm]: ");
  Serial.println(CM);
}

void przod()
{
  digitalWrite(PWMA,HIGH);
  digitalWrite(PWMB,HIGH);

  digitalWrite(AIN1,HIGH);
  digitalWrite(AIN2,LOW);
  digitalWrite(BIN1,HIGH);
  digitalWrite(BIN2,LOW);
}

void tyl()
{
  digitalWrite(PWMA,HIGH);
  digitalWrite(PWMB,HIGH);

  digitalWrite(AIN1,LOW);
  digitalWrite(AIN2,HIGH);
  digitalWrite(BIN1,LOW);
  digitalWrite(BIN2,HIGH);
}

void stoj()
{
  digitalWrite(PWMA,LOW);
  digitalWrite(PWMB,LOW);

  digitalWrite(AIN1,LOW);
  digitalWrite(AIN2,LOW);
  digitalWrite(BIN1,LOW);
  digitalWrite(BIN2,LOW);
}

void lewo() // albo prawo, uj wie XD
{
  digitalWrite(PWMA,HIGH);
  digitalWrite(PWMB,HIGH);

  digitalWrite(AIN1,HIGH);
  digitalWrite(AIN2,LOW);
  digitalWrite(BIN1,LOW);
  digitalWrite(BIN2,HIGH);
}

void prawo()  // a moze lewo no nie wiem xD
{
  digitalWrite(PWMA,HIGH);
  digitalWrite(PWMB,HIGH);

  digitalWrite(AIN1,LOW);
  digitalWrite(AIN2,HIGH);
  digitalWrite(BIN1,HIGH);
  digitalWrite(BIN2,LOW);
}

void odbicie()  // niby powyzej 500 to czornyj linio
{
  TCRT5000x1=analogRead(xA0);
  TCRT5000x2=analogRead(yA0);
  Serial.print("Pomiar z TRCT5000(PRZOD/TYL): ");
  Serial.print(xA0);
  Serial.print(" / ");
  Serial.println(yA0);
}