     if (state == CONFIG) {
         Serial.begin(9600);
         run_config(Serial, FIRMWARE_VERSION);
+
     } else if (state == RUNNING) {
-        Wire.begin(DEFAULT_I2C_SENSOR_ADDRESS);
+        uint8_t i2c_address;
+
+        if (use_custom_address() ) {
+            if (get_i2c_address() > 0) {
+                i2c_address = get_i2c_address();
+            } else {
+                i2c_address = DEFAULT_I2C_SENSOR_ADDRESS;
+            }
+        } else {
+            i2c_address = DEFAULT_I2C_SENSOR_ADDRESS;
+        }
+        
+        Wire.begin(i2c_address);
         Wire.onRequest(requestData);
     }
 

    }

    Serial.println(F("HCSR04 FIRMWARE - DEBUG MODE"));
#endif
}


    }
}

void setup() {

    // check to see if we're in config mode
