     if (!SERIAL_ON && WIICHUCK_ON) {
       wiiChuck();
     }
+    
+    if (PING_ON) {
+      pingLoop();
+    }
+}
+
+void pingLoop() {
+  long duration, distance;
+
+  digitalWrite(TRIG_PIN, LOW);  // Added this line
+  delayMicroseconds(2); // Added this line
+  digitalWrite(TRIG_PIN, HIGH);
+  delayMicroseconds(10); // Added this line
+  digitalWrite(TRIG_PIN, LOW);
+
+  duration = pulseIn(ECHO_PIN, HIGH);
+  distance = (duration/2) / 29.1;
+
+  if (distance < 6) {
+    stop();
+    reverse(leftspeed, rightspeed);
+    delay(1000);
+    stop();
+    delay(1000);
+    right(leftspeed, rightspeed);
+    delay(1000);
+    stop();
+    delay(1000);
+    forward(leftspeed, rightspeed);
+  } else {
+    forward(leftspeed, rightspeed);
+  }
+  
+  if (DEBUG) {
+    if (distance >= 200 || distance <= 0){
+      Serial.println("Out of range");
+    } else {
+      Serial.print(distance);
+      Serial.println(" cm");
+    }
+  }
+
+  delay(100);
 }
 
 void wiiChuck() {

    pinMode(11, OUTPUT); //Initiates Brake Channel B pin
  
    if (DEBUG) {
      Serial.begin(19200);
      Serial.println("setup");
    }
  

        nunchuck_setpowerpins();
        nunchuck_init(); // send the initilization handshake
    }
    
    if (PING_ON) {
      pinMode(TRIG_PIN, OUTPUT);
      pinMode(ECHO_PIN, INPUT);
    }
}

void loop() {
