   return result; // Vcc in millivolts
 }
 
+
 /********************************************************************************
-Sound functions
+Ping functions
 ********************************************************************************/
-/*
-void sound_nextNote()
-{
-  ++sound_current_note;
-  // @todo: remove hard coding to the 8 note melody
-  if (sound_current_note < 8) {
-    sound_playNote(sound_current_note);
+
+void ping_start()
+{
+  if (ping_timer == 0) {
+    ping_timer = timer.setInterval(ping_delay, ping_measure);
   } else {
-    // reset the counter
-   sound_current_note = 0; 
-  }
-}
-
-void sound_stopNote()
-{
-  // stop the tone playing:
-  noNewTone(sound_pin);
-  sound_nextNote();
-}
-
-void sound_playNote(int note)
-{
-  int noteDuration = 1000/noteDurations[note];
-  NewTone(sound_pin, melody[note], noteDuration);
-
-  // to distinguish the notes, set a minimum time between them.
-  // the note's duration + 30% seems to work well:
-  int pauseBetweenNotes = noteDuration * 1.30;
-  Serial.print(melody[note]); 
-  Serial.print(" : ");
-  Serial.println(pauseBetweenNotes);
-  timer.setTimeout(pauseBetweenNotes, sound_stopNote);
-}
-
-void sound_playMelody() 
-{
-  sound_playNote(sound_current_note);
-}
-*/
-
+    timer.enable(ping_timer); 
+  }
+  Serial.println("Ping started"); 
+}
+
+void ping_stop()
+{
+  timer.disable(ping_timer);
+  Serial.println("Ping stopped"); 
+}
+
+void ping_measure()
+{
+  unsigned int cm = sonar.ping_cm(); // Send a ping, get ping distance in cm.
+  
+  if (action_state == A_PINGSEARCH) {
+    
+    // looking for a new way to go.
+    //int max_cm = 
+    
+  } else {
+    
+    // Watch for obstacles as we are not doing a ping search.
+    if (cm > 0 && cm < 10) {
+      // too close! 
+      Serial.println("Too close!");
+      
+      // No more pinging for now.
+      ping_stop();
+      
+      // Don't hit the obstacle.
+      motion_stop();
+      
+      // play sound
+      playMelody();
+    
+      // back up
+      motion_rev();
+      
+      // then after a second, search for a new direction.
+      timer.setTimeout(1000, action_pingSearch);
+    }
+    
+    Serial.print("Ping: ");
+    Serial.print(cm);
+    Serial.println("cm");
+  }
+}
+
+
+
+/********************************************************************************
+Action functions
+********************************************************************************/
+/**
+ * The "state machine" that handles action.
+ */
+void action_run()
+{
+  /*
+  Serial.print("action_run ");
+  Serial.print(action_state);
+  Serial.print(" - ");
+  Serial.println(motion_state);
+  */
+  
+  switch (action_state) {
+    case A_STOPPED:
+    
+      break;
+    
+    case A_TRUNDLE:
+    
+      break;
+      
+    case A_PINGSEARCH:
+      switch (motion_state) {
+        case M_STOP:
+        Serial.println("SEARCH LEFT");
+          // Start pinging
+          ping_start();
+  
+          action_timeoutStart(1000);
+          // Rotate left for 1 second
+          motion_rotateLeft();          
+          break;
+          
+        case M_ROTATE_LEFT:
+          if (action_done) {
+            Serial.println("SEARCH RIGHT");
+            action_timeoutStart(1500); //@todo not fake the search!
+            // Rotate left
+            motion_rotateRight(); 
+          }
+          break;
+          
+        case M_ROTATE_RIGHT:
+          if (action_done) {
+            Serial.println("SEARCH DONE");
+            // Done the sweep, stop.
+            action_stop(); 
+            
+            // Stop pinging
+            ping_stop(); 
+            
+            // Trundle away
+            action_trundle();
+          }
+          break;
+      }
+      break;
+      
+    case A_DANCE1:
+    
+      break;
+      
+    case A_DANCE2:
+    
+      break;
+      
+    case A_BORED:
+    
+      break;
+      
+    case A_HAPPY:
+    
+      break;
+      
+    case A_CALIBRATION:
+    
+      break;
+  }
+}
+
+void action_stop()
+{
+  action_state = A_STOPPED;
+  motion_stop();
+}
+
+/**
+ * Moves forward avoiding obstacles.
+ */
+void action_trundle()
+{
+  Serial.println("TRUNDLING...");
+  // Remeber that we're trundling along.
+  action_state = A_TRUNDLE;
+  
+  // Start pinging
+  ping_start();
+
+  motion_fwd();
+}
+
+/**
+ * Look for somewhare to go.
+ */
+void action_pingSearch()
+{
+  Serial.println("SEARCHING");
+
+  motion_stop();
+  // Set the state, so the action can complete.
+  action_state = A_PINGSEARCH;
+}
+
+void action_timeoutStart(int duration)
+{
+  action_done = 0; 
+  // Delete the previous timer, so we can use it's slot.
+  timer.deleteTimer(timer_action); 
+  // Start a new one.
+  timer_action = timer.setTimeout(duration, action_timeoutDone);
+}
+
+void action_timeoutDone()
+{
+  action_done = 1; 
+}
+
+/********************************************************************************
+Motion functions
+********************************************************************************/
+
+void motion_stop()
+{
+  if (motion_state != M_STOP) {
+    analogWrite(motorA_pwm, 0); // Channel A at max speed 
+    analogWrite(motorB_pwm, 0); // Channel B at max speed 
+    motion_state = M_STOP;
+  }
+}
+
+void motion_fwd()
+{
+//@todo: set speed
+  digitalWrite(motorA_direction, HIGH); // Channel A forward
+  digitalWrite(motorB_direction, HIGH); // Channel B forward
+  analogWrite(motorA_pwm, 255); // Channel A at max speed 
+  analogWrite(motorB_pwm, 255); // Channel B at max speed 
+  motion_state = M_FWD;
+}
+
+void motion_rev()
+{
+  if (motion_state != M_REV) {
+//@todo: set speed
+    digitalWrite(motorA_direction, LOW); // Channel A backward
+    digitalWrite(motorB_direction, LOW); // Channel B backward
+    analogWrite(motorA_pwm, 125); // Channel A at half speed 
+    analogWrite(motorB_pwm, 125); // Channel B at half speed   
+    motion_state = M_REV; 
+  }
+}
+
+void motion_rotateLeft()
+{
+  if (motion_state != M_ROTATE_LEFT) {
+    Serial.println("LEFT");
+//@todo: set speed
+    digitalWrite(motorA_direction, HIGH); // Channel A forward
+    digitalWrite(motorB_direction, LOW); // Channel B backward
+    analogWrite(motorA_pwm, 75);
+    analogWrite(motorB_pwm, 75);   
+    motion_state = M_ROTATE_LEFT; 
+  }
+}
+
+void motion_rotateRight()
+{
+  if (motion_state != M_ROTATE_RIGHT) {
+    Serial.println("RIGHT");
+//@todo: set speed
+    digitalWrite(motorA_direction, LOW); // Channel A backward
+    digitalWrite(motorB_direction, HIGH); // Channel B forward
+    analogWrite(motorA_pwm, 75);
+    analogWrite(motorB_pwm, 75);   
+    motion_state = M_ROTATE_RIGHT; 
+  }
+}
+

  
  batteryLevel();
  
  // Turn in place constantly.
  digitalWrite(motorA_direction, HIGH); // Channel A forward
  digitalWrite(motorB_direction, LOW); // Channel B backward
  analogWrite(motorA_pwm, 255); // Channel A at max speed 
  analogWrite(motorB_pwm, 255); // Channel B at max speed
  
  
  
  playMelody();
  //sound_playMelody();
  
  int timer_ping = timer.setInterval(ping_delay, doPing);
  Serial.print("Ping tick started id=");
  Serial.println(timer_ping);
  
  
  int timer_melody = timer.setInterval(melody_delay, playMelody);
  Serial.print("Melody tick started id=");
  Serial.println(timer_melody);
  
  
  int timer_battery = timer.setInterval(battery_delay, batteryLevel);
  Serial.print("Battery tick started id=");

const int ping_distance = 80; // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
const byte ping_delay = 200; // Number of milliseconds to wait before next ping
NewPing sonar(ping_trigger, ping_echo, ping_distance); // NewPing setup of pins and maximum distance.
int ping_timer = 0; // Stores the timer used for the ping sonar.

// Battery configuration
const byte battery_led = 5;
