#include<LiquidCrystal.

h>
#include<Wire.h>
#include<SoftwareSerial.h>
#include<TimerOne.h>

LiquidCrystal lcd(5,6,16,17,18,19);
SoftwareSerial hc(14,15);

const int trig1 = 22;

int ENA = 13;

int phaA1 = 2;
int phaB1 = 4;
int phaA2 = 3;
int phaB2 = 7;

const int KHOIDONG = A0;

int kcTruoc;
int kcSau;
int kcTrai;
int kcPhai;

int potTruoc;
int potSau;

int xung1 = 0;
int xung2 = 0;

double tam;
float T,xung;
float tocdo, tocdodat;
double E,E1,E2;
double alpha, beta, gamma, Kp, Ki, Kd;
double Output, LastOutput;

int a = 200;
int b = 200;

int n;
char command;
String string;

void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(trig1,OUTPUT);
pinMode(trig2,OUTPUT);
pinMode(trig3,OUTPUT);
pinMode(echo1,INPUT);
pinMode(echo2,INPUT);
pinMode(echo3,INPUT);

pinMode(ENA,OUTPUT);
pinMode(tien1,OUTPUT);
pinMode(lui1,OUTPUT);
pinMode(tien2,OUTPUT);
pinMode(lui2,OUTPUT);
pinMode(ENB,OUTPUT);

pinMode(KHOIDONG,INPUT_PULLUP);
pinMode(TANG,INPUT_PULLUP);
pinMode(GIAM,INPUT_PULLUP);
pinMode(RUN,INPUT_PULLUP);

attachInterrupt(phaA1+2,vitriE1,FALLING);
attachInterrupt(phaA2+2,vitriE2,FALLING);
pinMode(phaA1,INPUT_PULLUP);
pinMode(phaA2,INPUT_PULLUP);
pinMode(phaB1,INPUT_PULLUP);
pinMode(phaB2,INPUT_PULLUP);

TCCR2B = TCCR2B & B11111000 | B00000001; // cho tần số PWM là 31372,55 Hz

lcd.begin(20,4);
lcd.setCursor(1,0);
lcd.print("---ROBOT HUT BUI---");
delay(500);
lcd.setCursor(0,1);
lcd.print("Bui Ngoc Minh-LD");
delay(200);
lcd.setCursor(0,2);
lcd.print("Hoang Van Hieu");
delay(200);
lcd.setCursor(0,3);
lcd.print("Hoang Van Nam");
delay(1000);
lcd.clear();
kiemtra();
}
void tien()
{
kcTruoc = dokcTr();
if(kcTruoc<25 && potTruoc>615 && potTruoc<=1024)
{
analogWrite(ENA,a);
 analogWrite(ENB,b);
digitalWrite(tien1,HIGH);
digitalWrite(tien2,HIGH);
digitalWrite(lui1,LOW);
digitalWrite(lui2,LOW);
}
else
{
delay(100);
}
}
void dungdongco()
{
analogWrite(ENA,a);
analogWrite(ENB,b);
digitalWrite(tien1,LOW);
digitalWrite(lui1,LOW);
digitalWrite(tien2,LOW);
digitalWrite(lui2,LOW);
}
void lui()
{
analogWrite(ENA,a);
analogWrite(ENB,b);
dungdongco();
digitalWrite(lui1,HIGH);
digitalWrite(lui2,HIGH);
}
void retrai()
{
 analogWrite(ENA,a);
analogWrite(ENB,b);
dungdongco();
digitalWrite(lui2,HIGH);
delay(3500);
digitalWrite(lui2,LOW);
}
void rephai()
{
analogWrite(ENA,a);
analogWrite(ENB,b);
dungdongco();
digitalWrite(lui1,HIGH);
delay(3500);
digitalWrite(lui1,LOW);
}
int dokcTrai()
{
kcTrai = 0;
digitalWrite(trig1,LOW);
delayMicroseconds(2);
digitalWrite(trig1,HIGH);
delayMicroseconds(10);
digitalWrite(trig1,LOW);
unsigned long t1 = pulseIn(echo1,HIGH);
kcTrai = t1/2/29.412;
return kcTrai;
}
int dokcTr()
{
 kcTruoc = 0;
digitalWrite(trig2,LOW);
delayMicroseconds(2);
digitalWrite(trig2,HIGH);
delayMicroseconds(10);
unsigned long t2 = pulseIn(echo2,HIGH);
kcTruoc = t2/2/29.412;
return kcTruoc;
}
int dokcPhai()
{
kcPhai = 0;
digitalWrite(trig3,LOW);
delayMicroseconds(2);
digitalWrite(trig3,HIGH);
delayMicroseconds(10);
unsigned long t3 = pulseIn(echo3,HIGH);
kcPhai = t3/2/29.412;
return kcPhai;
}
void vitriE1()
{
if(digitalRead(phaB1)==LOW)
{
xung1++;
}
else
{
xung1--;
}
}
void vitriE2()
{
if(digitalRead(phaB2) == LOW)
{
xung2++;
}
else
{
xung2--;
}
}
void kiemtra()
{
if(kcTrai>0 && kcTrai <25)
{
n=1; //re phai
}
if(kcPhai>0 && kcPhai<25)
{
n=0; // re trai
}
}
void dichuyen()
{
if( n==1 )
{
tien();
rephai();
tien();
 delay(2000);
dungdongco();
rephai();
n=0;
}
if(n==0)
{
tien();
retrai();
tien();
delay(2000);
dungdongco();
retrai();
n=1;
}
}
void PID()
{
tocdo=((xung/210)*(1/T)*60);
xung=0;
E = tocdodat - tocdo;
alpha = 2*T*Kp + Ki*T*T + 2*Kd;
beta = T*T*Ki - 4*Kd - 2*T*Kp;
gamma = 2*Kd;
Output = (alpha*E + beta*E1 + gamma*E2 + 2*T*LastOutput)/(2*T);
LastOutput = Output;
E2=E1;
E1=E;
if(Output>255)
Output =255;
 if(Output<0)
Output =0;
}
void loop() {
// put your main code here, to run repeatedly:
if(digitalRead(KHOIDONG)==HIGH)
{
dungdongco();
if(Serial.available() > 0)
{
string = "";
}
while(Serial.available() > 0)
{
command = ((byte)Serial.read());
if(command == ':')
{
break;
}
else
{
string += command;
}
delay(100);
}
if(string == "G") // Kiểm tra nếu như dữ liệu gửi đến là "TIEN"
{
dungdongco();
analogWrite(ENA,b);
analogWrite(ENB,a);
 digitalWrite(tien1,HIGH);
digitalWrite(tien2,HIGH);
}
if(string == "B") // Kiểm tra nếu như dữ liệu gửi đến là "LUI"
{
dungdongco();
analogWrite(ENA,b);
analogWrite(ENB,a);
digitalWrite(lui1,HIGH);
digitalWrite(lui2,HIGH);
}
if(string == "S") // Kiểm tra nếu như dữ liệu gửi đến là "S"
{
lcd.setCursor(1,0);
lcd.print("---BLUETOOTH CONNECTED---");

}
if(string == "D") // Kiểm tra nếu như dữ liệu gửi đến là "S"
{
dungdongco();
}
if(string == "L") // Kiểm tra nếu như dữ liệu gửi đến là "LUI"
{
dungdongco();
analogWrite(ENA,a);
analogWrite(ENB,b);
digitalWrite(lui2,HIGH);
}
if(string == "R") // Kiểm tra nếu như dữ liệu gửi đến là "LUI"
{
 dungdongco();
analogWrite(ENA,a);
analogWrite(ENB,b);
digitalWrite(lui1,HIGH);
}
}
else
{
lcd.setCursor(0,0);
lcd.print("START");
delay(200);
lcd.setCursor(0,1);
lcd.print(a);
if(digitalRead(TANG) == LOW)
{
if( a>0 && a<255 )
{
a = a+10;
}
if(a >= 255)
{
a = 255;
}
if( a <= 0)
{
a=0;
}
}
if(digitalRead(GIAM) == LOW)
{
 if( a>0 && a<255 )
{
a = a-10;
}
if( a >= 255 )
{
a=255;
}
if( a <= 0)
{
a = 0;
}
}
if(digitalRead(RUN) == LOW)
{
lcd.setCursor(7,0);
lcd.print("RUN");
delay(200);
lcd.setCursor(12,0);
lcd.print("E1:");
lcd.setCursor(12,1);
lcd.print("E2:");
lcd.setCursor(15,0);
lcd.print(xung1);
lcd.setCursor(15,1);
lcd.print(xung2);
b = a;
kcTruoc = dokcTr();
kcTrai = dokcTrai();
kcPhai = dokcPhai();
 lcd.setCursor(0,2);
lcd.print(kcTruoc);
lcd.setCursor(5,2);
lcd.print(kcSau);
lcd.setCursor(10,2);
lcd.print(kcTrai);
lcd.setCursor(15,2);
lcd.print(kcPhai);
lcd.setCursor(0,3);
lcd.print("X:");
lcd.setCursor(7,3);
lcd.print("Y:");
lcd.setCursor(13,3);
lcd.print("Z:");
potTruoc = analogRead(cbhntr);
dichuyen();
}
}
}