 #define PIN_SWITCH 2
 
 Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMBER_LEDS, PIN_STRIP, NEO_GRB + NEO_KHZ800);
-//Servo servo;
-
-//Servo variables
-//int servoPos = 0;
-//int servoSteps = 6;
-//int servoMinAngle = 0;
-//int servoMaxAngle = 90;
-//int servoStepAngle = (servoMaxAngle-servoMinAngle)/(servoSteps-1);
-//int servoMSPerDegree = 5;
+
 
 //Store of the measured distances of each servo step
-//int distanceValues[6];
-  int distanceValues[3];
+int distanceValues[3];
 
 void setup() {  
   //Init distance store to far away
-//  for(int i=0; i<servoSteps; i+=1){
-//    distanceValues[i] = 0;
-//  }
-  distanceValues[0] = 0;
-  distanceValues[1] = 0;
-  distanceValues[2] = 0;
-  
-  pinMode(PIN_SWITCH, INPUT_PULLUP);           // set pin to input with internal pullup resistor
-  
-  //
+  for(int i=0; i<NUMBER_STRIPS; i+=1){
+    distanceValues[i] = 0;
+  }
+  
+  pinMode(PIN_SWITCH, INPUT_PULLUP); // set pin to input with internal pullup resistor
+  
   pingTimer1 = millis() + pingSpeed; // Sensor 1 fires after 100ms (pingSpeed)
   pingTimer2 = pingTimer1 + (pingSpeed / 2); // Sensor 2 fires 50ms later
   pingTimer3 = pingTimer2 + (pingSpeed / 2); // Sensor 3 fires 50ms later

  strip.begin();
  strip.setBrightness(64);
  strip.show();
  
  //Set servo to init position
  //And wait for the servo reaching the position
//  servo.attach(PIN_SERVO);
//  servo.write(servoPos);
//  delay(5*360);
}

/**
 *
 *  MAIN
 *  Move servo, read sensor and write leds
 *
 */
void loop(){
//  int i = 0;
//  for(servoPos = servoMinAngle; servoPos < servoMaxAngle; servoPos += servoStepAngle){
//    readSensorWriteLEDs(servoPos, i);
//    i+=1;
//  }
//  
//  for(servoPos = servoMaxAngle; servoPos>servoMinAngle; servoPos-=servoStepAngle)
//  {                
//    readSensorWriteLEDs(servoPos, i);
//    i-=1;
//  }
   if (millis() >= pingTimer1) {
   pingTimer1 += pingSpeed; // Make sensor 1 fire again 100ms later (pingSpeed)
   distanceValues[0] = sonar1.ping_cm();
   Serial.print("Ping 1: ");
   Serial.print(distanceValues[0]); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
   Serial.println("cm");
   
 }
 if (millis() >= pingTimer2) {
   pingTimer2 = pingTimer1 + (pingSpeed / 2); // Make sensor 2 fire again 50ms after sensor 1 fires

  strip.show();
}


//void printDistance(int distance){
//  Serial.print(distance);
//  Serial.print(" cm");
