#include <Servo.h> 

Servo myservo;  // create servo object to control a servo 
Servo myservo2;  // create servo object to control a servo 

int val;    // variable to read the value from the analog pin 
int hip;
int neck;
int multiplier;

// variables to take x number of readings and then average them
// to remove the jitter/noise from the SRF05 sonar readings

const int numOfReadings = 10;                   // number of readings to take/ items in the array
int readings[numOfReadings];                    // stores the distance readings in an array
int arrayIndex = 0;                             // arrayIndex of the current item in the array
int total = 0;                                  // stores the cumlative total
int averageDistance = 0;                        // stores the average value

int addition = 1;

// setup pins and variables for SRF05 sonar device

int echoPin = 2;                                // SRF05 echo pin (digital 2)
int initPin = 3;                                // SRF05 trigger pin (digital 3)
unsigned long pulseTime = 0;                    // stores the pulse in Micro Seconds
unsigned long distance = 0;                     // variable for storing the distance (cm)

int xArray[180] ;
int yArray[180] ;
int i = 0;

void setup() 
{ 
  myservo.attach(9);  // hip
  myservo2.attach(10);  // neck
  
  //sonar sensor setup
  pinMode(initPin, OUTPUT);                     // set init pin 3 (blue) as output
  pinMode(echoPin, INPUT);                      // set echo pin 2 (green) as input
  for (int thisReading = 0; thisReading < numOfReadings; thisReading++) {
    readings[thisReading] = 0;
  }
  
  Serial.begin(9600); 
  botMove(70,90);
  delay(2500);
} 

void loop() 
{   
 
  
  
  //botMove(xArray[i],yArray[i]);
   botMove(i,i);

  
  //sonar sensor loop
  digitalWrite(initPin, HIGH);                    // send 10 microsecond pulse
  delayMicroseconds(10);                  // wait 10 microseconds before turning off
  digitalWrite(initPin, LOW);                     // stop sending the pulse
  pulseTime = pulseIn(echoPin, HIGH);             // Look for a return pulse, it should be high as the pulse goes low-high-low
  distance = pulseTime/58;                        // Distance = pulse time / 58 to convert to cm.
  total= total - readings[arrayIndex];           // subtract the last distance
  readings[arrayIndex] = distance;                // add distance reading to array
  total= total + readings[arrayIndex];            // add the reading to the total
  arrayIndex = arrayIndex + 1;                    // go to the next item in the array
  // At the end of the array (10 items) then start again
  if (arrayIndex >= numOfReadings)  {
    arrayIndex = 0;
  }

  averageDistance = total / numOfReadings;      // calculate the average distance

  // output cm/////////////
  selectLineOne();
  Serial.print("  cm:");
  Serial.print(averageDistance);         // print out the average distance to the debugger
  Serial.print("  ");
  ///////////////////////
  
  i = i + addition;
  if (i > 180 || i < 1)
    addition = addition * -1;

  //delay(100);
  //clearLCD();
  
} 



void botMove(int x, int y)
{
   //selectLineOne();
   hip = x;
   myservo.write(hip);                  // sets the servo position according to the scaled value 
   Serial.print("Xh:");Serial.print(hip);Serial.print("    ");
   selectLineTwo();
   neck = y;
   myservo2.write(neck);                  // sets the servo position according to the scaled value 
   Serial.print("Yn:");Serial.print(neck);Serial.print("   i="); Serial.print(i);Serial.print("    ");
}

void selectLineOne(){  //puts the cursor at line 0 char 0.
  Serial.print(0xFE, BYTE);   //command flag
  Serial.print(128, BYTE);    //position
}
void selectLineTwo(){  //puts the cursor at line 0 char 0.
  Serial.print(0xFE, BYTE);   //command flag
  Serial.print(192, BYTE);    //position
}
void clearLCD(){
   Serial.print(0xFE, BYTE);   //command flag
   Serial.print(0x01, BYTE);   //clear command.
}