   servoLeft.writeMicroseconds(1500);
   servoRight.writeMicroseconds(1500);
 }
-void standStill() {
-  
-}
 
 long microsecondsToCentimeters(long microseconds) {
   // The speed of sound is 340 m/s or 29 microseconds per centimeter.

  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);

  //Serial.println(String(cm)+" cm");
  return cm;
}

void rotateStep(bool clockwise) {

// Test of the ping sensor. Rotates the robot 
// around and gathers measurements;

#include <Servo.h> // Include servo library
    
Servo servoLeft;
Servo servoRight;

const int pingPin = 8;

long      pingDist;
const int nMeasurements = 20;
int       measurements[nMeasurements];
int       measureIndex = 0;

const bool clockwise = true;
const bool counterClockwise = false;

void setup() {
  // initialize serial communication:
  Serial.begin(9600);
  
  servoLeft.attach(13);
  servoRight.attach(12);
}

void loop() {
  rotateStep(counterClockwise);
  pingDist = measurePingDist();
  
  measurements[measureIndex] = pingDist;
  Serial.println(arrayToString(measurements,nMeasurements));
  
  //Loop around to 0 before out of bounds:
  ++measureIndex %= nMeasurements;

  delay(100);
}

String arrayToString(int array[], int arrayLength)
{
  String s = "[ ";
  for(int i=0; i < arrayLength; i++){
    s+=String(array[i])+" ";
  }
  return s + "]";
}
long measurePingDist(){
  long duration, cm;
