#include <Servo.

h>
#include <SoftwareSerial.h>
#include <TinyGPS++.h>
#include<Wire.h>
TinyGPSPlus gps;
float latf,lngf;
long latn,lngn;
long cht,dvt,chn,dvn;

String textMessage;
char luu[100];
int chay;

long chayr,ttr;
Servo myservo; // create servo object to control a servo
int pos = 0; // variable to store the servo position

const int trig = 7;

const int echo = 4;
int enbA=3;
int in1 = 5;
int in2 = 6;
int in3 = 9;
int in4 = 10;
int enbB=11;
int dongcoservo = 8;

int gioihan = 30;//khoảng cách nhận biết vật

unsigned long thoigian;

int khoangcach;
int khoangcachtrai, khoangcachphai;
///////////////
void xoa_tin();
void displayInfo();
void sendSMS();
///////////////
void dokhoangcach();
void dithang(int duongdi);
void disangtrai();
void disangphai();
void dilui();
void resetdongco();
void quaycbsangphai();
void quaycbsangtrai();

void setup() {
// put your setup code here, to run once:
myservo.attach(dongcoservo);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);

pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(enbA, OUTPUT);
pinMode(enbB, OUTPUT);
 digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
digitalWrite(enbA, LOW);
digitalWrite(enbB, LOW);
myservo.write(90);
delay(500);
Serial.begin(9600);
////////////////SIM+GPS
Serial1.begin(9600);
Serial2.begin(9600);

Serial.print("Sim800L ready...");
delay(15000);
Serial1.print("AT+CMGF=1\r\n");
delay(100);
Serial1.print("AT+CNMI=2,2,0,0,0\r\n");
delay(100);
chay=0;
Serial1.println("AT+CMGF=1");
delay(1000);
Serial1.println("AT+CMGS=\"+84328941512\"\r\n");
delay(1000);
Serial1.println("KHOI DONG ");
delay(1000);
Serial1.println((char)26);
delay(1000);
Serial.print("OK OK ");
chay=0;
///////////////SIM+GPS

void loop()
{
khoangcach = 0;
dokhoangcach();
Serial.println(khoangcach);
if (khoangcach > gioihan || khoangcach == 0)
{
dithang();
Serial.println("Di toi");
}
else
{
dunglai();delay(300);
quaycbsangtrai();delay(1000);
dokhoangcach();
khoangcachtrai = khoangcach;
Serial.println(khoangcachtrai);
delay(300);
quaycbsangphai();delay(1000);
dokhoangcach();
khoangcachphai = khoangcach;
Serial.println(khoangcachphai);
if (khoangcachphai < 25 && khoangcachtrai < 25) {
dilui();delay(300);dunglai();delay(300);
disangtrai();delay(400);
 //Serial.println("Di sang phai");
//Serial.println("Di lui");
//Serial.println("dung lai");
}
else
{
if (khoangcachphai >= khoangcachtrai)
{ dilui();delay(300);dunglai();delay(300);
disangphai();
Serial.println("Di lui");
Serial.println("Di sang phai");
delay(400);dunglai();delay(300);
}
if (khoangcachphai < khoangcachtrai)
{ dilui();delay(300);dunglai();delay(300);
disangtrai();
Serial.println("Di sang trai");
delay(400);dunglai();delay(300);
}
}
}
///////////////////GPS+MODULESIM
chay++;
if(chay<100)
{
if(Serial1.available()>0)
{
textMessage = Serial1.readString();
delay(10);
Serial.println(Serial1.readString());
}
if(textMessage.indexOf("KTR")>=0)
{
sendSMS();
textMessage = "";
xoa_tin();
}
}
else
{
chay=0;
while(Serial2.available()>0)
if(gps.encode(Serial2.read()))
displayInfo();
// hienthi();
}
/////////////////////////////
}
void dithang()
{
analogWrite(enbA, 150);
analogWrite(enbB, 150);
digitalWrite(in1, 0);
digitalWrite(in2, 1);
digitalWrite(in3, 0);
digitalWrite(in4, 1);

}
void dunglai(){
analogWrite(enbA, 0);
analogWrite(enbB, 0);
digitalWrite(in1, 0);
digitalWrite(in2, 0);
digitalWrite(in3, 0);
digitalWrite(in4, 0);
}

void disangtrai()
{
analogWrite(enbA, 150);
analogWrite(enbB, 0);
digitalWrite(in1, 0);
digitalWrite(in2, 1);
digitalWrite(in3, 0);
digitalWrite(in4, 0);

}
void disangphai()
{
analogWrite(enbA, 0);
analogWrite(enbB, 150);
digitalWrite(in1, 0);
digitalWrite(in2, 0);
digitalWrite(in3, 0);
digitalWrite(in4, 1);

void dilui()
{
analogWrite(enbA, 150);
analogWrite(enbB, 150);
digitalWrite(in1, 1);
digitalWrite(in2, 0);
digitalWrite(in3, 1);
digitalWrite(in4, 0);

void dokhoangcach()
{

digitalWrite(trig, 0);
delayMicroseconds(2);
digitalWrite(trig, 1);
delayMicroseconds(10);
digitalWrite(trig, 0);

// Đo độ rộng xung HIGH ở chân echo.

thoigian = pulseIn(echo, HIGH);

khoangcach = thoigian / 2 / 29.412;

}

void quaycbsangtrai()
{
myservo.write(150); // tell servo to go to position in variable
'pos'
delay(1000);
dokhoangcach();
myservo.write(90); // tell servo to go to position in variable 'pos'
}
void quaycbsangphai()
{
myservo.write(30); // tell servo to go to position in variable 'pos'
delay(1000);
dokhoangcach();
myservo.write(90); // tell servo to go to position in variable 'pos'
}
void resetservo()
{
myservo.write(90);
}
//////////////
void sendSMS()
{
sprintf(luu, "http://www.google.com/maps/?q=%ld.%ld,%ld.%ld",cht,dvt,chn,dvn);
Serial1.println("AT+CMGF=1");
delay(1000);
Serial1.print("AT+CMGS=\"+84328941512\"\r\n"); //doi sdt can gui toi di
0347145673
delay(1000);
Serial1.print(luu);
delay(1000);
Serial1.println((char)26);
delay(1000);
}

void displayInfo()
{
if(gps.location.isValid())
{
Serial.print(gps.location.lat(), 6);
Serial.print(F(","));
Serial.println(gps.location.lng(), 6);
latf=gps.location.lat()*1000000; // vi do
lngf=gps.location.lng()*1000000; // kinh di

latn=latf;
lngn=lngf;
chn=lngn/1000000;
dvn=lngn%1000000;
cht=latn/1000000;
dvt=latn%1000000;

}
}

void xoa_tin()
{
 Serial1.print("AT+CMGF=1\r");
delay(100);
Serial1.print("AT+CNMI=2,2,0,0,0\r");
delay(100);
}
//////////////