diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7f5496a..dd62483 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,6 +36,8 @@ set(PROJECT_INCLUDE_DIRECTORIES
         SHAL/Include/Peripheral/GPIO/Reg
         SHAL/Include/Peripheral/UART
         SHAL/Include/Peripheral/UART/Reg
+        SHAL/Include/Peripheral/I2C
+        SHAL/Include/Peripheral/I2C/Reg
         SHAL/Include/Peripheral/EXT/
         ${CMAKE_CURRENT_SOURCE_DIR}/SHAL/Include
 )
diff --git a/Drivers/CMSIS/Device/ST/STM32F0xx/Include/stm32f072xb.h b/Drivers/CMSIS/Device/ST/STM32F0xx/Include/stm32f072xb.h
index 849bade..cbcb0d8 100644
--- a/Drivers/CMSIS/Device/ST/STM32F0xx/Include/stm32f072xb.h
+++ b/Drivers/CMSIS/Device/ST/STM32F0xx/Include/stm32f072xb.h
@@ -369,16 +369,16 @@ typedef struct
 
 typedef struct
 {
-  __IO uint32_t MODER;        /*!< GPIO port mode register,                     Address offset: 0x00      */
-  __IO uint32_t OTYPER;       /*!< GPIO port output type register,              Address offset: 0x04      */
-  __IO uint32_t OSPEEDR;      /*!< GPIO port output speed register,             Address offset: 0x08      */
-  __IO uint32_t PUPDR;        /*!< GPIO port pull-up/pull-down register,        Address offset: 0x0C      */
-  __IO uint32_t IDR;          /*!< GPIO port input data register,               Address offset: 0x10      */
-  __IO uint32_t ODR;          /*!< GPIO port output data register,              Address offset: 0x14      */
-  __IO uint32_t BSRR;         /*!< GPIO port bit set/reset register,      Address offset: 0x1A */
-  __IO uint32_t LCKR;         /*!< GPIO port configuration lock register,       Address offset: 0x1C      */
-  __IO uint32_t AFR[2];       /*!< GPIO alternate function low register,  Address offset: 0x20-0x24 */
-  __IO uint32_t BRR;          /*!< GPIO bit reset register,                     Address offset: 0x28      */
+  __IO uint32_t MODER;        /*!< SHAL_GPIO port mode register,                     Address offset: 0x00      */
+  __IO uint32_t OTYPER;       /*!< SHAL_GPIO port output type register,              Address offset: 0x04      */
+  __IO uint32_t OSPEEDR;      /*!< SHAL_GPIO port output speed register,             Address offset: 0x08      */
+  __IO uint32_t PUPDR;        /*!< SHAL_GPIO port pull-up/pull-down register,        Address offset: 0x0C      */
+  __IO uint32_t IDR;          /*!< SHAL_GPIO port input data register,               Address offset: 0x10      */
+  __IO uint32_t ODR;          /*!< SHAL_GPIO port output data register,              Address offset: 0x14      */
+  __IO uint32_t BSRR;         /*!< SHAL_GPIO port bit set/reset register,      Address offset: 0x1A */
+  __IO uint32_t LCKR;         /*!< SHAL_GPIO port configuration lock register,       Address offset: 0x1C      */
+  __IO uint32_t AFR[2];       /*!< SHAL_GPIO alternate function low register,  Address offset: 0x20-0x24 */
+  __IO uint32_t BRR;          /*!< SHAL_GPIO bit reset register,                     Address offset: 0x28      */
 } GPIO_TypeDef;
 
 /**
@@ -6490,7 +6490,7 @@ typedef struct
 
 /******************************************************************************/
 /*                                                                            */
-/*                       General Purpose IOs (GPIO)                           */
+/*                       General Purpose IOs (SHAL_GPIO)                           */
 /*                                                                            */
 /******************************************************************************/
 /*******************  Bit definition for GPIO_MODER register  *****************/
@@ -10933,7 +10933,7 @@ typedef struct
                                        ((INSTANCE) == DMA1_Channel6) || \
                                        ((INSTANCE) == DMA1_Channel7))
 
-/****************************** GPIO Instances ********************************/
+/****************************** SHAL_GPIO Instances ********************************/
 #define IS_GPIO_ALL_INSTANCE(INSTANCE)  (((INSTANCE) == GPIOA) || \
                                          ((INSTANCE) == GPIOB) || \
                                          ((INSTANCE) == GPIOC) || \
@@ -10941,14 +10941,14 @@ typedef struct
                                          ((INSTANCE) == GPIOE) || \
                                          ((INSTANCE) == GPIOF))
 
-/**************************** GPIO Alternate Function Instances ***************/
+/**************************** SHAL_GPIO Alternate Function Instances ***************/
 #define IS_GPIO_AF_INSTANCE(INSTANCE)   (((INSTANCE) == GPIOA) || \
                                          ((INSTANCE) == GPIOB) || \
                                          ((INSTANCE) == GPIOC) || \
                                          ((INSTANCE) == GPIOD) || \
                                          ((INSTANCE) == GPIOE))
 
-/****************************** GPIO Lock Instances ***************************/
+/****************************** SHAL_GPIO Lock Instances ***************************/
 #define IS_GPIO_LOCK_INSTANCE(INSTANCE) (((INSTANCE) == GPIOA) || \
                                          ((INSTANCE) == GPIOB))
 
@@ -11192,11 +11192,11 @@ typedef struct
 /****************************** TSC Instances *********************************/
 #define IS_TSC_ALL_INSTANCE(INSTANCE) ((INSTANCE) == TSC)
 
-/*********************** UART Instances : IRDA mode ***************************/
+/*********************** SHAL_UART Instances : IRDA mode ***************************/
 #define IS_IRDA_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                     ((INSTANCE) == USART2))
 
-/********************* UART Instances : Smard card mode ***********************/
+/********************* SHAL_UART Instances : Smard card mode ***********************/
 #define IS_SMARTCARD_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                          ((INSTANCE) == USART2))
 
@@ -11210,35 +11210,35 @@ typedef struct
 #define IS_USART_AUTOBAUDRATE_DETECTION_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                                             ((INSTANCE) == USART2))
 
-/******************** UART Instances : Asynchronous mode **********************/
+/******************** SHAL_UART Instances : Asynchronous mode **********************/
 #define IS_UART_INSTANCE(INSTANCE)   (((INSTANCE) == USART1) || \
                                       ((INSTANCE) == USART2) || \
                                       ((INSTANCE) == USART3) || \
                                       ((INSTANCE) == USART4))
 
-/******************** UART Instances : Half-Duplex mode **********************/
+/******************** SHAL_UART Instances : Half-Duplex mode **********************/
 #define IS_UART_HALFDUPLEX_INSTANCE(INSTANCE)   (((INSTANCE) == USART1) || \
                                                  ((INSTANCE) == USART2) || \
                                                  ((INSTANCE) == USART3) || \
                                                  ((INSTANCE) == USART4))
 
-/****************** UART Instances : Hardware Flow control ********************/
+/****************** SHAL_UART Instances : Hardware Flow control ********************/
 #define IS_UART_HWFLOW_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                            ((INSTANCE) == USART2) || \
                                            ((INSTANCE) == USART3) || \
                                            ((INSTANCE) == USART4))
 
-/****************** UART Instances : LIN mode ********************/
+/****************** SHAL_UART Instances : LIN mode ********************/
 #define IS_UART_LIN_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                         ((INSTANCE) == USART2))
 
-/****************** UART Instances : wakeup from stop mode ********************/
+/****************** SHAL_UART Instances : wakeup from stop mode ********************/
 #define IS_UART_WAKEUP_FROMSTOP_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                                     ((INSTANCE) == USART2))
 /* Old macro definition maintained for legacy purpose */
 #define IS_UART_WAKEUP_INSTANCE         IS_UART_WAKEUP_FROMSTOP_INSTANCE
 
-/****************** UART Instances : Driver enable detection ********************/
+/****************** SHAL_UART Instances : Driver enable detection ********************/
 #define IS_UART_DRIVER_ENABLE_INSTANCE(INSTANCE) (((INSTANCE) == USART1) || \
                                                   ((INSTANCE) == USART2) || \
                                                   ((INSTANCE) == USART3) || \
diff --git a/SHAL/Include/Core/SHAL_CORE.h b/SHAL/Include/Core/SHAL_CORE.h
index c37cda4..c891dda 100644
--- a/SHAL/Include/Core/SHAL_CORE.h
+++ b/SHAL/Include/Core/SHAL_CORE.h
@@ -11,11 +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
+}
 
 //---------------------------------------------------------
 
diff --git a/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_REG_F072xB.h b/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_REG_F072xB.h
index b05892f..c12c60a 100644
--- a/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_REG_F072xB.h
+++ b/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_REG_F072xB.h
@@ -20,7 +20,7 @@
     X(C0) X(C1) X(C2) X(C3) X(C4) X(C5) X(C6) X(C7) X(C8) X(C9) X(C10) X(C11) X(C12) X(C13) X(C14) X(C15)
 
 
-//Build enum map of available GPIO pins
+//Build enum map of available SHAL_GPIO pins
 enum class GPIO_Key : uint8_t {
     #define X(key) key,
         AVAILABLE_GPIO
diff --git a/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_TYPES.h b/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_TYPES.h
index a4fa55f..0dcbeb1 100644
--- a/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_TYPES.h
+++ b/SHAL/Include/Peripheral/GPIO/Reg/SHAL_GPIO_TYPES.h
@@ -7,7 +7,6 @@
 
 #include "SHAL_CORE.h"
 
-
 struct SHAL_EXTIO_Register{
     volatile uint32_t* EXT_ICR;
     uint32_t mask;
@@ -24,6 +23,49 @@ struct SHAL_Peripheral_Register {
     unsigned long offset;
 };
 
+enum class PinMode : uint8_t{
+    INPUT_MODE                  = 0x00,
+    OUTPUT_MODE                 = 0x01,
+    ALTERNATE_FUNCTION_MODE     = 0x02,
+    ANALOG_MODE                 = 0x03,
+    INVALID                     = 0x00,
+};
+
+enum class GPIO_Alternate_Function : uint8_t{
+    AF0                         = 0x00,
+    AF1                         = 0x01,
+    AF2                         = 0x02,
+    AF3                         = 0x03,
+    AF4                         = 0x04,
+    AF5                         = 0x05,
+    AF6                         = 0x06,
+    AF7                         = 0x07,
+};
+
+enum class PinType : uint8_t{
+    PUSH_PULL                   = 0x00,
+    OPEN_DRAIN                  = 0x01,
+};
+
+enum class InternalResistorType : uint8_t{
+    NO_PULL                     = 0x00,
+    PULLUP                      = 0x01,
+    PULLDOWN                    = 0x02,
+};
+
+enum class OutputSpeed : uint8_t{
+    LOW_SPEED                   = 0x00,
+    MEDIUM_SPEED                = 0x01,
+    HIGH_SPEED                  = 0x02,
+    VERY_HIGH_SPEED             = 0x03,
+};
+
+enum class TriggerMode : uint8_t{
+    RISING_EDGE,
+    FALLING_EDGE,
+    RISING_FALLING_EDGE
+};
+
 
 
 #endif //SHMINGO_HAL_SHAL_GPIO_TYPES_H
diff --git a/SHAL/Include/Peripheral/GPIO/SHAL_GPIO.h b/SHAL/Include/Peripheral/GPIO/SHAL_GPIO.h
index 01cbb93..793a597 100644
--- a/SHAL/Include/Peripheral/GPIO/SHAL_GPIO.h
+++ b/SHAL/Include/Peripheral/GPIO/SHAL_GPIO.h
@@ -13,23 +13,10 @@
 
 
 
-enum class PinMode : uint8_t{
-    INPUT_MODE,
-    OUTPUT_MODE,
-    ALTERNATE_FUNCTION_MODE,
-    ANALOG_MODE,
-    INVALID
-};
-unsigned long getPinMode(PinMode mode);
 
-enum class TriggerMode : uint8_t{
-    RISING_EDGE,
-    FALLING_EDGE,
-    RISING_FALLING_EDGE
-};
 
-//Abstraction of GPIO registers
-class GPIO{
+//Abstraction of SHAL_GPIO registers
+class SHAL_GPIO{
 
 public:
 
@@ -39,37 +26,55 @@ public:
     void setHigh();
     void setLow();
 
+    void setPinMode(PinMode mode) volatile;
+
+    void setAlternateFunction(GPIO_Alternate_Function AF) volatile;
+
+    void setPinType(PinType type) volatile;
+
+    void setOutputSpeed(OutputSpeed speed) volatile;
+
+    void setInternalResistor(InternalResistorType type) volatile;
+
+
+    void useAsExternalInterrupt(TriggerMode mode, EXTICallback callback);
+
 private:
 
     friend class GPIOManager;
 
-    explicit GPIO(GPIO_Key key, PinMode pinMode);
-    GPIO();
+    explicit SHAL_GPIO(GPIO_Key key);
+    SHAL_GPIO();
 
     GPIO_Key m_GPIO_KEY = GPIO_Key::INVALID;
 
 };
 
-//Init GPIO for normal use
-#define initGPIO(GPIO_KEY, PIN_MODE) GPIOManager::get(GPIO_KEY, PIN_MODE)
 
-//Init GPIO for use as an external interrupt
-#define useGPIOAsInterrupt(GPIO_KEY, Trigger_Mode, Callback) GPIOManager::getInterruptGPIO(GPIO_KEY, Trigger_Mode, Callback)
 
-//Manages instances of GPIO objects
+
+
+//Init SHAL_GPIO for normal use
+#define PIN_TO_KEY(name) GPIO_Key::name
+#define PIN(name) GPIOManager::get(PIN_TO_KEY(name))
+
+#define GET_GPIO(key) GPIOManager::get(key)
+
+#define GPIO_A
+
+//Manages instances of SHAL_GPIO objects
 class GPIOManager{
 
 public:
 
-    static GPIO& get(GPIO_Key, PinMode pinMode);
+    static SHAL_GPIO& get(GPIO_Key);
 
-    static void getInterruptGPIO(GPIO_Key key, TriggerMode mode, EXTICallback callback);
 
     GPIOManager() = delete;
 
 private:
 
-inline static GPIO m_gpios[AVAILABLE_PORTS][PINS_PER_PORT] = {{}};
+inline static SHAL_GPIO m_gpios[AVAILABLE_PORTS][PINS_PER_PORT] = {{}};
 
 };
 
diff --git a/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG.h b/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG.h
new file mode 100644
index 0000000..6ebdccb
--- /dev/null
+++ b/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG.h
@@ -0,0 +1,44 @@
+//
+// Created by Luca on 9/9/2025.
+//
+
+#ifndef SHAL_I2C_REG_H
+#define SHAL_I2C_REG_H
+
+#if defined(STM32F030x6)
+#include "stm32f030x6.h"
+#elif defined(STM32F030x8)
+#include "stm32f030x8.h"
+#elif defined(STM32F031x6)
+#include "stm32f031x6.h"
+#elif defined(STM32F038xx)
+#include "stm32f038xx.h"
+#elif defined(STM32F042x6)
+#include "stm32f042x6.h"
+#elif defined(STM32F048xx)
+#include "stm32f048xx.h"
+#elif defined(STM32F051x8)
+#include "stm32f051x8.h"
+#elif defined(STM32F058xx)
+#include "stm32f058xx.h"
+#elif defined(STM32F070x6)
+#include "stm32f070x6.h"
+#elif defined(STM32F070xB)
+#include "stm32f070xb.h"
+#elif defined(STM32F071xB)
+#include "stm32f071xb.h"
+#elif defined(STM32F072xB)
+#include "SHAL_I2C_REG_F072xB.h"
+#elif defined(STM32F078xx)
+#include "stm32f078xx.h"
+#elif defined(STM32F091xC)
+  #include "stm32f091xc.h"
+#elif defined(STM32F098xx)
+  #include "stm32f098xx.h"
+#elif defined(STM32F030xC)
+  #include "stm32f030xc.h"
+#else
+ #error "Please select first the target STM32F0xx device used in your application (in stm32f0xx.h file)"
+#endif
+
+#endif //SHAL_I2C_REG_H
diff --git a/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG_F072xB.h b/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG_F072xB.h
new file mode 100644
index 0000000..c3431a2
--- /dev/null
+++ b/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_REG_F072xB.h
@@ -0,0 +1,94 @@
+//
+// Created by Luca on 9/9/2025.
+//
+
+#ifndef SHAL_I2C_REG_F072XB_H
+#define SHAL_I2C_REG_F072XB_H
+
+#include "SHAL_CORE.h"
+#include <cassert>
+
+#include "SHAL_I2C_TYPES.h"
+
+#define NUM_I2C_BUSES 2
+
+#define SHAL_I2C1 I2C(1)
+#define SHAL_I2C2 I2C(2)
+
+enum class I2C_Pair : uint8_t{
+    //I2C_1
+    SCL1B6_SDA1B7, //AF1
+    SCL1B8_SDA1B9, //AF1
+
+    //I2C_2
+    SCL2B10_SDA2B11, //AF1
+    SCL2B13_SDA2B14, //AF5
+
+    NUM_PAIRS,
+    INVALID
+};
+
+constexpr SHAL_I2C_Pair getI2CPair(const I2C_Pair pair){
+    switch(pair){
+        case I2C_Pair::SCL1B6_SDA1B7: return {I2C1,GPIO_Key::B6,GPIO_Key::B7,GPIO_Alternate_Function::AF1,GPIO_Alternate_Function::AF1};
+        case I2C_Pair::SCL1B8_SDA1B9: return {I2C1,GPIO_Key::B8,GPIO_Key::B9,GPIO_Alternate_Function::AF1,GPIO_Alternate_Function::AF1};
+        case I2C_Pair::SCL2B10_SDA2B11: return {I2C2,GPIO_Key::B10,GPIO_Key::B11,GPIO_Alternate_Function::AF1,GPIO_Alternate_Function::AF1};
+        case I2C_Pair::SCL2B13_SDA2B14: return {I2C2,GPIO_Key::B13,GPIO_Key::B14,GPIO_Alternate_Function::AF5,GPIO_Alternate_Function::AF5};
+        case I2C_Pair::NUM_PAIRS:
+        case I2C_Pair::INVALID:
+            assert(false);
+            return {nullptr,GPIO_Key::INVALID,GPIO_Key::INVALID,GPIO_Alternate_Function::AF0,GPIO_Alternate_Function::AF0};
+    }
+    __builtin_unreachable();
+}
+
+constexpr SHAL_I2C_Enable_Reg getI2CEnableReg(const I2C_Pair pair){
+    switch(pair){
+        case I2C_Pair::SCL1B6_SDA1B7:
+        case I2C_Pair::SCL1B8_SDA1B9:
+            return {&RCC->APB1ENR,RCC_APB1ENR_I2C1EN};
+        case I2C_Pair::SCL2B10_SDA2B11:
+        case I2C_Pair::SCL2B13_SDA2B14:
+            return {&RCC->APB1ENR,RCC_APB1ENR_I2C2EN};
+        case I2C_Pair::NUM_PAIRS:
+        case I2C_Pair::INVALID:
+            assert(false);
+            return {nullptr, 0};
+    }
+    __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
diff --git a/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_TYPES.h b/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_TYPES.h
new file mode 100644
index 0000000..d83ff4d
--- /dev/null
+++ b/SHAL/Include/Peripheral/I2C/Reg/SHAL_I2C_TYPES.h
@@ -0,0 +1,40 @@
+//
+// Created by Luca on 9/9/2025.
+//
+
+#ifndef SHMINGO_HAL_SHAL_I2C_TYPES_H
+#define SHMINGO_HAL_SHAL_I2C_TYPES_H
+
+#include "SHAL_CORE.h"
+#include "SHAL_GPIO_REG.h"
+
+//Information necessary for I2C
+struct SHAL_I2C_Pair {
+    I2C_TypeDef* I2CReg;
+    GPIO_Key SCL_Key;
+    GPIO_Key SDA_Key;
+    GPIO_Alternate_Function SCL_Mask;
+    GPIO_Alternate_Function SDA_Mask;
+};
+
+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
diff --git a/SHAL/Include/Peripheral/I2C/SHAL_I2C.h b/SHAL/Include/Peripheral/I2C/SHAL_I2C.h
new file mode 100644
index 0000000..a765d45
--- /dev/null
+++ b/SHAL/Include/Peripheral/I2C/SHAL_I2C.h
@@ -0,0 +1,75 @@
+//
+// Created by Luca on 9/9/2025.
+//
+
+#ifndef SHMINGO_HAL_SHAL_I2C_H
+#define SHMINGO_HAL_SHAL_I2C_H
+
+#include <cstdio>
+
+#include "SHAL_CORE.h"
+#include "SHAL_I2C_REG.h"
+
+
+class SHAL_I2C{
+
+    friend class I2CManager;
+
+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;
+
+    I2C_Pair m_I2CPair = I2C_Pair::INVALID; //Initialize to invalid
+
+};
+
+#define I2C(num) I2CManager::get(num)
+
+class I2CManager{
+
+public:
+
+    static SHAL_I2C& get(uint8_t i2c);
+
+    I2CManager() = delete;
+
+private:
+
+    inline static SHAL_I2C m_I2CBuses[NUM_I2C_BUSES] = {};
+
+};
+
+#endif //SHMINGO_HAL_SHAL_I2C_H
diff --git a/SHAL/Include/Peripheral/Timer/Reg/SHAL_TIM_REG_F072xB.h b/SHAL/Include/Peripheral/Timer/Reg/SHAL_TIM_REG_F072xB.h
index 8b373ca..f4bf7cd 100644
--- a/SHAL/Include/Peripheral/Timer/Reg/SHAL_TIM_REG_F072xB.h
+++ b/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) {
diff --git a/SHAL/Include/Peripheral/Timer/SHAL_TIM.h b/SHAL/Include/Peripheral/Timer/SHAL_TIM.h
index 484b75e..8c431d0 100644
--- a/SHAL/Include/Peripheral/Timer/SHAL_TIM.h
+++ b/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:
diff --git a/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG.h b/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG.h
index ec6009c..a89c0da 100644
--- a/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG.h
+++ b/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG.h
@@ -5,10 +5,6 @@
 #ifndef SHAL_UART_REG_H
 #define SHAL_UART_REG_H
 
-//
-// Created by Luca on 9/6/2025.
-//
-
 
 #if defined(STM32F030x6)
 #include "stm32f030x6.h"
diff --git a/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG_F072xB.h b/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG_F072xB.h
index ff67299..975bff5 100644
--- a/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_REG_F072xB.h
+++ b/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
@@ -38,19 +43,19 @@ enum class UART_Pair : uint8_t{
 
 constexpr SHAL_UART_Pair getUARTPair(const UART_Pair pair){
     switch(pair){
-        case UART_Pair::Tx1A9_Rx1A10: return {USART1,GPIO_Key::A9,GPIO_Key::A10,AF_Mask::AF1,AF_Mask::AF1};
-        case UART_Pair::Tx1B6_Rx1B7: return {USART1,GPIO_Key::B6,GPIO_Key::B7,AF_Mask::AF0,AF_Mask::AF0};
-        case UART_Pair::Tx2A2_Rx2A3: return {USART2,GPIO_Key::A2,GPIO_Key::A3,AF_Mask::AF1,AF_Mask::AF1};
-        case UART_Pair::Tx2A14_Rx2A15: return {USART2,GPIO_Key::A14,GPIO_Key::A15,AF_Mask::AF1,AF_Mask::AF1};
-        case UART_Pair::Tx3B10_Rx3B11: return {USART3,GPIO_Key::B10,GPIO_Key::B11,AF_Mask::AF4,AF_Mask::AF4};
-        case UART_Pair::Tx3C4_Rx3C5: return {USART3,GPIO_Key::C4,GPIO_Key::C5,AF_Mask::AF1,AF_Mask::AF1};
-        case UART_Pair::Tx3C10_Rx3C11: return {USART3,GPIO_Key::C10,GPIO_Key::C11,AF_Mask::AF1,AF_Mask::AF1};
-        case UART_Pair::Tx4A0_Rx4A1: return {USART4,GPIO_Key::A0,GPIO_Key::A1,AF_Mask::AF4,AF_Mask::AF4};
-        case UART_Pair::Tx4C10_Rx4C11: return {USART4,GPIO_Key::C10,GPIO_Key::C11,AF_Mask::AF0,AF_Mask::AF0};
+        case UART_Pair::Tx1A9_Rx1A10: return {USART1, GPIO_Key::A9, GPIO_Key::A10, GPIO_Alternate_Function::AF1, GPIO_Alternate_Function::AF1};
+        case UART_Pair::Tx1B6_Rx1B7: return {USART1, GPIO_Key::B6, GPIO_Key::B7, GPIO_Alternate_Function::AF0, GPIO_Alternate_Function::AF0};
+        case UART_Pair::Tx2A2_Rx2A3: return {USART2, GPIO_Key::A2, GPIO_Key::A3, GPIO_Alternate_Function::AF1, GPIO_Alternate_Function::AF1};
+        case UART_Pair::Tx2A14_Rx2A15: return {USART2, GPIO_Key::A14, GPIO_Key::A15, GPIO_Alternate_Function::AF1, GPIO_Alternate_Function::AF1};
+        case UART_Pair::Tx3B10_Rx3B11: return {USART3, GPIO_Key::B10, GPIO_Key::B11, GPIO_Alternate_Function::AF4, GPIO_Alternate_Function::AF4};
+        case UART_Pair::Tx3C4_Rx3C5: return {USART3, GPIO_Key::C4, GPIO_Key::C5, GPIO_Alternate_Function::AF1, GPIO_Alternate_Function::AF1};
+        case UART_Pair::Tx3C10_Rx3C11: return {USART3, GPIO_Key::C10, GPIO_Key::C11, GPIO_Alternate_Function::AF1, GPIO_Alternate_Function::AF1};
+        case UART_Pair::Tx4A0_Rx4A1: return {USART4, GPIO_Key::A0, GPIO_Key::A1, GPIO_Alternate_Function::AF4, GPIO_Alternate_Function::AF4};
+        case UART_Pair::Tx4C10_Rx4C11: return {USART4, GPIO_Key::C10, GPIO_Key::C11, GPIO_Alternate_Function::AF0, GPIO_Alternate_Function::AF0};
         case UART_Pair::NUM_PAIRS:
         case UART_Pair::INVALID:
             assert(false);
-            return {nullptr,GPIO_Key::INVALID,GPIO_Key::INVALID,AF_Mask::AF0,AF_Mask::AF0};
+            return {nullptr, GPIO_Key::INVALID, GPIO_Key::INVALID, GPIO_Alternate_Function::AF0, GPIO_Alternate_Function::AF0};
     }
     __builtin_unreachable();
 }
@@ -101,16 +106,16 @@ constexpr SHAL_UART_ENABLE_REG getUARTEnableReg(const UART_Pair pair){
     __builtin_unreachable();
 }
 
-constexpr uint32_t getAFMask(const AF_Mask mask){
+constexpr uint32_t getAFMask(const GPIO_Alternate_Function mask){
     switch(mask){
-        case AF_Mask::AF0: return 0x00;
-        case AF_Mask::AF1: return 0x01;
-        case AF_Mask::AF2: return 0x02;
-        case AF_Mask::AF3: return 0x03;
-        case AF_Mask::AF4: return 0x04;
-        case AF_Mask::AF5: return 0x05;
-        case AF_Mask::AF6: return 0x06;
-        case AF_Mask::AF7: return 0x07;
+        case GPIO_Alternate_Function::AF0: return 0x00;
+        case GPIO_Alternate_Function::AF1: return 0x01;
+        case GPIO_Alternate_Function::AF2: return 0x02;
+        case GPIO_Alternate_Function::AF3: return 0x03;
+        case GPIO_Alternate_Function::AF4: return 0x04;
+        case GPIO_Alternate_Function::AF5: return 0x05;
+        case GPIO_Alternate_Function::AF6: return 0x06;
+        case GPIO_Alternate_Function::AF7: return 0x07;
     }
     __builtin_unreachable();
 }
diff --git a/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_TYPES.h b/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_TYPES.h
index 1220373..b6a658d 100644
--- a/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_TYPES.h
+++ b/SHAL/Include/Peripheral/UART/Reg/SHAL_UART_TYPES.h
@@ -8,25 +8,15 @@
 #include "SHAL_CORE.h"
 #include "SHAL_GPIO_REG.h"
 
-enum class AF_Mask : uint8_t{
-    AF0,
-    AF1,
-    AF2,
-    AF3,
-    AF4,
-    AF5,
-    AF6,
-    AF7
-};
+
 
 //Represents a pair of pins usable for USART Tx + Rx in combination, and their alternate function mapping
 struct SHAL_UART_Pair{
-
     USART_TypeDef* USARTReg;
     GPIO_Key TxKey;
     GPIO_Key RxKey;
-    AF_Mask TxMask;
-    AF_Mask RxMask;
+    GPIO_Alternate_Function TxAlternateFunctionMask;
+    GPIO_Alternate_Function RxAlternateFunctionMask;
 };
 
 struct SHAL_UART_ENABLE_REG{
diff --git a/SHAL/Include/Peripheral/UART/SHAL_UART.h b/SHAL/Include/Peripheral/UART/SHAL_UART.h
index 2a6ebfc..e1cbb45 100644
--- a/SHAL/Include/Peripheral/UART/SHAL_UART.h
+++ b/SHAL/Include/Peripheral/UART/SHAL_UART.h
@@ -2,7 +2,7 @@
   ******************************************************************************
   * @file    SHAL_TIM.h
   * @author  Luca Lizaranzu
-  * @brief   Relating to UART and USART object abstractions
+  * @brief   Relating to SHAL_UART and USART object abstractions
   ******************************************************************************
   */
 
@@ -11,11 +11,13 @@
 
 #include "SHAL_UART_REG.h"
 
-class UART{
+class SHAL_UART{
 
     friend class UARTManager;
 public:
 
+    void init(UART_Pair pair);
+
     //begins Tx and Usart TODO either modify this function or add a new one that supports Rx
     void begin(uint32_t baudRate) volatile;
 
@@ -27,30 +29,28 @@ public:
 
 private:
 
-    UART() = default; //Initializer for array
+    SHAL_UART() = default; //Initializer for array
 
-    //Creates a UART based on a pair of two valid U(S)ART pins
-    explicit UART(UART_Pair pair);
+    //Creates a SHAL_UART based on a pair of two valid U(S)ART pins
 
     UART_Pair m_UARTPair = UART_Pair::INVALID;
 
 };
 
 
-#define getUART(uart_pair) UARTManager::get(uart_pair)
+#define UART(num) UARTManager::get(num)
 
 class UARTManager{
 
 public:
 
-    static UART& get(UART_Pair pair);
-
+    static SHAL_UART& get(uint8_t uart);
 
     UARTManager() = delete;
 
 private:
 
-    inline static UART m_UARTs[NUM_USART_LINES] = {};
+    inline static SHAL_UART m_UARTs[NUM_USART_LINES] = {};
 
 };
 
diff --git a/SHAL/Include/SHAL.h b/SHAL/Include/SHAL.h
index 9756334..2e6d6de 100644
--- a/SHAL/Include/SHAL.h
+++ b/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
diff --git a/SHAL/Src/Core/SHAL_CORE.cpp b/SHAL/Src/Core/SHAL_CORE.cpp
new file mode 100644
index 0000000..8f46a8f
--- /dev/null
+++ b/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);
+    }
+}
diff --git a/SHAL/Src/Peripheral/GPIO/SHAL_GPIO.cpp b/SHAL/Src/Peripheral/GPIO/SHAL_GPIO.cpp
index 2c6a2b8..dea9db5 100644
--- a/SHAL/Src/Peripheral/GPIO/SHAL_GPIO.cpp
+++ b/SHAL/Src/Peripheral/GPIO/SHAL_GPIO.cpp
@@ -5,97 +5,87 @@
 #include "SHAL_GPIO.h"
 #include "SHAL_EXTI_CALLBACK.h"
 
-unsigned long getPinMode(PinMode mode){
-    switch(mode){
-        case PinMode::INPUT_MODE:
-            return 0b00;
-        case PinMode::OUTPUT_MODE:
-            return 0b01;
-        case PinMode::ALTERNATE_FUNCTION_MODE:
-            return 0b10;
-        case PinMode::ANALOG_MODE:
-            return 0b11;
-        case PinMode::INVALID:
-            assert(false);
-            return 0;
-    }
-    __builtin_unreachable();
-}
 
 
-
-
-GPIO::GPIO() : m_GPIO_KEY(GPIO_Key::INVALID){
+SHAL_GPIO::SHAL_GPIO() : m_GPIO_KEY(GPIO_Key::INVALID){
     //Do not initialize anything
 }
 
-GPIO::GPIO(GPIO_Key key, PinMode pinMode) : m_GPIO_KEY(key) {
-
-    SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(key);
-
-    auto gpioRegister = gpioPeripheral.reg;
-    unsigned long registerOffset = gpioPeripheral.global_offset;
+SHAL_GPIO::SHAL_GPIO(GPIO_Key key) : m_GPIO_KEY(key) {
 
     volatile unsigned long* gpioEnable = getGPIORCCEnable(key).reg;
     unsigned long gpioOffset = getGPIORCCEnable(key).offset;
 
     *gpioEnable |= (1 << gpioOffset); //Set enable flag
-
-    gpioRegister->MODER &= ~(0b11 << (2 * registerOffset)); //Clear any previous mode
-    gpioRegister->MODER |= (getPinMode(pinMode) << (2 * registerOffset)); //Set mode based on pinmode bit structure
 }
 
-void GPIO::setLow() {
+void SHAL_GPIO::setLow() {
     auto gpioPeripheral = getGPIORegister(m_GPIO_KEY);
     gpioPeripheral.reg->ODR &= ~(1 << gpioPeripheral.global_offset);
 }
 
-void GPIO::setHigh() {
+void SHAL_GPIO::setHigh() {
     auto gpioPeripheral = getGPIORegister(m_GPIO_KEY);
     gpioPeripheral.reg->ODR |= (1 << gpioPeripheral.global_offset);
 }
 
-void GPIO::toggle() volatile {
+void SHAL_GPIO::toggle() volatile {
     SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(m_GPIO_KEY);
     gpioPeripheral.reg->ODR ^= (1 << gpioPeripheral.global_offset);
 }
 
 
 
-GPIO& GPIOManager::get(GPIO_Key key, PinMode pinMode) {
-
-    unsigned int gpioPort = getGPIOPortNumber(key);
-    unsigned long gpioPin = getGPIORegister(key).global_offset; //Use existing structs to get offset
-
-    if (m_gpios[gpioPort][gpioPin].m_GPIO_KEY == GPIO_Key::INVALID){
-        m_gpios[gpioPort][gpioPin] = GPIO(key,pinMode);
-    }
-
-    return m_gpios[gpioPort][gpioPin];
+void SHAL_GPIO::setPinType(PinType type) volatile {
+    SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(m_GPIO_KEY);
+    gpioPeripheral.reg->OTYPER &= ~(1 << gpioPeripheral.global_offset);
+    gpioPeripheral.reg->OTYPER |= (static_cast<uint8_t>(type) << gpioPeripheral.global_offset);
 }
 
-void GPIOManager::getInterruptGPIO(GPIO_Key key, TriggerMode triggerMode, EXTICallback callback) {
+void SHAL_GPIO::setOutputSpeed(OutputSpeed speed) volatile {
+    SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(m_GPIO_KEY);
+    gpioPeripheral.reg->OSPEEDR |= (static_cast<uint8_t>(speed) << (2 * gpioPeripheral.global_offset));
+}
 
-    uint32_t gpioPort = getGPIOPortNumber(key);
-    uint32_t gpioPin = getGPIORegister(key).global_offset; //Use existing structs to get offset
+void SHAL_GPIO::setInternalResistor(InternalResistorType type) volatile {
+    SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(m_GPIO_KEY);
+    gpioPeripheral.reg->PUPDR &= ~(0x03 << (2 * gpioPeripheral.global_offset));
+    gpioPeripheral.reg->PUPDR |= (static_cast<uint8_t>(type) << (2 * gpioPeripheral.global_offset));
+}
 
-    if (m_gpios[gpioPort][gpioPin].m_GPIO_KEY == GPIO_Key::INVALID){
-        m_gpios[gpioPort][gpioPin] = GPIO(key,PinMode::INPUT_MODE); //Hardcode input mode for interrupt
-    }
+void SHAL_GPIO::setAlternateFunction(GPIO_Alternate_Function AF) volatile {
+    SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(m_GPIO_KEY);
+
+    int afrIndex = gpioPeripheral.global_offset < 8 ? 0 : 1; //Get index of AFR
+
+    gpioPeripheral.reg->AFR[afrIndex] &= ~(0xF << (gpioPeripheral.global_offset * 4));
+    gpioPeripheral.reg->AFR[afrIndex] |= (static_cast<int>(AF) << (gpioPeripheral.global_offset * 4));
+}
+
+void SHAL_GPIO::setPinMode(PinMode mode) volatile {
+    SHAL_GPIO_Peripheral gpioPeripheral = getGPIORegister(m_GPIO_KEY);
+    gpioPeripheral.reg->MODER &= ~(0x03 << (2 * gpioPeripheral.global_offset)); //Clear any previous mode
+    gpioPeripheral.reg->MODER |= (static_cast<uint8_t>(mode) << (2 * gpioPeripheral.global_offset)); //Set mode based on pinmode bit structure
+}
+
+void SHAL_GPIO::useAsExternalInterrupt(TriggerMode mode, EXTICallback callback) {
+
+    uint32_t gpioPin = getGPIORegister(m_GPIO_KEY).global_offset; //Use existing structs to get offset
+
+    setPinMode(PinMode::INPUT_MODE); //Explicitly set mode to input
 
     RCC->APB2ENR |= RCC_APB2ENR_SYSCFGCOMPEN; //Enable EXT, TODO check if this is different across STM32 models
-    NVIC_EnableIRQ(getGPIOEXTICR(key).IRQN); //Enable IRQN for pin
+    NVIC_EnableIRQ(getGPIOEXTICR(m_GPIO_KEY).IRQN); //Enable IRQN for pin
     EXTI->IMR |= (1 << gpioPin); //Enable correct EXTI line
 
-    SHAL_EXTIO_Register EXTILineEnable = getGPIOEXTICR(key);
+    SHAL_EXTIO_Register EXTILineEnable = getGPIOEXTICR(m_GPIO_KEY);
     *EXTILineEnable.EXT_ICR |= EXTILineEnable.mask; //Set bits to enable correct port on correct line TODO Find way to clear bits before
 
-
     uint32_t rising_mask = 0x00;
     uint32_t falling_mask = 0x00;
 
     //Set rising and falling edge triggers based on pin offset (enabled EXTI line)
-    switch(triggerMode){
+    switch(mode){
         case TriggerMode::RISING_EDGE:
             rising_mask = 1 << gpioPin;
             break;
@@ -112,7 +102,20 @@ void GPIOManager::getInterruptGPIO(GPIO_Key key, TriggerMode triggerMode, EXTICa
     EXTI->FTSR |= falling_mask;
 
     //Set callback
-    registerEXTICallback(key,callback);
+    registerEXTICallback(m_GPIO_KEY,callback);
 
     __enable_irq(); //Enable IRQ just in case
+}
+
+
+SHAL_GPIO& GPIOManager::get(GPIO_Key key) {
+
+    unsigned int gpioPort = getGPIOPortNumber(key);
+    unsigned long gpioPin = getGPIORegister(key).global_offset; //Use existing structs to get offset
+
+    if (m_gpios[gpioPort][gpioPin].m_GPIO_KEY == GPIO_Key::INVALID){
+        m_gpios[gpioPort][gpioPin] = SHAL_GPIO(key);
+    }
+
+    return m_gpios[gpioPort][gpioPin];
 }
\ No newline at end of file
diff --git a/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp b/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
new file mode 100644
index 0000000..b8ea899
--- /dev/null
+++ b/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
@@ -0,0 +1,142 @@
+//
+// 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];
+}
diff --git a/SHAL/Src/Peripheral/Timer/SHAL_TIM.cpp b/SHAL/Src/Peripheral/Timer/SHAL_TIM.cpp
index fa0601d..24b39b2 100644
--- a/SHAL/Src/Peripheral/Timer/SHAL_TIM.cpp
+++ b/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)];
 }
+
+
diff --git a/SHAL/Src/Peripheral/UART/SHAL_UART.cpp b/SHAL/Src/Peripheral/UART/SHAL_UART.cpp
index 990ff41..237ea24 100644
--- a/SHAL/Src/Peripheral/UART/SHAL_UART.cpp
+++ b/SHAL/Src/Peripheral/UART/SHAL_UART.cpp
@@ -2,7 +2,7 @@
   ******************************************************************************
   * @file    SHAL_TIM.h
   * @author  Luca Lizaranzu
-  * @brief   Related to USART and UART abstractions
+  * @brief   Related to USART and SHAL_UART abstractions
   ******************************************************************************
   */
 
@@ -10,38 +10,30 @@
 #include "SHAL_UART.h"
 #include "SHAL_GPIO.h"
 
-UART::UART(const UART_Pair pair) : m_UARTPair(pair){
+void SHAL_UART::init(const UART_Pair pair){
+
+    m_UARTPair = pair;
+
     SHAL_UART_Pair uart_pair = getUARTPair(pair); //Get the UART_PAIR information to be initialized
 
-    //Get the GPIO pins for this UART setup
+    //Get the SHAL_GPIO pins for this SHAL_UART setup
     GPIO_Key Tx_Key = uart_pair.TxKey; //Tx pin
     GPIO_Key Rx_Key = uart_pair.RxKey; //Rx pin
 
-    uint8_t Tx_Pin = getGPIORegister(Tx_Key).global_offset;
-    uint8_t Rx_Pin = getGPIORegister(Rx_Key).global_offset;
+    GET_GPIO(Tx_Key).setPinMode(PinMode::ALTERNATE_FUNCTION_MODE);
+    GET_GPIO(Rx_Key).setPinMode(PinMode::ALTERNATE_FUNCTION_MODE);
 
-    initGPIO(Tx_Key,PinMode::ALTERNATE_FUNCTION_MODE); //Initialize Tx GPIO with alternate function (initializes GPIO port as well)
-    initGPIO(Rx_Key,PinMode::ALTERNATE_FUNCTION_MODE); //Initialize Rx GPIO with alternate function
+    GET_GPIO(Tx_Key).setAlternateFunction(uart_pair.TxAlternateFunctionMask);
+    GET_GPIO(Rx_Key).setAlternateFunction(uart_pair.RxAlternateFunctionMask);
 
-    //Determine which AFR register (high or low) to write depending on pin
-    uint8_t TxAFR = Tx_Pin < 8 ? 0 : 1; //Use AFR[0] if pin < 8, AFR[1] if pin >= 8
-    uint8_t RxAFR = Rx_Pin < 8 ? 0 : 1;
+    SHAL_UART_ENABLE_REG pairUARTEnable = getUARTEnableReg(pair); //Register and mask to enable the SHAL_UART channel
 
-    /*Apply Alternate Function masks to the AFR registers for each GPIO to enable alternate functions
-     * The AFR register for GPIO_Typedef* is actually two registers - a low reg and high reg.
-     * The low reg handles pins 0-7, and the high reg handles 8-15.
-     * Each pin gets 4 bits in the register for AFR0 - AFR7. Hence 8 * 4 = 32 bits.
-     * Each AFR is a different function, look at the DATASHEET (not reference manual) to find these alternate function mappings
-     */
-    getGPIORegister(Tx_Key).reg->AFR[TxAFR] |= getAFMask(uart_pair.TxMask) << (4 * (Tx_Pin % 8));
-    getGPIORegister(Rx_Key).reg->AFR[RxAFR] |= getAFMask(uart_pair.RxMask) << (4 * (Rx_Pin % 8));
+    *pairUARTEnable.reg |= pairUARTEnable.mask; //Enable SHAL_UART line
 
-    SHAL_UART_ENABLE_REG pairUARTEnable = getUARTEnableReg(pair); //Register and mask to enable the UART channel
 
-    *pairUARTEnable.reg |= pairUARTEnable.mask; //Enable UART line
 }
 
-void UART::begin(uint32_t baudRate) volatile {
+void SHAL_UART::begin(uint32_t baudRate) volatile {
 
     USART_TypeDef* usart = getUARTPair(m_UARTPair).USARTReg;
 
@@ -57,11 +49,11 @@ void UART::begin(uint32_t baudRate) volatile {
 
 }
 
-void UART::sendString(const char *s) volatile {
+void SHAL_UART::sendString(const char *s) volatile {
     while (*s) sendChar(*s++); //Send chars while we haven't reached end of s
 }
 
-void UART::sendChar(char c) volatile {
+void SHAL_UART::sendChar(char c) volatile {
 
     USART_TypeDef* usart = getUARTPair(m_UARTPair).USARTReg;
 
@@ -72,11 +64,12 @@ void UART::sendChar(char c) volatile {
 
 
 
-UART& UARTManager::get(UART_Pair pair) {
+SHAL_UART& UARTManager::get(uint8_t uart) {
 
-    //Reassign if pair doesn't match
-    if(m_UARTs[getUARTChannel(pair)].m_UARTPair != pair) {
-        m_UARTs[getUARTChannel(pair)] = UART(pair);
+    if(uart > NUM_USART_LINES - 1){
+        assert(false);
+        //Memory fault
     }
-    return m_UARTs[getUARTChannel(pair)];
+
+    return m_UARTs[uart];
 }
diff --git a/SHAL/Src/main.cpp b/SHAL/Src/main.cpp
index 9069cdd..db71ff0 100644
--- a/SHAL/Src/main.cpp
+++ b/SHAL/Src/main.cpp
@@ -1,36 +1,89 @@
 #include "SHAL.h"
 #include "stm32f0xx.h"
 
-
-volatile GPIO* blueLED = nullptr;
-volatile GPIO* greenLED = nullptr;
-volatile UART* uart2;
+#include <stdlib.h>
 
 void c3Interrupt(){
-    greenLED->toggle();
+    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(){
-    blueLED->toggle();
+    PIN(A4).toggle();
 }
 
 int main() {
 
-    uart2 = &getUART(UART_Pair::Tx2A2_Rx2A3);
+    SHAL_init();
 
-    uart2->begin(115200);
+    //Setup UART2 (used by nucleo devices for USB comms)
+    SHAL_UART2.init(UART_Pair::Tx2A2_Rx2A3);
+    SHAL_UART2.begin(115200);
 
-    useGPIOAsInterrupt(GPIO_Key::C3,TriggerMode::RISING_EDGE,c3Interrupt);
+    SHAL_I2C1.init(I2C_Pair::SCL1B6_SDA1B7);
+    SHAL_I2C1.setClockConfig(0x2000090E);
 
-    Timer timer2 = getTimer(Timer_Key::S_TIM2);
+    //Use pin C3 to trigger a function on external interrupt
+    PIN(C3).useAsExternalInterrupt(TriggerMode::RISING_EDGE,c3Interrupt);
 
-    blueLED = &initGPIO(GPIO_Key::A4, PinMode::OUTPUT_MODE);
-    greenLED = &initGPIO(GPIO_Key::A5, PinMode::OUTPUT_MODE);
+    SHAL_TIM2.init(8000-1,1500-1);
+    SHAL_TIM2.setCallbackFunc(tim2Handler);
+    SHAL_TIM2.start();
 
-    timer2.setPrescaler(8000 - 1);
-    timer2.setARR(1500 - 1);
-    timer2.setCallbackFunc(tim2Handler);
-    timer2.start();
+    PIN(A4).setPinMode(PinMode::OUTPUT_MODE);
+    PIN(A5).setPinMode(PinMode::OUTPUT_MODE);
+
+
+
+    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();