Arduino code for the direction sensor with two ultrasonic Ping sensors
#include <Servo.h>
Servo myservo;
int pos = 0;
int degree = 180;
//decline detection part
int detect = 0;
int dir = 0;
int detect_previous = 0;
int detect_previous2 = 0;
int limit = 20;
//decline sensor part
const int pingPinLeft = 8;
const int pingPinRight = 7;
void setup(){
myservo.attach(3);
Serial.begin(9600);
}
void loop(){
//ping sensor (readymade code)
long durationRight,durationLeft, cmRight, cmLeft;
pinMode(pingPinRight, OUTPUT);
digitalWrite(pingPinRight, LOW);
delayMicroseconds(2);
digitalWrite(pingPinRight, HIGH);
delayMicroseconds(5);
digitalWrite(pingPinRight, LOW);
pinMode(pingPinRight, INPUT);
durationRight = pulseIn(pingPinRight, HIGH);
pinMode(pingPinLeft, OUTPUT);
digitalWrite(pingPinLeft, LOW);
delayMicroseconds(2);
digitalWrite(pingPinLeft, HIGH);
delayMicroseconds(5);
digitalWrite(pingPinLeft, LOW);
pinMode(pingPinLeft, INPUT);
durationLeft = pulseIn(pingPinLeft, HIGH);
cmRight = microsecondsToCentimeters(durationRight);
cmLeft = microsecondsToCentimeters(durationLeft);
/*
Serial.print("left");
Serial.print(cmLeft);
Serial.println(" ");
Serial.print("right");
Serial.print(cmRight);
Serial.println();
*/
//--detect location of viewer--(left, middle, right, far from)
//I didn't understand properly "detect!= 1" part...
boolean left = (cmLeft<limit);
boolean right = (cmRight<limit);
//if(detect==0){
if(left == 1 && right == 0 && detect != 1) {
detect_previous2 = detect_previous;
detect_previous = detect;
detect = 1; //left:1
}else if (left == 1 && right == 1 && detect != 2){
detect_previous2 = detect_previous;
detect_previous = detect;
detect = 2;//middle:2
}else if (left == 0 && right == 1 && detect != 3){
detect_previous2 = detect_previous;
detect_previous = detect;
detect = 3;// right:3
}else if (left == 0 && right ==0 && detect != 4){
detect = 4;//far from: 4
detect_previous2 = detect_previous;
detect_previous = detect;
}
//}
/*--decide the direction of the viewer--*/
//only left->middle->right or the other way around triggers movement, otherwise, no reaction!!
//approaching from the left
Serial.print(detect_previous2);
Serial.print(" ");
Serial.print(detect_previous);
Serial.print(" ");
Serial.println(detect);
if(detect_previous2 ==1){
if(detect_previous ==2){
if(detect == 3){
dir = 1;}
else{
dir = 0;
}
}else
{
dir = 0;
}
}
//approaching from the center
else if (detect_previous2 == 2){
dir = 0;
}
//approaching from the right
else if (detect_previous2 == 3){
if (detect_previous == 2){
if(detect == 1){
dir = -1;
}else{
dir = 0;
}
}else{
dir = 0;
}
}
//not approaching
else{
dir = 0;
}
//move servo 180 degrees at a time
if (dir == 1){
Serial.println("moving");
for(; pos < 180; pos += 2) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}else if (dir == -1) {
for(; pos>=1; pos-=2) // goes from 180 degrees to 0 degrees
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
delay(10);
Serial.println(dir);
// ping function (time to distance)
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}