#include <NewPing.h>
#include <Servo.h>
//Ultrasonic Sensor
#define RIGHT_TRIG_PIN A1
#define RIGHT_ECHO_PIN A0
#define CENTER_TRIG_PIN A3
#define CENTER_ECHO_PIN A2
#define LEFT_TRIG_PIN A5
#define LEFT_ECHO_PIN A4
#define MAX_DISTANCE 200
NewPing right(RIGHT_TRIG_PIN, RIGHT_ECHO_PIN, MAX_DISTANCE);
NewPing center(CENTER_TRIG_PIN, CENTER_ECHO_PIN, MAX_DISTANCE);
NewPing left(LEFT_TRIG_PIN, LEFT_ECHO_PIN, MAX_DISTANCE);
//Rigth motor
#define in1 3 //Motor1 L298 Pin in1
#define in2 4 //Motor1 L298 Pin in2
//Left motor
#define in3 5 //Motor2 L298 Pin in3
#define in4 6 //Motor2 L298 Pin in4
//Servo
Servo servo1; //hand servo
Servo servo2; //hand servo
Servo servo3; //grip servo
Servo servo4; //grip servo
int pos = 0;
int pos1 = 0;
void setup() {
Serial.begin(115200);
servo1.attach(11); //hand servo
servo2.attach(10); //hand servo
servo3.attach(9); //grip servo
servo4.attach(8);
servo1.write(60);
servo2.write(120);
servo3.write(20);
servo4.write(160);
}
void loop() {
int rightDistance = right.ping_cm();
int centerDistance = center.ping_cm();
int leftDistance = left.ping_cm();
Serial.print("Right: ");
Serial.print(rightDistance);
Serial.print("cm");
Serial.print(" Center: ");
Serial.print(centerDistance);
Serial.print("cm");
Serial.print(" Left: ");
Serial.print(leftDistance);
Serial.println("cm");
delay(15);
Forward();
/----------------------------------------------------------/
if(centerDistance <= 10){
Stop();
delay(1000);
Backward();
delay(500);
Stop();
delay(500);
myServo();
delay(1000);
if(rightDistance > leftDistance){
Serial.println("Left");
Stop();
delay(500);
turnRight();
delay(500);
}
else{
Serial.println("Right");
Stop();
delay(500);
turnLeft();
delay(500);
}
}
if (rightDistance <= 10){
Stop();
delay(1000);
turnRight();
}
if( leftDistance <= 10){
Stop();
delay(1000);
turnLeft();
}
}
//-----------------Servo Setup------------------------------
void myServo(){
for (pos = 60, pos1 =120 ; pos <= 165, pos1>=15; pos += 2, pos1-=2 ) { // goes from 90 degrees to 180 degrees
// in steps of 1 degree
servo1.write(pos);
servo2.write(pos1);
delay(15); // waits 15 ms for the servo to reach the position
}
delay(500);
for(pos = 20,pos1 = 160; pos <=90, pos1>=90; pos +=2, pos1-=2){
servo3.write(pos);
servo4.write(pos1);
delay(15);
}
delay(1000);
for (pos = 165, pos1 = 15; pos >= 30 , pos1<=150; pos -= 2, pos1+=2 )
{
servo1.write(pos);
servo2.write(pos1);
delay(15);
}
delay(1000);
for(pos = 90,pos1 = 90; pos >=20, pos1<=160; pos-=2, pos1+=2){
servo3.write(pos);
servo4.write(pos1);
delay(15);
}
delay(1000);
servo1.write(60);
servo2.write(120);
servo3.write(20);
servo4.write(160);
}
//-----------------Motor Setup------------------------------
void Stop(){
digitalWrite(in1, 0); //Left Motor backward Pin
digitalWrite(in2, 0); //Left Motor forward Pin
digitalWrite(in3, 0); //Right Motor forward Pin
digitalWrite(in4, 0); //Right Motor backward Pin
}
void Forward(){ //forward
digitalWrite(in1, 255); //Left Motor backward Pin
digitalWrite(in2, 0); //Left Motor forward Pin
digitalWrite(in3, 255); //Right Motor forward Pin
digitalWrite(in4, 0); //Right Motor backward Pin
}
void Backward(){ //Backward
digitalWrite(in1, 0);
digitalWrite(in2, 80);
digitalWrite(in3, 0);
digitalWrite(in4, 80);
delay(500);
}
void turnLeft(){ //turnLeft
digitalWrite(in1, 125);
digitalWrite(in2, 0);
digitalWrite(in3, 0);
digitalWrite(in4, 125);
delay(500);
}
void turnRight(){ //turnRight
digitalWrite(in1, 0);
digitalWrite(in2, 125);
digitalWrite(in3, 125);
digitalWrite(in4, 0);
delay(500);
}