#include <16F628A.H>
#fuses HS, NOWDT, NOPROTECT, BROWNOUT, PUT, NOLVP
#use delay(clock = 20000000)
#use i2c (Master, sda = PIN_A3, scl = PIN_A2, RESTART_WDT)
#use rs232 (baud = 9600, xmit = PIN_A1, rcv = PIN_A0, ERRORS)

#define R_ANKLE PIN_B0                        //Pin definitions
#define L_ANKLE PIN_B1
#define R_KNEE PIN_B2
#define L_KNEE PIN_B3
#define R_HIP PIN_B4
#define L_HIP PIN_B5
#define HEAD PIN_B6
#define SONAR_SCL PIN_A2                      //I2C Pins
#define SONAR_SDA PIN_A3
#define SFR08_ADDRESS 0xE0                    //SRF08 Address
      
int16 result_f, result_r, result_l;
int16 get_SRF08(void);
int i,j;

look_left(void);                             //Function declarations
look_right(void);
look_ahead(void);
stand(void);
first_tilt(void);
first_tilt_right_turn(void);
tilt_left(void);
tilt_left_right_turn(void);
right_up(void);
right_up_right_turn(void);
tilt_right(void);
tilt_right_right_turn(void);
left_up(void);
walk_ahead(void);
look_and_turn(void);
right_turn(void);
left_turn(void);

//Main Program..................................................................

main()
{
   setup_comparator (NC_NC_NC_NC);

   while (1)
   {
      for (i = 0; i < 150; i++)
         stand ();

      look_ahead ();
      result_f = get_SRF08 ();

      if (result_f > 10)
         walk_ahead ();

      else look_and_turn ();
   }
}

//SRF08 sonar reading function..................................................

 INT16 get_SRF08 (void)
  {
    int16 range;
    i2c_start ();                  //initate start condition
    i2c_write (SFR08_ADDRESS);     //device address
    i2c_write (0x00);              //register address
    i2c_write (0x50);              //set units to inches
    i2c_stop ();
    delay_ms (105);                //wait FOR returning ping
    i2c_start ();                  //initiate a new start condition
    i2c_write (SFR08_ADDRESS);     //device address
    i2c_write (0x02);              //address of high byte register
    i2c_start ();
    i2c_write (SFR08_ADDRESS + 1); //device address but read
    range = i2c_read (1);          //read first byte and store to preserve int16
    range = range << 8;            //shift left
    range += i2c_read (0);         //read second byte and add to 1st
    i2c_stop ();
    return range;
  }
  
//Walking Routines..............................................................

walk_ahead (void)            //walk forward 6 steps
  {
    for (i = 0; i < 8; i++)
       first_tilt ();

    for (i = 0; i < 45; i++)
       left_up ();

    for (j = 0; j < 3; j++)
     {
       for (i = 0; i < 45; i++)
          tilt_left ();

       for (i = 0; i < 45; i++)
          right_up ();

       for (i = 0; i < 45; i++)
          tilt_right ();

       for (i = 0; i < 45; i++)
          left_up ();
     }
  }

right_turn (void)
  {
    for (i = 0; i < 8; i++)
       first_tilt_right_turn ();

    for (j = 0; j < 4; j++)
     {
       for (i = 0; i < 45; i++)
          right_up_right_turn ();

       for (i = 0; i < 45; i++)
          tilt_right_right_turn ();

       for (i = 0; i < 45; i++)
          tilt_left_right_turn ();
     }
 }

 left_turn (void)
  {
    for (i = 0; i < 8; i++)
       first_tilt ();

    for (j = 0; j < 4; j++)
     {
       for (i = 0; i < 45; i++)
          left_up ();

       for (i = 0; i < 45; i++)
          tilt_left ();

       for (i = 0; i < 45; i++)
          tilt_right ();
     }
  }

//Looking Routines..............................................................

 look_and_turn (void)             //turn away from the closest objects
  {
    signed int16 diff;
    look_right ();
    result_r = get_SRF08 ();
    look_left ();
    result_l = get_SRF08 ();
    look_ahead ();
    diff = result_r - result_l;

    if (diff < 1)
       left_turn ();

    else
    right_turn ();
  }

 look_left (void)
  {
    for (i = 0; i < 30; i++)
     {
       output_high (HEAD);
       delay_us (1000);
       output_low (HEAD);
       delay_ms (19);
     }
  }

 look_right (void)
  {
    for (i = 0; i < 30; i++)
     {
       output_high (HEAD);
       delay_us (2000);
       output_low (HEAD);
       delay_ms (18);
     }
  }

 look_ahead (void)
  {
    for (i = 0; i < 30; i++)
     {
       output_high (HEAD);
       delay_us (1500);
       output_low (HEAD);
       delay_ms (18);
     }
  }

//Interim static positions

 stand (void)
  {
    output_high (R_ANKLE);
    delay_us (1500);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1500);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1500);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1500);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1500);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 first_tilt (void)
  {
    output_high (R_ANKLE);
    delay_us (1650);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1700);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1500);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1500);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1500);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 first_tilt_right_turn (void)
  {
    output_high (R_ANKLE);
    delay_us (1350);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1375);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1500);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1500);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1500);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 tilt_left (void)
  {
    output_high (R_ANKLE);
    delay_us (1350);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1300);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1500);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1850);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1400);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1650);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 tilt_left_right_turn (void)
  {
    output_high (R_ANKLE);
    delay_us (1350);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1300);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1500);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1850);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1450);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1800);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 right_up (void)
  {
    output_high (R_ANKLE);
    delay_us (1350);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1250);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1850);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1700);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1400);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 right_up_right_turn (void)
  {
    output_high (R_ANKLE);
    delay_us (1350);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1350);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1850);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1800);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1450);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 tilt_right (void)
  {
    output_high (R_ANKLE);
    delay_us (1700);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1700);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1850);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1650);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1400);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 tilt_right_right_turn (void)
  {
    output_high (R_ANKLE);
    delay_us (1725);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1725);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1850);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1500);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1800);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1450);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }

 left_up (void)
  {
    output_high (R_ANKLE);
    delay_us (1700);
    output_low (R_ANKLE);
    output_high (L_ANKLE);
    delay_us (1600);
    output_low (L_ANKLE);
    output_high (R_KNEE);
    delay_us (1500);
    output_low (R_KNEE);
    output_high (L_KNEE);
    delay_us (1850);
    output_low (L_KNEE);
    output_high (R_HIP);
    delay_us (1400);
    output_low (R_HIP);
    output_high (L_HIP);
    delay_us (1700);
    output_low (L_HIP);
    delay_us (1500);
    output_low (HEAD);
    delay_ms (9);
  }