#define motor1_a 12
#define motor1_b 11
#define motor1_pwm 6
#define motor2_a 8
#define motor2_b 7
#define motor2_pwm 5

#define servo 9

#define ultrazvuk_1 A2
#define ultrazvuk_2 A3

#include <Servo.h>
Servo myservo;

#include "Ultrasonic.h"

Ultrasonic ultrasonic(ultrazvuk_1, ultrazvuk_2);
int distance;

void setup() {
Serial.begin(9600);
myservo.attach(servo);
myservo.write(90); 
pinMode(motor1_a, OUTPUT);
pinMode(motor1_b, OUTPUT);
pinMode(motor1_pwm, OUTPUT);
}
void loop() {
auto_dopredu(255);
myservo.write(0);
vzdalenost();
auto_stop();
myservo.write(90);
vzdalenost();
auto_dozadu(255);
myservo.write(180);
vzdalenost();
auto_stop();
myservo.write(90);
vzdalenost();
}

void vzdalenost(){
  for (int pp=0;pp<4;pp++){
   distance = ultrasonic.read();
  
  Serial.print("Distance in CM: ");
  Serial.println(distance);
    delay(1000);
  }
}

void auto_dopredu(int rychlost){
  motor1_dopredu(rychlost);
  motor2_dopredu(rychlost);
}


void auto_dozadu(int rychlost){
  motor1_dozadu(rychlost);
  motor2_dozadu(rychlost);
}


void auto_stop(){
  motor1_stop();
  motor2_stop();
}

void motor1_dopredu(int rychlost){
  digitalWrite(motor1_a, HIGH);
  digitalWrite(motor1_b, LOW);
  analogWrite(motor1_pwm, rychlost);
}


void motor1_dozadu(int rychlost){
  digitalWrite(motor1_b, HIGH);
  digitalWrite(motor1_a, LOW);
  analogWrite(motor1_pwm, rychlost);
}


void motor1_stop(){
  digitalWrite(motor1_a, LOW);
  digitalWrite(motor1_b, LOW);
  analogWrite(motor1_pwm, 0);
}


void motor2_dopredu(int rychlost){
  digitalWrite(motor2_a, HIGH);
  digitalWrite(motor2_b, LOW);
  analogWrite(motor2_pwm, rychlost);
}


void motor2_dozadu(int rychlost){
  digitalWrite(motor2_b, HIGH);
  digitalWrite(motor2_a, LOW);
  analogWrite(motor2_pwm, rychlost);
}


void motor2_stop(){
  digitalWrite(motor2_a, LOW);
  digitalWrite(motor2_b, LOW);
  analogWrite(motor2_pwm, 0);
}