#include <SoftwareSerial.h>
SoftwareSerial BT(6,7);
int setdis;
int dir;
int DATA;
float distance;
int disX10;
float ultrasonic_distance_11_10() {
digitalWrite(11, LOW);
digitalWrite(10, LOW);
delayMicroseconds(5);
digitalWrite(11, HIGH);
delayMicroseconds(10);
digitalWrite(11, LOW);
unsigned long sonic_duration = pulseIn(10, HIGH);
float distance_cm = (sonic_duration / 2.0) / 29.1;
return distance_cm;
}
void Go() {
digitalWrite(12,HIGH);
digitalWrite(13,LOW);
dir = 1;
}
void Stop() {
digitalWrite(12,LOW);
digitalWrite(13,LOW);
dir = 0;
}
void Back() {
digitalWrite(12,LOW);
digitalWrite(13,HIGH);
dir = 2;
}
void setup()
{
BT.begin(9600);
pinMode(8, INPUT_PULLUP);
pinMode(9, INPUT_PULLUP);
pinMode( 11 , OUTPUT);
pinMode( 10 , INPUT);
setdis = 0;
dir = 0;
DATA = 0;
distance = 0;
Stop();
pinMode(12, OUTPUT);
pinMode(13, OUTPUT);
}
void loop()
{
if (digitalRead(8) == 0 && dir == 1) {
Stop();
}
if (digitalRead(9) == 0 && dir == 2) {
Stop();
}
distance = ultrasonic_distance_11_10( );
disX10=distance*10;
delay(20);
BT.write(disX10);
if (BT.available() > 0) { 
DATA = BT.read(); 
if (DATA == 'F') {
setdis = 0;
Go();
} else if (DATA == 'S') {
Stop();
} else if (DATA == 'B') {
Back();
} else if (DATA >= 2 && DATA <= 10) {
setdis = DATA;
}
}
if (setdis != 0) {
if (distance > setdis + 0.5) {
Go();
} else if (distance < setdis - 0.5) {
Back();
} else {
Stop();
}
}
}