I2C tested, main file now contains rough sample for use with DHT20

SHAL/Include/Core/SHAL_CORE.h

@@ -11,9 +11,56 @@
 
 #include <cstdint>
 
+//Overall init function for SHAL --------------------------
+
+void SHAL_init();
+
+//---------------------------------------------------------
+
+
+
+
 //Universal structs and defines ---------------------------
 
+typedef bool (*condition_fn_t)(void);
 
+#define SHAL_WAIT_FOR_CONDITION_US(cond, timeout_us) \
+    SHAL_wait_for_condition_us([&](){ return (cond); }, (timeout_us))
+
+#define SHAL_WAIT_FOR_CONDITION_MS(cond, timeout_ms) \
+    SHAL_wait_for_condition_ms([&](){ return (cond); }, (timeout_ms))
+
+
+
+//Currently configures systick to count down in microseconds
+void systick_init();
+
+//Max of 16ms, use SHAL_delay_ms for longer delay
+void SHAL_delay_us(uint32_t us);
+
+void SHAL_delay_ms(uint32_t ms);
+
+template<typename Condition>
+bool SHAL_wait_for_condition_us(Condition cond, uint32_t timeout_us) {
+    while (timeout_us--) {
+        if (cond()) {
+            return true; // success
+        }
+        SHAL_delay_us(1);
+    }
+    return false; // timeout
+}
+
+template<typename Condition>
+bool SHAL_wait_for_condition_ms(Condition cond, uint32_t timeout_ms) {
+    while (timeout_ms--) {
+        if (cond()) {
+            return true; // success
+        }
+        SHAL_delay_ms(1);
+    }
+    return false; // timeout
+}
 
 //---------------------------------------------------------
 

SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG_F072xB.h

@@ -12,6 +12,8 @@
 
 #define NUM_I2C_BUSES 2
 
+#define SHAL_I2C1 I2C(1)
+#define SHAL_I2C2 I2C(2)
 
 enum class I2C_Pair : uint8_t{
     //I2C_1
@@ -40,7 +42,7 @@ constexpr SHAL_I2C_Pair getI2CPair(const I2C_Pair pair){
     __builtin_unreachable();
 }
 
-constexpr SHAL_I2C_Enable_REG getI2CEnableReg(const I2C_Pair pair){
+constexpr SHAL_I2C_Enable_Reg getI2CEnableReg(const I2C_Pair pair){
     switch(pair){
         case I2C_Pair::SCL1B6_SDA1B7:
         case I2C_Pair::SCL1B8_SDA1B9:
@@ -56,4 +58,37 @@ constexpr SHAL_I2C_Enable_REG getI2CEnableReg(const I2C_Pair pair){
     __builtin_unreachable();
 }
 
+constexpr SHAL_I2C_Reset_Reg getI2CResetReg(const I2C_Pair pair){
+    switch(pair){
+        case I2C_Pair::SCL1B6_SDA1B7:
+        case I2C_Pair::SCL1B8_SDA1B9:
+            return {&RCC->APB1RSTR,RCC_APB1RSTR_I2C1RST};
+        case I2C_Pair::SCL2B10_SDA2B11:
+        case I2C_Pair::SCL2B13_SDA2B14:
+            return {&RCC->APB1RSTR,RCC_APB1RSTR_I2C2RST};
+        case I2C_Pair::NUM_PAIRS:
+        case I2C_Pair::INVALID:
+            assert(false);
+            return {nullptr, 0};
+    }
+    __builtin_unreachable();
+}
+
+//Gets all the bits in the I2C timer register, these values should rarely be manually set, but I wanted to support it anyway
+constexpr SHAL_I2C_Timing_Reg getI2CTimerReg(const I2C_Pair pair){
+    switch(pair){
+        case I2C_Pair::SCL1B6_SDA1B7:
+        case I2C_Pair::SCL1B8_SDA1B9:
+            return {&I2C1->TIMINGR,31,23,19,15,7};
+        case I2C_Pair::SCL2B10_SDA2B11:
+        case I2C_Pair::SCL2B13_SDA2B14:
+            return {&I2C2->TIMINGR,31,23,19,15,7};
+        case I2C_Pair::NUM_PAIRS:
+        case I2C_Pair::INVALID:
+            assert(false);
+            __builtin_unreachable();
+    }
+    __builtin_unreachable();
+}
+
 #endif //SHAL_I2C_REG_F072XB_H

SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_TYPES.h

@@ -17,9 +17,24 @@ struct SHAL_I2C_Pair {
     GPIO_Alternate_Function SDA_Mask;
 };
 
-struct SHAL_I2C_Enable_REG{
+struct SHAL_I2C_Enable_Reg{
     volatile uint32_t* reg;
     uint32_t mask;
 };
 
+struct SHAL_I2C_Reset_Reg{
+    volatile uint32_t* reg;
+    uint32_t mask;
+};
+
+//Manual values for I2C timer register
+struct SHAL_I2C_Timing_Reg{
+    volatile uint32_t* reg;
+    uint8_t prescaler_offset;
+    uint8_t dataSetupTime_offset;
+    uint8_t dataHoldTime_offset;
+    uint8_t SCLHighPeriod_offset;
+    uint8_t SCLLowPeriod_offset;
+};
+
 #endif //SHMINGO_HAL_SHAL_I2C_TYPES_H

SHAL/Include/Peripheral/I2C/SHAL_I2C.h

@@ -5,6 +5,8 @@
 #ifndef SHMINGO_HAL_SHAL_I2C_H
 #define SHMINGO_HAL_SHAL_I2C_H
 
+#include <cstdio>
+
 #include "SHAL_CORE.h"
 #include "SHAL_I2C_REG.h"
 
@@ -15,6 +17,37 @@ class SHAL_I2C{
 
 public:
 
+    void init(I2C_Pair pair) volatile;
+
+    /// General I2C function to send commands to a device, then read back any returned data if necessary
+    /// \param addr address of slave device
+    /// \param writeData pointer to array of write commands
+    /// \param writeLen number of write commands
+    /// \param readData pointer to buffer to write received data to
+    /// \param readLen number of bytes to be read
+    void masterWriteRead(uint8_t addr,const uint8_t* writeData, size_t writeLen, uint8_t* readData, size_t readLen);
+
+    uint8_t masterWriteReadByte(uint8_t addr, const uint8_t* writeData, size_t writeLen);
+
+    /// Function to write an array of commands to an I2C device
+    /// \param addr Address of slave device
+    /// \param writeData Pointer to array of commands
+    /// \param writeLen Number of commands
+    void masterWrite(uint8_t addr, const uint8_t* writeData, uint8_t writeLen);
+
+    /// Function to read bytes from an I2C device
+    /// \param addr Address of slave device
+    /// \param readBuffer Pointer to buffer where data will be placed
+    /// \param bytesToRead Number of bytes to read
+    void masterRead(uint8_t addr, uint8_t* readBuffer, uint8_t bytesToRead);
+
+
+    //Manually set the clock configuration. Refer to your MCU's reference manual for examples
+    void setClockConfig(uint8_t prescaler, uint8_t dataSetupTime, uint8_t dataHoldTime, uint8_t SCLHighPeriod, uint8_t SCLLowPeriod);
+
+    //Set clock configuration based on a value calculated from STM32CubeMX, or other similar tools
+    void setClockConfig(uint32_t configuration);
+
 private:
 
     SHAL_I2C() = default;
@@ -23,9 +56,7 @@ private:
 
 };
 
-
-
-
+#define I2C(num) I2CManager::get(num)
 
 class I2CManager{
 
@@ -37,7 +68,7 @@ public:
 
 private:
 
-    inline static SHAL_I2C m_UARTs[NUM_I2C_BUSES] = {};
+    inline static SHAL_I2C m_I2CBuses[NUM_I2C_BUSES] = {};
 
 };
 

SHAL/Include/Peripheral/Timer/Reg/SHAL_TIM_REG_F072xB.h

@@ -29,6 +29,17 @@ enum class Timer_Key : uint8_t { //For STM32F072
     S_TIM_INVALID
 };
 
+#define SHAL_TIM1 TimerManager::get(Timer_Key::S_TIM1)
+#define SHAL_TIM2 TimerManager::get(Timer_Key::S_TIM2)
+#define SHAL_TIM3 TimerManager::get(Timer_Key::S_TIM3)
+#define SHAL_TIM6 TimerManager::get(Timer_Key::S_TIM6)
+#define SHAL_TIM7 TimerManager::get(Timer_Key::S_TIM7)
+#define SHAL_TIM14 TimerManager::get(Timer_Key::S_TIM14)
+#define SHAL_TIM15 TimerManager::get(Timer_Key::S_TIM15)
+#define SHAL_TIM16 TimerManager::get(Timer_Key::S_TIM16)
+#define SHAL_TIM17 TimerManager::get(Timer_Key::S_TIM17)
+
+
 
 //Get TIMER_KEY peripheral struct including bus register, enable mask, TIMER_KEY mask
 constexpr TIM_RCC_Enable getTimerRCC(Timer_Key t) {

SHAL/Include/Peripheral/Timer/SHAL_TIM.h

@@ -18,6 +18,11 @@ class Timer {
     friend class TimerManager;
 public:
 
+    ///
+    /// \param prescaler The amount of times the base clock has to cycle before the timer adds one to the count
+    /// \param autoReload The number of timer counts before the count is reset and IRQ is called
+    void init(uint32_t prescaler, uint32_t autoReload);
+
     //Starts the counter
     void start();
 
@@ -49,12 +54,16 @@ private:
 
 
 #define getTimer(timer_key) TimerManager::get(timer_key)
+#define TIM(num) TimerManager::getTimerFromIndex(num)
 
 //Manages all timers so user does not have to personally initialize
 class TimerManager{
 public:
 
     static Timer& get(Timer_Key);
+
+    static Timer& getTimerFromIndex(uint8_t index){return timers[index];}
+
     TimerManager() = delete;
 
 private:

SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG_F072xB.h

@@ -12,6 +12,11 @@
 
 #define NUM_USART_LINES 4
 
+#define SHAL_UART1 UART(1)
+#define SHAL_UART2 UART(2)
+#define SHAL_UART3 UART(3)
+#define SHAL_UART4 UART(4)
+
 //Valid usart Tx and Rx pairings for STM32F072
 enum class UART_Pair : uint8_t{
     //UART1

SHAL/Include/SHAL.h

@@ -11,5 +11,6 @@
 #include "SHAL_TIM.h"
 #include "SHAL_GPIO.h"
 #include "SHAL_UART.h"
+#include "SHAL_I2C.h"
 
 #endif

SHAL/Src/Core/SHAL_CORE.cpp

@@ -0,0 +1,42 @@
+//
+// Created by Luca on 9/15/2025.
+//
+
+#include "SHAL_CORE.h"
+
+void SHAL_init(){
+    systick_init(); //Just this for now
+}
+
+
+void systick_init(){
+    SysTick->CTRL = 0;           //Disable first
+    SysTick->LOAD = 0xFFFFFF;    //Max 24-bit
+    SysTick->VAL  = 0;           //Clear
+    SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;
+}
+
+
+void SHAL_delay_us(uint32_t us){
+    uint32_t ticks = us * (SystemCoreClock / 1000000U);
+    uint32_t start = SysTick->VAL;
+
+    //Calculate target value (may wrap around)
+    uint32_t target = (start >= ticks) ? (start - ticks) : (start + 0x01000000 - ticks);
+    target &= 0x00FFFFFF;
+
+    //Wait until we reach the target
+    if (start >= ticks) {
+        //No wraparound case
+        while (SysTick->VAL > target) {}
+    } else {
+        while (SysTick->VAL <= start) {} //Wait for wraparound
+        while (SysTick->VAL > target) {} //Wait for target
+    }
+}
+
+void SHAL_delay_ms(uint32_t ms){
+    while(ms-- > 0){
+        SHAL_delay_us(1000);
+    }
+}

SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp

@@ -2,3 +2,141 @@
 // Created by Luca on 9/9/2025.
 //
 
+#include "SHAL_I2C.h"
+#include "SHAL_GPIO.h"
+
+#include "SHAL_UART.h"
+
+void SHAL_I2C::init(I2C_Pair pair) volatile {
+    m_I2CPair = pair;
+
+    SHAL_I2C_Pair I2CPair = getI2CPair(pair); //Get the I2C_PAIR information to be initialized
+
+    //Get the SHAL_GPIO pins for this SHAL_I2C setup
+    GPIO_Key SCL_Key = I2CPair.SCL_Key; //SCL pin
+    GPIO_Key SDA_Key = I2CPair.SDA_Key; //SDA pin
+
+    SHAL_I2C_Enable_Reg pairI2CEnable = getI2CEnableReg(pair); //Register and mask to enable the I2C peripheral
+
+    *pairI2CEnable.reg &= ~pairI2CEnable.mask; //Enable I2C peripheral clock
+
+    GET_GPIO(SCL_Key).setPinMode(PinMode::ALTERNATE_FUNCTION_MODE); //Implicitly initializes and enables GPIO bus
+    GET_GPIO(SDA_Key).setPinMode(PinMode::ALTERNATE_FUNCTION_MODE);
+
+    GET_GPIO(SCL_Key).setAlternateFunction(I2CPair.SCL_Mask);
+    GET_GPIO(SDA_Key).setAlternateFunction(I2CPair.SDA_Mask);
+
+    //These may be abstracted further to support multiple I2C configurations
+    GET_GPIO(SCL_Key).setPinType(PinType::OPEN_DRAIN);
+    GET_GPIO(SDA_Key).setPinType(PinType::OPEN_DRAIN);
+
+    GET_GPIO(SCL_Key).setOutputSpeed(OutputSpeed::HIGH_SPEED);
+    GET_GPIO(SDA_Key).setOutputSpeed(OutputSpeed::HIGH_SPEED);
+
+    GET_GPIO(SCL_Key).setInternalResistor(InternalResistorType::PULLUP);
+    GET_GPIO(SDA_Key).setInternalResistor(InternalResistorType::PULLUP);
+
+    SHAL_I2C_Reset_Reg pairI2CReset = getI2CResetReg(pair);
+
+    *pairI2CEnable.reg |= pairI2CEnable.mask; //Enable I2C peripheral clock
+
+    *pairI2CReset.reg |= pairI2CReset.mask; //Reset peripheral
+    *pairI2CReset.reg &= ~pairI2CReset.mask; //Reset peripheral
+}
+
+void SHAL_I2C::setClockConfig(uint8_t prescaler, uint8_t dataSetupTime, uint8_t dataHoldTime, uint8_t SCLHighPeriod, uint8_t SCLLowPeriod) {
+
+    SHAL_I2C_Timing_Reg clockReg = getI2CTimerReg(m_I2CPair);
+
+    *clockReg.reg |= (prescaler << clockReg.prescaler_offset);
+    *clockReg.reg |= (dataSetupTime << clockReg.dataSetupTime_offset);
+    *clockReg.reg |= (dataHoldTime << clockReg.dataHoldTime_offset);
+    *clockReg.reg |= (SCLHighPeriod << clockReg.SCLHighPeriod_offset);
+    *clockReg.reg |= (SCLLowPeriod << clockReg.SCLLowPeriod_offset);
+
+    getI2CPair(m_I2CPair).I2CReg->CR1 |= I2C_CR1_PE; //Enable I2C peripheral
+}
+
+void SHAL_I2C::setClockConfig(uint32_t configuration) {
+    *getI2CTimerReg(m_I2CPair).reg = configuration;
+
+    getI2CPair(m_I2CPair).I2CReg->CR1 |= I2C_CR1_PE; //Enable I2C peripheral
+}
+
+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;
+
+    if(!SHAL_WAIT_FOR_CONDITION_MS((I2CPeripheral->ISR & I2C_ISR_BUSY) == 0, 100)){
+        SHAL_UART2.sendString("I2C timed out waiting for not busy\r\n");
+        return;
+    }
+
+    //Write phase
+    if (writeLen > 0) {
+        //Configure: NBYTES = wlen, write mode, START
+        I2CPeripheral->CR2 = (addr << 1) | (writeLen << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
+
+        for (size_t i = 0; i < writeLen; i++) {
+            if(!SHAL_WAIT_FOR_CONDITION_MS((I2CPeripheral->ISR & I2C_ISR_TXIS) != 0, 100)){
+                SHAL_UART2.sendString("I2C timed out waiting for TX\r\n");
+                return;
+            }
+            I2CPeripheral->TXDR = writeData[i];
+        }
+
+        //Wait until transfer complete
+        if(!SHAL_WAIT_FOR_CONDITION_MS((I2CPeripheral->ISR & I2C_ISR_TC) != 0, 100)){
+            SHAL_UART2.sendString("I2C timed out waiting for TC\r\n");
+            return;
+        }
+    }
+
+    //Read phase
+    if (readLen > 0) {
+
+        SHAL_UART2.sendString("Read initiated\r\n");
+
+        I2CPeripheral->CR2 &= ~(I2C_CR2_NBYTES | I2C_CR2_SADD | I2C_CR2_RD_WRN);
+        I2CPeripheral->CR2 |= (addr << 1) |
+                              I2C_CR2_RD_WRN |
+                              (readLen << I2C_CR2_NBYTES_Pos) |
+                              I2C_CR2_START | I2C_CR2_AUTOEND;
+
+        for (size_t i = 0; i < readLen; i++) {
+            if(!SHAL_WAIT_FOR_CONDITION_MS((I2CPeripheral->ISR & I2C_ISR_RXNE) != 0 , 100)){
+                SHAL_UART2.sendString("I2C timed out waiting for RXNE\r\n");
+                return;
+            }
+            SHAL_UART2.sendString("Read byte");
+            readData[i] = static_cast<uint8_t>(I2CPeripheral->RXDR);
+        }
+    }
+    else{
+        I2CPeripheral->CR2 |= I2C_CR2_STOP;
+    }
+}
+
+void SHAL_I2C::masterWrite(uint8_t addr, const uint8_t *writeData, uint8_t writeLen) {
+    masterWriteRead(addr,writeData,writeLen,nullptr,0);
+}
+
+void SHAL_I2C::masterRead(uint8_t addr, uint8_t *readBuffer, uint8_t bytesToRead) {
+    masterWriteRead(addr,nullptr,0,readBuffer,bytesToRead);
+}
+
+uint8_t SHAL_I2C::masterWriteReadByte(uint8_t addr, const uint8_t *writeData, size_t writeLen) {
+    uint8_t val = 0;
+    masterWriteRead(addr, writeData, writeLen, &val, 1);
+    return val;
+}
+
+SHAL_I2C& I2CManager::get(uint8_t I2CBus) {
+
+    if(I2CBus > NUM_I2C_BUSES - 1){
+        assert(false);
+        //Memory fault
+    }
+
+    return m_I2CBuses[I2CBus];
+}

SHAL/Src/Peripheral/Timer/SHAL_TIM.cpp

@@ -6,8 +6,7 @@
 #include <cassert>
 
 Timer::Timer(Timer_Key t) : TIMER_KEY(t){
-    TIM_RCC_Enable rcc = getTimerRCC(TIMER_KEY);
-    *rcc.busEnableReg |= (1 << rcc.offset);
+
 }
 
 Timer::Timer() : TIMER_KEY(Timer_Key::S_TIM_INVALID){
@@ -37,6 +36,14 @@ void Timer::enableInterrupt() {
     NVIC_EnableIRQ(getIRQn(TIMER_KEY));
 }
 
+void Timer::init(uint32_t prescaler, uint32_t autoReload) {
+    TIM_RCC_Enable rcc = getTimerRCC(TIMER_KEY);
+    *rcc.busEnableReg |= (1 << rcc.offset);
+
+    setPrescaler(prescaler);
+    setARR(autoReload);
+}
+
 
 Timer &TimerManager::get(Timer_Key timer_key) {
 
@@ -52,3 +59,5 @@ Timer &TimerManager::get(Timer_Key timer_key) {
 
     return timers[static_cast<int>(timer_key)];
 }
+
+

SHAL/Src/Peripheral/UART/SHAL_UART.cpp

@@ -29,6 +29,8 @@ void SHAL_UART::init(const UART_Pair pair){
     SHAL_UART_ENABLE_REG pairUARTEnable = getUARTEnableReg(pair); //Register and mask to enable the SHAL_UART channel
 
     *pairUARTEnable.reg |= pairUARTEnable.mask; //Enable SHAL_UART line
+
+
 }
 
 void SHAL_UART::begin(uint32_t baudRate) volatile {
@@ -63,5 +65,11 @@ void SHAL_UART::sendChar(char c) volatile {
 
 
 SHAL_UART& UARTManager::get(uint8_t uart) {
+
+    if(uart > NUM_USART_LINES - 1){
+        assert(false);
+        //Memory fault
+    }
+
     return m_UARTs[uart];
 }

SHAL/Src/main.cpp

@@ -1,10 +1,44 @@
 #include "SHAL.h"
 #include "stm32f0xx.h"
 
+#include <stdlib.h>
 
 void c3Interrupt(){
-    PIN(A5).toggle();
-    UART(2).sendString("New test");
+    SHAL_UART2.sendString("Begin\r\n");
+
+    uint8_t cmd[3] = {0xAC, 0x33, 0x00};
+    SHAL_I2C1.masterWrite(0x38, cmd, 3);
+
+    SHAL_delay_ms(100);
+
+    uint8_t dht_buf[7] = {0};
+
+    //Read 7 bytes (status + 5 data + CRC)
+    SHAL_I2C1.masterRead(0x38, dht_buf, 7);
+
+    //Parse humidity (20 bits)
+    uint32_t rawHumidity = ((uint32_t)dht_buf[1] << 12) |
+                           ((uint32_t)dht_buf[2] << 4) |
+                           ((uint32_t)dht_buf[3] >> 4);
+
+    uint32_t rawTemp = (((uint32_t)dht_buf[3] & 0x0F) << 16) |
+                       ((uint32_t)dht_buf[4] << 8) |
+                       ((uint32_t)dht_buf[5]);
+
+    // Use 64-bit intermediate to avoid overflow
+    uint32_t hum_hundredths = (uint32_t)(((uint64_t)rawHumidity * 10000ULL) >> 20);
+    int32_t temp_hundredths = (int32_t)((((uint64_t)rawTemp * 20000ULL) >> 20) - 5000);
+
+    char out[80];
+    sprintf(out, "rawH=0x%05lX rawT=0x%05lX\r\n",
+            (unsigned long)rawHumidity, (unsigned long)rawTemp);
+    SHAL_UART2.sendString(out);
+
+    // print as X.YY
+    sprintf(out, "Temp: %ld.%02ld C, Hum: %ld.%02ld %%\r\n",
+            (long)(temp_hundredths / 100), (long)(abs(temp_hundredths % 100)),
+            (long)(hum_hundredths / 100), (long)(hum_hundredths % 100));
+    SHAL_UART2.sendString(out);
 }
 
 void tim2Handler(){
@@ -13,22 +47,43 @@ void tim2Handler(){
 
 int main() {
 
+    SHAL_init();
 
-    UART(2).init(UART_Pair::Tx2A2_Rx2A3);
+    //Setup UART2 (used by nucleo devices for USB comms)
+    SHAL_UART2.init(UART_Pair::Tx2A2_Rx2A3);
+    SHAL_UART2.begin(115200);
 
-    UART(2).begin(115200);
+    SHAL_I2C1.init(I2C_Pair::SCL1B6_SDA1B7);
+    SHAL_I2C1.setClockConfig(0x2000090E);
 
+    //Use pin C3 to trigger a function on external interrupt
     PIN(C3).useAsExternalInterrupt(TriggerMode::RISING_EDGE,c3Interrupt);
 
-    Timer timer2 = getTimer(Timer_Key::S_TIM2);
+    SHAL_TIM2.init(8000-1,1500-1);
+    SHAL_TIM2.setCallbackFunc(tim2Handler);
+    SHAL_TIM2.start();
 
     PIN(A4).setPinMode(PinMode::OUTPUT_MODE);
     PIN(A5).setPinMode(PinMode::OUTPUT_MODE);
 
-    timer2.setPrescaler(8000 - 1);
-    timer2.setARR(1500 - 1);
-    timer2.setCallbackFunc(tim2Handler);
-    timer2.start();
+
+
+    SHAL_delay_ms(3000); //Wait 100 ms from datasheet
+
+    uint8_t cmd = 0x71;
+    uint8_t status = 0;
+
+    SHAL_I2C1.masterWriteRead(0x38, &cmd, 1, &status, 1);
+
+    char statusString[32];
+    sprintf(statusString, "Status = 0x%02X\r\n", status);
+    SHAL_UART2.sendString(statusString);
+
+
+    SHAL_delay_ms(10);
+
+    c3Interrupt();
+    //End setup
 
     while (true) {
     	__WFI();