///Piny lewego silnika
#define INA_1 12
#define INB_1 13
#define PWM_1 35

///Piny prawego silnika
#define INA_2 5
#define INB_2 6
#define PWM_2 36
 


void setup()
{

//Ustawienie pinu lewego silnika jako wyjściowego (OUTPUT)
pinMode(INA_1,OUTPUT); 
pinMode(INB_1,OUTPUT);

//Ustawienie pinu prawego silnika jako wyjściowego (OUTPUT)
pinMode(INA_2,OUTPUT);
pinMode(INB_2,OUTPUT);
}


void loop()
{

//Ruch do przodu przez 5 sekund
move_forward();
delay(5000);

//Zatrzymanie się na 1 sekundę
stop();
delay(1000);

//Ruch do tyłu przez 5 sekund
move_backward();
delay(5000);

//Zatrzymanie się na 1 sekundę
stop();
delay(1000);

//Ruch w lewo przez 5 sekund
move_left();
delay(5000);

//Zatrzymanie się na 1 sekundę
stop();
delay(1000);

//Ruch w prawo przez 5 sekund
move_right();
delay(5000);  

//Zatrzymanie się na 1 sekundę
stop();
delay(1000);

}
 
void move_forward()
{
//Ustawienie obrotu CW lewego silnika i CCW prawego silnika
 //Lewy silnik

digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,LOW);
analogWrite(PWM_1,255);

//Prawy silnik
digitalWrite(INA_2,LOW);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,255);  

}



void move_left()
{
 
//Lewy silnik
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,0);

//Prawy silnik
digitalWrite(INA_2,LOW);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,255);  


}

void move_right()
{

//Lewy silnik
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,LOW);
analogWrite(PWM_1,255);

//Prawy silnik
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,0);  
}

void stop()
{

//Lewy silnik
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,0);

//Prawy silnik
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,0);  
}

/////////////////////////////////////////////////

void move_backward()

{

//Lewy silnik
digitalWrite(INA_1,LOW);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,255);

//Prawy silnik
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,LOW);
analogWrite(PWM_2,255);  
}
