#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);
}