#define PIN_ANALOG_X 0
#define PIN_ANALOG_Y 1
#define X_THRESHOLD_LOW 500
#define X_THRESHOLD_HIGH 650
#define Y_THRESHOLD_LOW 400
#define Y_THRESHOLD_HIGH 550
int x_position;
int y_position;
int x_direction;
int y_direction;
#define L_M_1 5 //left motor input 1 @ pwm pin
#define L_M_2 6 //left motor input 2 @ pwm pin
#define R_M_1 10 //right motor input 1 @ pwm pin
#define R_M_2 11 //right motor input 2 @ pwm pin
#define Led_L 44 //indicator for left
#define Led_R 40 //indicator led for right
#define Led_O 42
#define trigPin 14 //sonar trigger to arduino pin
#define echoPin 15 //sonar echo to arduino pin
#define buzPin 50 //buzzer pin
// L motor values: 65, 100, 150, 200
//corresponding R motor values: 45, 75, 120, 170
#define min_clearance 10 // minimum clearance from backside is 10 cm
long duration, cm, inches,distance;
void setup() {
pinMode(L_M_1,OUTPUT); // for motor
pinMode(L_M_2,OUTPUT);
pinMode(R_M_1,OUTPUT);
pinMode(R_M_2,OUTPUT);
pinMode(trigPin,OUTPUT); //sonar sensor
pinMode(echoPin,INPUT);
pinMode(Led_L,OUTPUT); //sonar sensor
pinMode(Led_R,OUTPUT);
pinMode(Led_O,OUTPUT);
Serial.begin(9600);
void loop() {
x_direction = 0;
y_direction = 0;
x_position = analogRead(PIN_ANALOG_X);
y_position = analogRead(PIN_ANALOG_Y);
if (x_position > X_THRESHOLD_HIGH) {
x_direction = 1;
} else if (x_position < X_THRESHOLD_LOW) {
x_direction = -1;
}
if (y_position > Y_THRESHOLD_HIGH) {
y_direction = 1;
} else if (y_position < Y_THRESHOLD_LOW) {
y_direction = -1;
}
digitalWrite(trigPin, LOW);// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
delayMicroseconds(5);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the signal from the sensor: a HIGH pulse whose
// duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
duration = pulseIn(echoPin, HIGH);
// convert the time into a distance
cm = (duration/2) / 29.1;
// inches = (duration/2) / 74;
distance= cm;
if (x_direction == -1)
{
if (y_direction == -1) {
Serial.println("left-down");
if(distance<= 10)
{
Serial.print(distance);
Stop(50);
Blink();
}
else
Backword(140,90,20);
} else if (y_direction == 0) {
Serial.println("left");
Turn_Left(100,20);
digitalWrite(Led_L,HIGH);
}
else
{
// y_direction == 1
Serial.println("left-up");
Forword(150,120,10);
}
}
else if (x_direction == 0)
{
if (y_direction == -1)
{
Serial.println("down");
if(distance<= 10)
{
Serial.print(distance);
Stop(50);
Blink();
Buzzer(10);
}
else
{
Backword(150,120,10);
digitalWrite(Led_O,HIGH);
}
}
else if (y_direction == 0)
{
Serial.println("centered");
Stop(250);
}
else
{
// y_direction == 1
Serial.println("Forword");
Forword(140,115,30);
}
}
else
{
// x_direction == 1
if (y_direction == -1)
{
Serial.println("right-down");
if(distance<= 5)
{
Serial.print(distance);
Blink();
Stop(100);
Buzzer(10);
}
else
{
Backword(150,120,10);
digitalWrite(Led_O,HIGH);
}
}
else if (y_direction == 0)
{
Serial.println("right");
Turn_Right(130,20);
digitalWrite(Led_R,HIGH);
}
else
{
// y_direction == 1
Serial.println("right-up");
Forword(150,125,10);
}
}
delay(250);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Forword(int i,int j,int k)
//both motor are forword
{
//lcd.print("FORWORD");
Serial.println("\t \t FORWORD");
analogWrite(L_M_1,i); // left motor is forword
analogWrite(L_M_2,0);
analogWrite(R_M_1,j); // right motor is forword
analogWrite(R_M_2,0);
digitalWrite(Led_L,LOW);
digitalWrite(Led_R,LOW);
digitalWrite(Led_O,LOW);
delay(k);
}
void Backword(int i,int j,int k)
//both motor are backword
{
Serial.println("\t \t BACKWORD");
analogWrite(L_M_1,0); // left motor is backword
analogWrite(L_M_2,i);
analogWrite(R_M_1,0); // right motor is backword
analogWrite(R_M_2,j);
delay(k);
}
void Turn_Left(int i,int j)
//Left motor is off
//Right motor is forword
{
Serial.println("\t \t Left Turn");
analogWrite(L_M_1,0); // left motor is off
analogWrite(L_M_2,0);
analogWrite(R_M_1,i); // right motor is forword
analogWrite(R_M_2,0);
digitalWrite(Led_R,LOW);
digitalWrite(Led_O,LOW);
delay(j);
}
void Turn_Right(int i,int j)
//Right motor is off
//left motor is forword
{
Serial.println("\t \t Turn Right");
analogWrite(L_M_1,i); // left motor is forword
analogWrite(L_M_2,0);
analogWrite(R_M_1,0); // right motor is off
analogWrite(R_M_2,0);
digitalWrite(Led_L,LOW);
digitalWrite(Led_O,LOW);
delay(j);
}
void Stop(int k)
//Both motor is stopped
{
Serial.println("\t \t STOP");
analogWrite(L_M_1,0); //left motor is off
analogWrite(L_M_2,0);
analogWrite(R_M_1,0);//right motor is off
analogWrite(R_M_2,0);
digitalWrite(Led_L,LOW);
digitalWrite(Led_R,LOW);
delay(k);
}
void Blink()
{
digitalWrite(Led_O,HIGH);
digitalWrite(Led_O,HIGH);
digitalWrite(Led_O,HIGH);
delay(20);
digitalWrite(Led_O,LOW);
digitalWrite(Led_O,LOW);
digitalWrite(Led_O,LOW);
delay(20);
digitalWrite(Led_O,HIGH);
delay(20);
digitalWrite(Led_O,LOW);
delay(20);
}
void Buzzer(int i) // here i is the frequency in KHz
{
for (int j=0;j<100;j++)
{
long T=1000/i; //frequency converted into Time(cycle) mili seconds
digitalWrite(buzPin,HIGH);
delay(T/2); //half cycle time on
digitalWrite(buzPin,LOW);
delay(T/2); // half cycle time off
}
}
