     gPanServo.writeMicroseconds( gPanServoLimits.convertAngleToPWM( gPanServoAngle ) );
     gTiltServo.writeMicroseconds( gTiltServoLimits.convertAngleToPWM( gTiltServoAngle ) );
     
+    // Periodically kick off an ultrasonic sensor read
+    if ( curTime - gLastUltrasonicReadTimeMS >= SENSOR_READ_INTERVAL_MS
+       && !gbTakingUltrasonicSensorReading )
+    {
+        // Start to measure the range
+        pinMode( ULTRASONIC_PIN, OUTPUT );
+        digitalWrite( ULTRASONIC_PIN, LOW );
+        delayMicroseconds( 2 );
+        digitalWrite( ULTRASONIC_PIN, HIGH );
+        delayMicroseconds( 5 );
+        digitalWrite( ULTRASONIC_PIN, LOW );
+    
+        unsigned long startWaitTimeUS = micros();
+    
+        // Wait for the response pulse to start
+        pinMode( ULTRASONIC_PIN, INPUT );
+        bool bPulseStarted = false; 
+        while ( !bPulseStarted
+           && micros() - startWaitTimeUS < 500 )
+        {
+            bPulseStarted = digitalRead( ULTRASONIC_PIN ) == HIGH;
+        }
+        
+        if ( bPulseStarted )
+        {
+            gUltrasonicPulseStartTimeUS = micros();
+            gbTakingUltrasonicSensorReading = true;
+        }
+        else
+        {
+            // Looks like no sensor is attached. Wait a bit before trying again
+            gLastUltrasonicReadTimeMS = curTime;
+        }
+    }
+    
+    // Check the status of an ongoing ultrasonic read
+    if ( gbTakingUltrasonicSensorReading
+        && digitalRead( ULTRASONIC_PIN ) == LOW )
+    {
+        // Pulse has ended
+        unsigned long durationUS = micros() - gUltrasonicPulseStartTimeUS;
+        
+        gLastUltrasonicReading = durationUS/ULTRASONIC_US_PER_CM;;
+        gLastUltrasonicReadTimeMS = curTime;
+        gbTakingUltrasonicSensorReading = false;
+    }
+    
     // Periodically read from the sensors and send the readings back
     if ( curTime - gLastSensorReadTimeMS >= SENSOR_READ_INTERVAL_MS )
     {

volatile uint8_t gCurMotorWaveTick = 0;

int gLastUltrasonicReading = -1;

// This byte determines which pins are used as digital inputs
// A bit set to 1 indicates that the pin is used as a digital input


//------------------------------------------------------------------------------
const uint8_t VERSION_MAJOR = 0;
const uint8_t VERSION_MINOR = 36;
const uint16_t FIRMWARE_ID = 0xACED;

const uint16_t MAX_MSG_SIZE = 32;
