diff --git a/SHAL/Include/Core/SHAL_CORE.h b/SHAL/Include/Core/SHAL_CORE.h
index 256d0cc..9abbb29 100644
--- a/SHAL/Include/Core/SHAL_CORE.h
+++ b/SHAL/Include/Core/SHAL_CORE.h
@@ -22,6 +22,9 @@ void SHAL_init();
 
 //Universal structs and defines ---------------------------
 
+typedef bool (*condition_fn_t)(void);
+
+
 //Currently configures systick to count down in microseconds
 void systick_init();
 
@@ -30,6 +33,10 @@ void SHAL_delay_us(uint32_t us);
 
 void SHAL_delay_ms(uint32_t ms);
 
+bool SHAL_wait_for_condition_us(condition_fn_t condition, uint32_t timeout_us);
+
+bool SHAL_wait_for_condition_ms(condition_fn_t condition, uint32_t timeout_ms);
+
 //---------------------------------------------------------
 
 
diff --git a/SHAL/Src/Core/SHAL_CORE.cpp b/SHAL/Src/Core/SHAL_CORE.cpp
index 2f6f1b3..6f44eb2 100644
--- a/SHAL/Src/Core/SHAL_CORE.cpp
+++ b/SHAL/Src/Core/SHAL_CORE.cpp
@@ -39,4 +39,24 @@ void SHAL_delay_ms(uint32_t ms){
     while(ms-- > 0){
         SHAL_delay_us(1000);
     }
+}
+
+bool SHAL_wait_for_condition_us(condition_fn_t condition, uint32_t timeout_us){
+    while (timeout_us--) {
+        if (condition()) {
+            return true; // Condition met
+        }
+        SHAL_delay_us(1); // Wait 1 µs
+    }
+    return false; // Timeout
+}
+
+bool SHAL_wait_for_condition_ms(condition_fn_t condition, uint32_t timeout_ms){
+    while (timeout_ms--) {
+        if (condition()) {
+            return true; // Condition met
+        }
+        SHAL_delay_ms(1); // Wait 1 µs
+    }
+    return false; // Timeout
 }
\ No newline at end of file
diff --git a/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp b/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
index cc89305..a6db22e 100644
--- a/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
+++ b/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
@@ -64,39 +64,34 @@ void SHAL_I2C::setClockConfig(uint32_t configuration) {
 }
 
 void SHAL_I2C::masterWriteRead(uint8_t addr,const uint8_t* writeData, size_t writeLen, uint8_t* readData, size_t readLen) {
-    volatile I2C_TypeDef* I2CPeripheral = getI2CPair(m_I2CPair).I2CReg;
 
-    SHAL_UART2.sendString("1\t\n");
+    SHAL_UART2.sendString("Beginning of writeread\r\n");
+
+
+    volatile I2C_TypeDef* I2CPeripheral = getI2CPair(m_I2CPair).I2CReg;
 
     //Wait for I2C  bus
     while (I2CPeripheral->ISR & I2C_ISR_BUSY);
 
-    SHAL_UART2.sendString("2\t\n");
-
-
     //Write phase
     if (writeLen > 0) {
         //Configure: NBYTES = wlen, write mode, START
         I2CPeripheral->CR2 = (addr << 1) | (writeLen << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
 
-        SHAL_UART2.sendString("2.5\t\n");
-
         for (size_t i = 0; i < writeLen; i++) {
             while(!(I2CPeripheral->ISR & I2C_ISR_TXIS)); //TX ready
             I2CPeripheral->TXDR = writeData[i];
         }
 
-        SHAL_UART2.sendString("2.67\t\n");
-
         //Wait until transfer complete
         while (!(I2CPeripheral->ISR & I2C_ISR_TC));
     }
 
-    SHAL_UART2.sendString("3\t\n");
-
-
     //Read phase
     if (readLen > 0) {
+
+        SHAL_UART2.sendString("Read initiated\r\n");
+
         I2CPeripheral->CR2 = (addr << 1) |
                              I2C_CR2_RD_WRN |
                              (readLen << I2C_CR2_NBYTES_Pos) |
@@ -104,12 +99,11 @@ void SHAL_I2C::masterWriteRead(uint8_t addr,const uint8_t* writeData, size_t wri
 
         for (size_t i = 0; i < readLen; i++) {
             while (!(I2CPeripheral->ISR & I2C_ISR_RXNE)); //RX ready
+            SHAL_UART2.sendString("Read byte");
             readData[i] = static_cast<uint8_t>(I2CPeripheral->RXDR);
         }
     }
-
-    SHAL_UART2.sendString("4\t\n");
-
+    SHAL_UART2.sendString("\r\n");
 }
 
 void SHAL_I2C::masterWrite(uint8_t addr, const uint8_t *writeData, uint8_t writeLen) {
diff --git a/SHAL/Src/main.cpp b/SHAL/Src/main.cpp
index 302378c..0db2e09 100644
--- a/SHAL/Src/main.cpp
+++ b/SHAL/Src/main.cpp
@@ -3,8 +3,40 @@
 
 
 void c3Interrupt(){
-    PIN(A5).toggle();
-    SHAL_UART2.sendString("New test");
+    SHAL_UART2.sendString("Begin\r\n");
+
+    uint8_t cmd[3] = {0xAC, 0x33, 0x00};
+    SHAL_I2C1.masterWrite(0x38, cmd, 3);
+
+    SHAL_UART2.sendString("Hello\r\n");
+
+    SHAL_delay_ms(100);
+
+    uint8_t buffer[7] = {0};
+
+    SHAL_UART2.sendString("Buffer created?\r\n");
+
+    //Read 7 bytes (status + 5 data + CRC)
+    SHAL_I2C1.masterRead(0x38, buffer, 7);
+
+    SHAL_UART2.sendString("Read complete\r\n");
+
+    //Parse humidity (20 bits)
+    uint32_t rawHumidity = ((uint32_t)buffer[1] << 12) |
+                           ((uint32_t)buffer[2] << 4) |
+                           ((uint32_t)buffer[3] >> 4);
+
+    // Parse temperature (20 bits)
+    uint32_t rawTemp = (((uint32_t)buffer[3] & 0x0F) << 16) |
+                       ((uint32_t)buffer[4] << 8) |
+                       ((uint32_t)buffer[5]);
+
+    float humidity = (rawHumidity * 100.0f) / 1048576.0f;     // 2^20 = 1048576
+    float temperature = (rawTemp * 200.0f) / 1048576.0f - 50.0f;
+
+    char buf[64];
+    sprintf(buf, "Temp: %.2f C, Hum: %.2f %%\r\n", temperature, humidity);
+    SHAL_UART2.sendString(buf);
 }
 
 void tim2Handler(){
@@ -32,25 +64,18 @@ int main() {
     PIN(A4).setPinMode(PinMode::OUTPUT_MODE);
     PIN(A5).setPinMode(PinMode::OUTPUT_MODE);
 
-    //Temporary setup for DHT20
 
-    PIN(A5).setLow();
 
-    SHAL_delay_ms(5000); //Wait 100 ms from datasheet
+    SHAL_delay_ms(3000); //Wait 100 ms from datasheet
 
-    PIN(A5).setHigh();
+    uint8_t cmd = 0x71;
+    uint8_t status = 0;
 
-    uint8_t initByte[1] = {0x71};
+    SHAL_I2C1.masterWriteRead(0x38, &cmd, 1, &status, 1);
 
-    uint8_t status = SHAL_I2C1.masterWriteReadByte(0xEE,initByte,1);
-
-    if ((status & 0x18) != 0x18) {
-        SHAL_UART2.sendString("DHT ready");
-    } else {
-        SHAL_UART2.sendString("DHT broke");
-    }
-
-    PIN(A5).setLow();
+    char statusString[32];
+    sprintf(statusString, "Status = 0x%02X\r\n", status);
+    SHAL_UART2.sendString(statusString);
 
     //End setup