//Buton Stanga

short butonSus = 6;

//Buton Inainte
short butonMijloc = 10;

//Buton Dreapta
short butonJos = 11;

///////////////////////STRATEGII PENTRU BUTOANE

//short butonStAtac = 6;
//short butonStrAparare = 10;
//short butonStSpeciala = 11;

//Motor A
short motorDreaptaInainte = 8;
short pwmDreapta = 9;
short motorDreaptaSpate = 7;

//Motor B
short motorStangaInainte = 13;
short pwmStanga = 5;
short motorStangaSpate = 12;

//Timp incepere batalie (5 secunde)

short timp = 4900;

//Timp invartire
short timp2 = 150;

//Senzori
short SenzorDreapta = A0;
short SenzorStanga = A1;

//State senzori
short senzorStateD;
short senzorStateS;

//Distanta senzorului (Recomandat 210)

short Distanta = 300;

//Putere
short p;

//Buton State
short butonStateSus = 0;
short butonStateMijloc = 0;
short butonStateJos = 0;

void setup()
{
Serial.begin(9600);
pinMode(6, INPUT);
pinMode(10, INPUT);
pinMode(11, INPUT);
pinMode(12, OUTPUT);
pinMode(5, OUTPUT);
pinMode(13, OUTPUT);
pinMode(7, OUTPUT);
 pinMode(9, OUTPUT);
pinMode(8, OUTPUT);
pinMode(SenzorDreapta, INPUT);
pinMode(SenzorStanga, INPUT);
digitalWrite(butonSus, 1);
digitalWrite(butonMijloc, 1);
digitalWrite(butonJos, 1);

while(butonStateSus==0 && butonStateMijloc==0 && butonStateJos==0)

{
if(digitalRead(butonSus)==0) butonStateSus=1;
if(digitalRead(butonMijloc)==0) butonStateMijloc=1;
if(digitalRead(butonJos)==0) butonStateJos=1;
}

delay(timp);
if (butonStateSus==1){p=200; StangaRapid(); delay(timp2);p=100;StangaRapid();}
if (butonStateJos==1){p=200; DreaptaRapid(); delay(timp2);p=100;DreaptaRapid();}
}

void loop()
{
citeste();
lupta();
}

void citeste()
{
SenzorDreapta=analogRead(0);
SenzorStanga=analogRead(1);
}

void lupta()
{
if (SenzorStanga > Distanta && SenzorDreapta > Distanta) {p=255; Inainte();}
if (SenzorStanga > Distanta && SenzorDreapta < Distanta){p=100; StangaRapid();}
if (SenzorStanga < Distanta && SenzorDreapta > Distanta){p=100; DreaptaRapid();}
}

void Inainte()
{
digitalWrite(motorStangaInainte, 1);
digitalWrite(motorStangaSpate, 0);
analogWrite(pwmStanga, p);
digitalWrite(motorDreaptaInainte, 1);
digitalWrite(motorDreaptaSpate, 0);
analogWrite(pwmDreapta, p);
}

void Stanga()
{
digitalWrite(motorStangaInainte, 0);
digitalWrite(motorStangaSpate, 0);
analogWrite(pwmStanga, 0);
digitalWrite(motorDreaptaInainte, 1);
digitalWrite(motorDreaptaSpate, 0);
analogWrite(pwmDreapta, p);
}
void Dreapta()
{
digitalWrite(motorStangaInainte, 1);
digitalWrite(motorStangaSpate, 0);
analogWrite(pwmStanga, p);
digitalWrite(motorDreaptaInainte, 0);
digitalWrite(motorDreaptaSpate, 0);
analogWrite(pwmDreapta, 0);
}

void StangaRapid()
{
digitalWrite(motorStangaInainte, 0);
digitalWrite(motorStangaSpate, 1);
analogWrite(pwmStanga, 0);
digitalWrite(motorDreaptaInainte, 1);
digitalWrite(motorDreaptaSpate, 0);
analogWrite(pwmDreapta, p);
}

void DreaptaRapid()
{
digitalWrite(motorStangaInainte, 1);
digitalWrite(motorStangaSpate, 0);
analogWrite(pwmStanga, 0);
digitalWrite(motorDreaptaInainte, 0);
digitalWrite(motorDreaptaSpate, 1);
analogWrite(pwmDreapta, p);
}