diff --git a/src/bin/temp_rh_measurement.rs b/src/bin/temp_rh_measurement.rs
index 03b9482..296cb0a 100644
--- a/src/bin/temp_rh_measurement.rs
+++ b/src/bin/temp_rh_measurement.rs
@@ -6,40 +6,113 @@ use nrf_softdevice::{raw, Softdevice};
 
 use defmt::*;
 use embassy_executor::Spawner;
-use embassy_nrf::twim::{self, Twim};
-use embassy_nrf::{bind_interrupts, peripherals};
+use embassy_nrf::gpio::Pin as GpioPin;
+use embassy_nrf::interrupt;
+use embassy_nrf::twim::{self, Instance as TwimInstance, InterruptHandler, Twim};
+use embassy_nrf::{bind_interrupts, peripherals, Peripheral};
 use {defmt_rtt as _, panic_probe as _};
 
-const ADDRESS: u8 = 0x44;
+struct I2cDriver<'a, T: TwimInstance> {
+    address: u8,
+    twim: Twim<'a, T>,
+}
 
-bind_interrupts!(struct Irqs {
-    TWIM0_TWIS0_TWI0 => twim::InterruptHandler<peripherals::TWI0>;
-});
+trait Driver {
+    /// The msg being mutable ensures that it doesn't have to be copied into RAM for DMA access, as
+    /// it is already there
+    fn write(&mut self, msg: &mut [u8]);
+    fn read(&mut self, buffer: &mut [u8]);
+}
 
-#[embassy_executor::main]
-async fn main(_spawner: Spawner) {
-    let p = embassy_nrf::init(Default::default());
+impl<'a, T: TwimInstance> Driver for I2cDriver<'a, T> {
+    fn write(&mut self, msg: &mut [u8]) {
+        unwrap!(self.twim.blocking_write(self.address, &mut *msg));
+    }
 
-    info!("Initializing TWI...");
+    fn read(&mut self, buffer: &mut [u8]) {
+        unwrap!(self.twim.blocking_read(self.address, &mut *buffer));
+    }
+}
 
-    let config = twim::Config::default();
-    let mut twi = Twim::new(p.TWI0, Irqs, p.P0_14, p.P0_13, config);
+impl<'a, T: TwimInstance> I2cDriver<'a, T> {
+    fn new(
+        address: u8,
+        peripheral: impl Peripheral<P = T> + 'a,
+        sda: impl Peripheral<P = impl GpioPin> + 'a,
+        scl: impl Peripheral<P = impl GpioPin> + 'a,
+        _irq: impl interrupt::typelevel::Binding<T::Interrupt, InterruptHandler<T>> + 'a,
+    ) -> Self {
+        let config = twim::Config::default();
+        let twim = Twim::new(peripheral, _irq, sda, scl, config);
 
-    loop {
-        unwrap!(twi.blocking_write(ADDRESS, &mut [0xFD]));
+        Self { address, twim }
+    }
+}
 
-        Timer::after_millis(10).await;
+struct MockI2cDriver;
 
+impl Driver for MockI2cDriver {
+    fn write(&mut self, _: &mut [u8]) {}
+
+    fn read(&mut self, buffer: &mut [u8]) {
+        buffer[0] = 127;
+        buffer[1] = 128;
+        buffer[2] = 129;
+        buffer[3] = 130;
+        buffer[4] = 131;
+        buffer[5] = 132;
+    }
+}
+
+impl MockI2cDriver {
+    fn new(_: u8) -> Self {
+        Self {}
+    }
+}
+
+struct TempHumiditySensor<T: Driver> {
+    driver: T,
+}
+
+// TODO: Implement CRC checks
+impl<T: Driver> TempHumiditySensor<T> {
+    fn new(driver: T) -> Self {
+        Self { driver }
+    }
+
+    /// The procedure for reading out the temperature and humidity and converting them to floats is
+    /// detailed in the datasheet of the SHT41-AD1B-R2.
+    async fn read_temp_and_humidity(&mut self) -> (f32, f32) {
         let mut buf = [0u8; 6];
-        unwrap!(twi.blocking_read(ADDRESS, &mut buf));
+
+        self.driver.write(&mut [0xFD]);
+        Timer::after_millis(10).await;
+        self.driver.read(&mut buf);
 
         let temp_int = u16::from_be_bytes([buf[0], buf[1]]);
         let temp_float: f32 = -45f32 + 175f32 * (temp_int as f32 / (2u32.pow(16) - 1) as f32);
 
         let rh_int = u16::from_be_bytes([buf[3], buf[4]]);
         let rh_float: f32 = -6f32 + 125f32 * (rh_int as f32 / (2u32.pow(16) - 1) as f32);
-        println!("{}\t{}", temp_float, rh_float);
 
+        return (temp_float, rh_float);
+    }
+}
+
+bind_interrupts!(struct Irqs {
+    SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0 => twim::InterruptHandler<peripherals::TWISPI0>;
+});
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p = embassy_nrf::init(Default::default());
+
+    let driver = I2cDriver::new(0x44, p.TWISPI0, p.P0_14, p.P0_13, Irqs);
+    let mut sensor = TempHumiditySensor::new(driver);
+
+    loop {
+        let (temp, humidity) = sensor.read_temp_and_humidity().await;
+        println!("{} °C\t{} %", temp, humidity);
         Timer::after_millis(50).await;
     }
 }