   // 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
+  }
   
-  // TODO: make these actually do something, just testing for now
-  // once every 1,300 counts
-  if (count % 1300 == 0){
-    rov_analog_output(1, "float");
+  // 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
+                                              
   }
-  // once every 10,000 counts
-  if (count == 10000){
-    rov_analog_output(0, "salinity");
-    count = 0;  // reset counter (this should be done by the longest "running" counter
-  }
-  count++;
 }
 
 // Actually create and send command


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?)

  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);
}
