   Serial.write(cmd_output);
   Serial.write(out);
   Serial.write(";");    // command delimiter
+  Serial.println();     // newline
 }
+
+// Output elapsed time
+void elapsed_time_output(unsigned long sTime){
+ #define SEC_PER_MIN 60
+ #define SEC_PER_HOUR 3600
+ unsigned long elapsedTime = (millis() - sTime)/1000;
+ Serial.print("Elapsed time is ");
+ Serial.print(elapsedTime/SEC_PER_HOUR);
+ Serial.print(":");
+ Serial.print((elapsedTime/SEC_PER_MIN) % SEC_PER_MIN);
+ Serial.print(":");  
+ Serial.println(elapsedTime % SEC_PER_MIN);
+}


int speed_offset = 31, l = 0, MIDPOINT = 128, 
    p = MIDPOINT, v = MIDPOINT, s = MIDPOINT, t = MIDPOINT;
    
int count;

void setup(){

  Serial.begin(9600);
 
  tilt.attach(5);
  light.attach(6);
  port.attach(9);
  vertical.attach(10);
  starbord.attach(11);

  count = 0;
}

void loop(){
  // ========================================================================================
  // ========================================================================================
  // Get commands from OpenROV
  // ========================================================================================
  // ========================================================================================
  
  if (Serial.available()) {
    delay(30); // number of characters to be read?  buffer delay (TODO:  affects timing of sensors... fix this?)

  // Write commands to OpenROV
  // ========================================================================================
  // ========================================================================================

// TODO: make these actually do something, just testing for now
  // once every interval1
  if (millis() - lastTime > interval1){
    lastTime = millis();  // reset interval1 timer
    count++; // increment interval2 count
    
    if (blink == 0){
      digitalWrite(led, HIGH);  // turn the led on (HIGH is the voltage level)
      blink = 1;  // reset toggle
    }
    else{
      digitalWrite(led, LOW);    // turn the LED off by making the voltage LOW
      blink = 0;  // reset toggle
    }
    
    rov_analog_output(1, "float");  // output value
  }
  
  // once every interval2
  if (count == interval2){
    count=0;  // reset interval2 counter

    // Output elapsed time  
    elapsed_time_output(startTime);

    pin_read = analogRead(0);                 //  read voltage on pin 0 and output to serial port
    voltage=pin_read*scale;                   //  15V will scale to 5V at analog in pin 0 with 3 to 1 voltage divider
    Serial.print("Battery Voltage:");         //  Connect Battery output to analog pin 0 using voltage divider
    Serial.println(voltage);                  //  Nominal Battery voltage is 12V - analog pin input 0-5V
                                              //  Divide by 3 with voltage divider
                                              
  }
}

// Actually create and send command
