   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) {
+void loop(){  
+ 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");
-   
+   Serial.println("cm");  
  }
  if (millis() >= pingTimer2) {
    pingTimer2 = pingTimer1 + (pingSpeed / 2); // Make sensor 2 fire again 50ms after sensor 1 fires

#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];

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
  
  //
  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


//Hardware constants
#define PIN_STRIP 5

#define NUMBER_LEDS 48
#define NUMBER_STRIPS 3
