#include <SoftwareSerial.

h>
#define pwma  9  
#define pwmb  3  
#define in1   8  
#define in2   7  
#define in3   5  
#define in4   4  
#define trig1   A0
#define echo1   A1
#define trig2   A6
#define echo2   A7
#define trig3   A2
#define echo3   A3
#define str1    A4
#define str2    A5
int allow_distance_front = 25;
int allow_distance_corner = 20;
int speed_begin=120;            
boolean stt=0;          
int front_distance = 0;
int left_distance = 0;  
int right_distance = 0;
void chaylui(uint8_t spd){
    analogWrite(pwma,spd);
    analogWrite(pwmb,spd+50);
    digitalWrite(in1,1);
    digitalWrite(in2,0);
    digitalWrite(in3,1);
    digitalWrite(in4,0);
  }
void chaythang(uint8_t spd){
    analogWrite(pwma,spd);
    analogWrite(pwmb,spd+50);
    digitalWrite(in1,0);
    digitalWrite(in2,1);
    digitalWrite(in3,0);
    digitalWrite(in4,1);
  }
void phai(uint8_t spd){
    analogWrite(pwma,spd);
    analogWrite(pwmb,spd+40);
    digitalWrite(in1,0);
    digitalWrite(in2,1);
    digitalWrite(in3,1);
    digitalWrite(in4,0);
  }
void trai(uint8_t spd){
    analogWrite(pwma,spd);
    analogWrite(pwmb,spd+40);
    digitalWrite(in1,1);
    digitalWrite(in2,0);
    digitalWrite(in3,0);
    digitalWrite(in4,1);
  }
void dung(){
    digitalWrite(in1,1);
    digitalWrite(in2,1);
    digitalWrite(in3,1);
    digitalWrite(in4,1);
    analogWrite(pwma,255);
    analogWrite(pwmb,255);
}
int objectDis (int trg, int ech)
{
  unsigned long duration;
  int distance;
 
  digitalWrite(trg,0);
  delayMicroseconds(2);
  digitalWrite(trg,1);
  delayMicroseconds(10);
  digitalWrite(trg,0);
 
  duration = pulseIn(ech,HIGH);
  distance = int(duration/2/29.412);
  return distance;
}
void setup()
{
  Serial.begin(9600);
  pinMode(pwmb,OUTPUT);
  pinMode(pwma,OUTPUT);
  pinMode(in1,OUTPUT);  
  pinMode(in2,OUTPUT);
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  pinMode(trig1,OUTPUT);
  pinMode(echo1,INPUT);
  pinMode(trig2,OUTPUT);
  pinMode(echo2,INPUT);
  pinMode(trig3,OUTPUT);
  pinMode(echo3,INPUT);
  pinMode(str1,INPUT);
  pinMode(str2,INPUT);
  if (left_distance <= allow_distance_corner) stt =0;
  if (right_distance <= allow_distance_corner) stt =1;
  chaythang(150);
  delay(100);
  chaythang(speed_begin);
  delay(200);
}
void quaytrai90()
{
     trai(120);
     delay(800);
     dung();
  }
void quayphai90()
{
     phai(120);
     delay(800);
     dung();
  }
void robot_avoider(){
  chaythang(speed_begin);
  if (front_distance > allow_distance_front)
    {
      chaythang(speed_begin);
    }
  else if (front_distance <= allow_distance_front){
      dung();
      delay(1000);
      chaylui(120);
      delay(500);
      if (stt==0){
          quayphai90();
          delay(1000);
          chaythang(speed_begin+20);
          delay(100);
          chaythang(speed_begin);
          delay(500);
        }
      if (stt==1) {
          quaytrai90();
          delay(1000);
          chaythang(speed_begin+20);
          delay(100);
          chaythang(speed_begin);
          delay(500);
        }
        stt = !stt;
        chaythang(speed_begin);
        delay(200);
    }
     if(digitalRead(str1) == 1)
    {
          quaytrai90();
          delay(1000);
          chaythang(speed_begin+20);
          delay(100);
          chaythang(speed_begin);
          delay(500);
    }
    if(digitalRead(str2) == 1)
    {
          quayphai90();
          delay(1000);
          chaythang(speed_begin+20);
          delay(100);
          chaythang(speed_begin);
          delay(500);
    }
    if(digitalRead(str1) == 1 && digitalRead(str2) == 1)
    {
      dung();
      delay(1000);
      chaylui(120);
      delay(500);
      quayphai90();
      delay(1000);
      chaythang(speed_begin+20);
      delay(100);
      chaythang(speed_begin);
      delay(500);
    }
}
void loop() {
  // put your main code here, to run repeatedly:
  front_distance = objectDis(trig2,echo2);
  left_distance  = objectDis(trig1,echo1);
  right_distance = objectDis(trig3,echo3);
  robot_avoider();
  Serial.println(front_distance);
  Serial.println(left_distance);
  Serial.println(right_distance);
  delay(100);
}