Quadruped_Robot_Arduino code for maze solver.

Quadruped_Robot_Arduinocodeformazesolver.ino

// Include the Servo library 
#include <Servo.h> 
// IR sensor
#define LS 9     // left sensor
#define RS 12     // right sensor
// Ultrasonic sensor pins
const int trigPin = 8;
const int echoPin = 10;
// Declare the Servo pin 
int servoPin1 = 0; 
int servoPin2 = 1; 
int servoPin3 = 2; 
int servoPin4 = 3; 
int servoPin5 = 4; 
int servoPin6 = 5; 
int servoPin7 = 6; 
int servoPin8 = 7; 
int servoPinu = 11;
int flag;
int cnt;
Servo Servo1,Servo2,Servo3,Servo4,Servo5,Servo6,Servo7,Servo8,Servou; 
void setup() { 
  pinMode(LS, INPUT);
  pinMode(RS, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
   Servo1.attach(servoPin1); 
      Servo2.attach(servoPin3); 
      Servo3.attach(servoPin5); 
       Servo4.attach(servoPin7); 
        Servo5.attach(servoPin2); 
       Servo6.attach(servoPin4); 
       Servo7.attach(servoPin6);
       Servo8.attach(servoPin8);
     
   flag=0;
   cnt=1;
}
void loop(){ 

  long duration, inches, cm;

 if(flag==0)//moving Leg A backwards for first time alone   
 {
  Servo5.write(50);
  delay(50);
  Servo1.write(150);
  delay(100);
  Servo5.write(90);
  delay(50);
 }
  // 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:
  pinMode(trigPin, OUTPUT);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10000);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10000);
  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.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  
if(cm>=26)//when obstacle is not present
{
 
if(!digitalRead(LS) & !digitalRead(RS) )     // Move Forward
  {
    
   Servo6.write(50);
  delay(50);
  Servo2.write(30);
  delay(100);
  Servo6.write(90);
  delay(50);

  Servo5.write(50);
  delay(50);
  Servo1.write(90);
  delay(100);
  Servo5.write(90);
  delay(50);
//main
 Servo6.write(120);
  delay(50);
   Servo8.write(120);
  delay(50);
  Servo4.write(60);
    delay(100);
  Servo2.write(60);
      delay(100);
    Servo6.write(90);
  delay(50);
  Servo8.write(90);
  delay(50);
  
 

  Servo7.write(50);
  delay(50);
  Servo3.write(150);
  delay(100);
  Servo7.write(90);
  delay(50);

 
  Servo8.write(50);
  delay(50);
  Servo4.write(90);
  delay(100);
  Servo8.write(90);
  delay(50);
  
   //main
  Servo7.write(120);
  delay(50);
  Servo5.write(120);
  delay(50);
  Servo1.write(120);//changed
      delay(100); 
  Servo3.write(60);
     delay(100); 
  Servo7.write(90);
  delay(50);
     Servo5.write(90);
  delay(50);
   flag=1;
  }
//when quadruped deviates the line in left side, then it should move rightwards 
  else if(!digitalRead(LS) & digitalRead(RS))//right
  {
       

 Servo6.write(50);
  delay(50);
  Servo2.write(30);
  delay(100);
  Servo6.write(90);
  delay(50);

  Servo5.write(50);
  delay(50);
  Servo1.write(90);
  delay(100);
  Servo5.write(90);
  delay(50);
//main
 Servo6.write(120);
  delay(50);
   Servo8.write(120);
  delay(50);
  Servo4.write(30);
    delay(100);
  Servo2.write(60);
      delay(100);
  
  Servo6.write(90);
  delay(50);
  Servo8.write(90);
  delay(50);
  
 

  Servo7.write(50);
  delay(50);
  Servo3.write(150);
  delay(100);
  Servo7.write(90);
  delay(50);
  Servo8.write(50);
  delay(50);
  Servo4.write(90);
  delay(100);
  Servo8.write(90);
  delay(50);
   //main
  Servo7.write(120);
  delay(50);
  Servo5.write(120);
  delay(50);
    Servo3.write(60);
        delay(100); 
  Servo1.write(120);

     delay(100); 
   
  Servo7.write(90);
  delay(50);
     Servo5.write(90);
  delay(50);
   flag=1;
  }
//when quadruped deviates the line in right side, then it should move leftwards 
  else if(digitalRead(LS) & !digitalRead(RS))//left
  {
       
  Servo6.write(50);
  delay(50);
  Servo2.write(30);
  delay(100);
  Servo6.write(90);
  delay(50);

  Servo5.write(50);
  delay(50);
  Servo1.write(90);
  delay(100);
  Servo5.write(90);
  delay(50);
//main
 Servo6.write(120);
  delay(50);
   Servo8.write(120);
  delay(50);
  Servo4.write(30);
    delay(100);
  Servo2.write(90);
      delay(100);
  Servo6.write(90);
  delay(50);
  Servo8.write(90);
  delay(50);
  Servo7.write(50);
  delay(50);
  Servo3.write(150);//changed
  delay(100);
  Servo7.write(90);
  delay(50);

 
  Servo8.write(50);
  delay(50);
  Servo4.write(90);
  delay(100);
  Servo8.write(90);
  delay(50);
   //main
  Servo7.write(120);
  delay(50);
  Servo5.write(120);
  delay(50);
  Servo1.write(120);
      delay(100); 
  Servo3.write(60);
     delay(100); 
  Servo7.write(90);
  delay(50);
     Servo5.write(90);
  delay(50);
   flag=1;
  }
}
//obstacle is detected, so taking complete right turn
else
{
// just turning the head right side and again to usual position
  Servou.write(30);
  delay(100);
   Servou.write(150);
     delay(100);
      Servou.write(90); 
      delay(500);
    
  if(digitalRead(LS) & !digitalRead(RS)) // if left sensor is outside black line, repeat right turn 4 times so that after turning it will be in correct position in line to walk
  {
    makeRight(4);
  }
  else if (!digitalRead(LS) & !digitalRead(RS))
  {
    makeRight(3); // if both sensor are in black line, repeat right turn 3 times
  }
 
  else
  {
     makeRight(2); // if right sensor is outside black line, repeat right turn 2 times
  }
      

}
}

void  makeRight(int j)  //to turn when there is an obstacle
{ 

  while(j>=0)
  {
    j--;
  Servo6.write(50);
  delay(100);
  Servo2.write(30);
  delay(100);
  Servo6.write(90);
  delay(100);

  Servo5.write(60);
  delay(100);
  Servo1.write(90);
  delay(100);
  Servo5.write(90);
  delay(100);

 Servo6.write(120);
  delay(100);
   Servo8.write(120);
  delay(100);
  Servo4.write(130);
    delay(100);
  Servo2.write(130);
      delay(200);
  
  Servo6.write(90);
  delay(100);
  Servo8.write(90);
  delay(100);
    Servo7.write(50);
  delay(100);
  Servo3.write(130);
  delay(100);
  Servo7.write(90);
  delay(100);
 
  Servo8.write(50);
  delay(100);
  Servo4.write(90);
  delay(100);
  Servo8.write(90);
  delay(100);
  
  Servo7.write(120);
  delay(100);
  Servo5.write(130);
  delay(100);
  Servo1.write(130);
     delay(100);
  Servo3.write(60);
   delay(200);
   
  Servo7.write(90);
  delay(100);
     Servo5.write(90);
  delay(100);
  }
}
long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}