#define M11 4
#define M12 5
#define M21 8
#define M22 9
#define left_line 6
#define right_line 7

#include <Ultrasonic.h>
Ultrasonic dalnomer (12, 13);

int right, left, state, i = 0;
float distance;


void setup() {
  pinMode(M11, OUTPUT); pinMode(M12, OUTPUT);
  pinMode(M21, OUTPUT); pinMode(M22, OUTPUT);
   
  Serial.begin(115200);
}

void loop() {
  left = digitalRead(left_line);
  right = digitalRead(right_line);
  distance = dalnomer.read();

  output_serial(left, right, distance);

  if(distance < 20)                {Stop();}
  else if(left == 0)               {Left(255);}
  else if(right == 0)              {Right(255);}
  else if(left == 1 && right == 1) {Forward(255);}

}

void output_serial(int dl, int dr, int daln){
  Serial.print("Left = " + String(dl));
  Serial.print(" / Right = " + String(dr));
  Serial.println(" / Dist = " + String(daln));
}

void Forward(int Speed){
  analogWrite(M12,LOW);
  analogWrite(M21,LOW);
  analogWrite(M11,Speed);
  analogWrite(M22,Speed);
}

void Back(int Speed){
  analogWrite(M12,Speed);
  analogWrite(M21,Speed);
  analogWrite(M11,LOW);
  analogWrite(M22,LOW);
}

void Left(int Speed){
  analogWrite(M12,LOW);
  analogWrite(M21,Speed);
  analogWrite(M11,Speed);
  analogWrite(M22,LOW);
}

void Right(int Speed){
  analogWrite(M12,Speed);
  analogWrite(M21,LOW);
  analogWrite(M11,LOW);
  analogWrite(M22,Speed);
}

void Stop(){
  analogWrite(M12,LOW);
  analogWrite(M21,LOW);
  analogWrite(M11,LOW);
  analogWrite(M22,LOW);
}