 {
   // Initialize serial port at 9600 baud.
   Serial.begin(9600);
-
+  
+  Serial.println("VCNL");
+  Wire.begin();
   pwm.begin();
   pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz.
+  
+  // Check if proximity sensor is connected.
+  uint8_t rev = read8(VCNL4000_PRODUCTID);
+  if ((rev & 0xF0) != 0x10) {
+    Serial.println("Sensor not found :(");
+    while (1);
+  }
+  
+  write8(VCNL4000_IRLED, 20);        // set to 20 * 10mA = 200mA
+  Serial.print("IR LED current = ");
+  Serial.print(read8(VCNL4000_IRLED) * 10, DEC);
+  Serial.println(" mA");
+  
+  Serial.print("Proximity measurement frequency = ");
+  uint8_t freq = read8(VCNL4000_SIGNALFREQ);
+  if (freq == VCNL4000_3M125) Serial.println("3.125 MHz");
+  if (freq == VCNL4000_1M5625) Serial.println("1.5625 MHz");
+  if (freq == VCNL4000_781K25) Serial.println("781.25 KHz");
+  if (freq == VCNL4000_390K625) Serial.println("390.625 KHz");
+  
+  write8(VCNL4000_PROXIMITYADJUST, 0x81);
+  Serial.print("Proximity adjustment register = ");
+  Serial.println(read8(VCNL4000_PROXIMITYADJUST), HEX);
   
   // Initialize the pushbutton pin as an input.
   pinMode(button_pin, INPUT);


void play_song(int song_number)
{
  // 26 notes long
  for ( int i = 0; i < 26; i++ )
  {
    if ( song_number == 0 )
    {
      int note_length = 1000 / mary_had_a_little_lamb_lengths[i];
      tone(speakerPin, mary_had_a_little_lamb_notes[i], note_length);
    }
    

  photocell_reading = analogRead(photocell_pin);
  Serial.print("Analog reading = ");
  Serial.print(photocell_reading);
  Serial.print("\t");
  
  // When it gets bright or dark, make a sound.
  // Don't play the sound again until it's normal lighting out.
  if ( photocell_reading >= 700 && photocell_reading <= 710 )
  {
    normal_light_flag = 1;
  }
  
  if ( photocell_reading < 700 && normal_light_flag )
  {
    play_song(0);
    normal_light_flag = 0;
  }
  
  button_state = digitalRead(button_pin);
  
  if ( button_state == HIGH )
  {
    hug();
    reset_arms();
  }
  
  // read ambient light
  write8(VCNL4000_COMMAND, VCNL4000_MEASUREAMBIENT | VCNL4000_MEASUREPROXIMITY);
  
  // read proximity sensor
  while (1)
  {
    uint8_t result = read8(VCNL4000_COMMAND);
    //Serial.print("Ready = 0x"); Serial.println(result, HEX);
    if ( (result & VCNL4000_AMBIENTREADY) && (result & VCNL4000_PROXIMITYREADY) )
    {
      ambient_reading = read16(VCNL4000_AMBIENTDATA);
      proximity_reading = read16(VCNL4000_PROXIMITYDATA);
      
      Serial.print("Ambient = ");
      Serial.print(ambient_reading);
      Serial.print("\tProximity = ");
      Serial.println(proximity_reading);
      break;
    }
    
    delay(10);
  }
  
  if ( ambient_reading < 50 || proximity_reading > 10000 )
  {
    for ( int i = 0; i < 4; i++ )
    {
