*Office Supplies HEXAPOD Code

#include <Servo.h> 

#define MAX_DEGREE 145
#define MIN_DEGREE 35
#define MID_DEGREE 90
#define BASIC_DELAY 250

Servo FL;  // create servo object to control a servo 
Servo FR;
Servo ML;
Servo MR;
Servo RL;
Servo RR;

void setup() 
{
 // put your setup code here, to run once:
 FL.attach(3);  
 FR.attach(5);
 ML.attach(6);
 MR.attach(9);
 RL.attach(10);
 RR.attach(11);
}

void Stand()
{
 FL.write(MID_DEGREE);
 FR.write(MID_DEGREE);
 ML.write(MID_DEGREE);
 MR.write(MID_DEGREE);
 RL.write(MID_DEGREE);
 RR.write(MID_DEGREE);
}

void Left_Position_Forward()
{
 FL.write(MIN_DEGREE);
 RL.write(MIN_DEGREE);  
}

void Left_Position_Backward()
{
 FL.write(MAX_DEGREE);
 RL.write(MAX_DEGREE);  
}

void Left_Position_Outward()
{
 FL.write(MIN_DEGREE);
 RL.write(MAX_DEGREE);    
}

void Left_Position_Inward()
{
 FL.write(MAX_DEGREE);
 RL.write(MIN_DEGREE);    
}

void Left_Position_Stand()
{
 FL.write(MID_DEGREE);
 RL.write(MID_DEGREE);
}

void Right_Position_Forward()
{
 FR.write(MAX_DEGREE);
 RR.write(MAX_DEGREE);  
}

void Right_Position_Backward()
{
 FR.write(MIN_DEGREE);
 RR.write(MIN_DEGREE);  
}

void Right_Position_Outward()
{
 FR.write(MAX_DEGREE);
 RR.write(MIN_DEGREE);  
}

void Right_Position_Inward()
{
 FR.write(MIN_DEGREE);
 RR.write(MAX_DEGREE);  
}

void Right_Position_Stand()
{
 FR.write(MID_DEGREE);
 RR.write(MID_DEGREE);
}

void Middle_Position_Left()
{
 ML.write(MAX_DEGREE);
 MR.write(MAX_DEGREE);
}

void Middle_Position_Right()
{
 ML.write(MIN_DEGREE);
 MR.write(MIN_DEGREE);
}

void Middle_Position_Stand()
{
 ML.write(MID_DEGREE);
 MR.write(MID_DEGREE);
}

void Middle_Position_Outward()
{
 ML.write(MAX_DEGREE);
 MR.write(MIN_DEGREE);  
}

void Middle_Position_Inward()
{
 ML.write(MIN_DEGREE);
 MR.write(MAX_DEGREE);  
}

void Middle_Lean_Left()
{
 ML.write(MIN_DEGREE);
 MR.write(MID_DEGREE);
 Left_Position_Outward();
}

void Middle_Lean_Right()
{
 ML.write(MID_DEGREE);
 MR.write(MAX_DEGREE);
 Right_Position_Outward();
}

void Step_Forward()
{
 Stand();
 delay(BASIC_DELAY);
 Left_Position_Forward();
 delay(BASIC_DELAY);
 Middle_Position_Inward();
 delay(BASIC_DELAY);
 Left_Position_Backward();
 delay(BASIC_DELAY);
 Middle_Position_Stand();
 delay(BASIC_DELAY);
 Left_Position_Stand();
 delay(BASIC_DELAY);
 Right_Position_Forward();
 delay(BASIC_DELAY);
 Middle_Position_Inward();
 delay(BASIC_DELAY);
 Right_Position_Backward();
 delay(BASIC_DELAY);
 Middle_Position_Stand();
 delay(BASIC_DELAY);
 Right_Position_Stand();
 delay(BASIC_DELAY);
}

void Step_Backward()
{
 Stand();
 delay(BASIC_DELAY);
 Left_Position_Backward();
 delay(BASIC_DELAY);
 Middle_Position_Inward();
 delay(BASIC_DELAY);
 Left_Position_Forward();
 delay(BASIC_DELAY);
 Middle_Position_Stand();
 delay(BASIC_DELAY);
 Left_Position_Stand();
 delay(BASIC_DELAY);
 Right_Position_Backward();
 delay(BASIC_DELAY);
 Middle_Position_Inward();
 delay(BASIC_DELAY);
 Right_Position_Forward();
 delay(BASIC_DELAY);
 Middle_Position_Stand();
 delay(BASIC_DELAY);
 Right_Position_Stand();
 delay(BASIC_DELAY);
}

void Step_Left()
 {
 Stand();
 delay(BASIC_DELAY);
 Right_Position_Forward();
 delay(BASIC_DELAY);
 Middle_Position_Inward();
 delay(BASIC_DELAY);
 Right_Position_Backward();
 delay(BASIC_DELAY);
 Middle_Position_Stand();
 delay(BASIC_DELAY);
 Right_Position_Stand();
 delay(BASIC_DELAY);
 
}

void Step_Right()
{
 Stand();
 delay(BASIC_DELAY);
 Left_Position_Forward();
 delay(BASIC_DELAY);
 Middle_Position_Inward();
 delay(BASIC_DELAY);
 Left_Position_Backward();
 delay(BASIC_DELAY);
 Middle_Position_Stand();
 delay(BASIC_DELAY);
 Left_Position_Stand();
 delay(BASIC_DELAY);

}

 int Left_Sensor;
 int Right_Sensor;
 int Front_Sensor;
 int Rear_Sensor;

void Set_Sensors()
{
 Left_Sensor = analogRead(2);
 Right_Sensor = analogRead(1);
 Front_Sensor = analogRead(3);
 Rear_Sensor = analogRead(0);  
}

int Sensor_Approximation(int S, int V)
{
 if (V > S * 1.2)
   return (1);
 if (V < S * 0.8)  
   return(0); 
 return(0);
}

int Dark_Direction()
{
 
}

void loop() 
{
 int x;
 
 
 Stand();
 delay(2000);
 
 Set_Sensors();
 
 while(1)
   {
     if (Sensor_Approximation(Left_Sensor, analogRead(2)))
       {
         for(x=0;x<5;++x)
           Step_Right();
         Set_Sensors();            
       }
     if (Sensor_Approximation(Right_Sensor, analogRead(1)))
       {
         for(x=0;x<5;++x)
           Step_Left();  
         Set_Sensors();        
       }
     if (Sensor_Approximation(Front_Sensor, analogRead(3)))
       {
         for(x=0;x<5;++x)
           Step_Backward();          
         Set_Sensors();
       }
     if (Sensor_Approximation(Rear_Sensor, analogRead(0)))
       {
         for(x=0;x<5;++x)
           Step_Forward();                    
         Set_Sensors();
       }
       
   }
 
 delay(2000);

 // put your main code here, to run repeatedly:

}