diff --git a/include/sta/config.hpp b/include/sta/config.hpp
new file mode 100644
index 0000000..3b22342
--- /dev/null
+++ b/include/sta/config.hpp
@@ -0,0 +1,11 @@
+#ifndef STA_CONFIG_HPP
+#define STA_CONFIG_HPP
+
+#include <sta/devices/raspi/mcu/common.hpp>
+
+#define STA_DEBUGGING_ENABLED
+#define STA_PRINTF_USE_STDLIB
+
+#define STA_ASSERT_FORCE
+
+#endif // STA_CONFIG_HPP
\ No newline at end of file
diff --git a/include/sta/assert.hpp b/include/sta/debug/assert.hpp
similarity index 100%
rename from include/sta/assert.hpp
rename to include/sta/debug/assert.hpp
diff --git a/include/sta/debugging.hpp b/include/sta/debug/debug.hpp
similarity index 94%
rename from include/sta/debugging.hpp
rename to include/sta/debug/debug.hpp
index e5604ed..0396ea1 100644
--- a/include/sta/debugging.hpp
+++ b/include/sta/debug/debug.hpp
@@ -4,7 +4,7 @@
 #include <sta/config.hpp>
 #ifdef STA_DEBUGGING_ENABLED
 
-#include <sta/printable.hpp>
+#include <sta/debug/printing/printable.hpp>
 
 namespace sta
 {
diff --git a/include/sta/printable.hpp b/include/sta/debug/printing/printable.hpp
similarity index 100%
rename from include/sta/printable.hpp
rename to include/sta/debug/printing/printable.hpp
diff --git a/include/sta/debug/printing/printable_printf.hpp b/include/sta/debug/printing/printable_printf.hpp
new file mode 100644
index 0000000..0d51f04
--- /dev/null
+++ b/include/sta/debug/printing/printable_printf.hpp
@@ -0,0 +1,28 @@
+#ifndef STA_CORE_PRINTABLE_PRINTF_HPP
+#define STA_CORE_PRINTABLE_PRINTF_HPP
+
+#include <sta/debug/printing/printable.hpp>
+
+namespace sta
+{
+    class PrintablePrintf : public Printable
+    {
+    public:
+        /**
+         * @brief Construct a new Printable Printf object
+         */
+        PrintablePrintf();
+
+        /**
+         * @brief Print string.
+         *
+         * @param str       String buffer
+         * @param length    String length
+         */
+        void print(const char * str, size_t length) override;
+    };
+} // namespace sta
+
+
+
+#endif // STA_CORE_PRINTABLE_PRINTF_HPP
\ No newline at end of file
diff --git a/include/sta/printable_uart.hpp b/include/sta/debug/printing/printable_uart.hpp
similarity index 94%
rename from include/sta/printable_uart.hpp
rename to include/sta/debug/printing/printable_uart.hpp
index 7e78b0e..93f92c5 100644
--- a/include/sta/printable_uart.hpp
+++ b/include/sta/debug/printing/printable_uart.hpp
@@ -6,7 +6,7 @@
 #define STA_CORE_PRINTABLE_UART_HPP
 
 #include <sta/bus/uart/uart.hpp>
-#include <sta/printable.hpp>
+#include <sta/debug/printing/printable.hpp>
 
 #include <cstddef>
 #include <cstdint>
diff --git a/include/sta/debug_printf.hpp b/include/sta/debug_printf.hpp
deleted file mode 100644
index 42f85a6..0000000
--- a/include/sta/debug_printf.hpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#ifndef STA_CORE_DEBUG_PRINTF_HPP
-#define STA_CORE_DEBUG_PRINTF_HPP
-
-#include <sta/config.hpp>
-
-// Determine if module should be enabled
-// Condition 1: STA_DEBUG_PRINTF is defined
-// Condition 2:
-//     STA_DEBUG_PRINTF_FORCE is defined
-//   or
-//     DEBUG is defined but not NDEBUG
-#ifdef STA_DEBUG_PRINTF
-#  ifdef STA_DEBUG_PRINTF_FORCE
-#    define STA_DEBUG_SERIAL_ENABLED
-#  else // !STA_DEBUG_PRINTF_FORCE
-#    if defined(DEBUG) && !defined(NDEBUG)
-#      define STA_DEBUG_PRINTF_ENABLED
-#    endif // DEBUG && !NDEBUG
-#  endif // !STA_DEBUG_PRINTF_FORCE
-#endif // STA_DEBUG_PRINTF
-
-#if defined(STA_DEBUG_PRINTF_ENABLED) || defined(DOXYGEN)
-
-#include <sta/printable.hpp>
-
-/**
- * @brief Print debug output.
- *
- * @param ... See @ref sta::PrintableUART::print
- *
- * @ingroup sta_core_debug
- */
-#  define STA_DEBUG_PRINT(...)      sta::print(__VA_ARGS__)
-/**
- * @brief Print debug output followed by new-line to UART.
- *
- * @param ... See @ref sta::PrintableUART::println
- *
- * @ingroup sta_core_debug
- */
-#  define STA_DEBUG_PRINTLN(...)    sta::println(__VA_ARGS__)
-
-
-#else // !STA_DEBUG_PRINTF_ENABLED
-#  define STA_DEBUG_PRINT(...)      ((void)0)
-#  define STA_DEBUG_PRINTLN(...)    ((void)0)
-#endif // !STA_DEBUG_PRINTF_ENABLED
-
-#endif // STA_CORE_DEBUG_PRINTF_HPP
\ No newline at end of file
diff --git a/include/sta/debug_serial.hpp b/include/sta/debug_serial.hpp
deleted file mode 100644
index ee3bd11..0000000
--- a/include/sta/debug_serial.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * @file
- * @brief Debug output via UART.
- *
- * Configuration:
- *   * STA_DEBUG_SERIAL_UART:   UART interface for output
- *   * STA_DEBUG_SERIAL_FORCE:  Ignore debug defines and always enable output
- *   * DEBUG:                   Enables output when defined
- *   * NDEBUG:                  Disables output when defined (overrides DEBUG)
- */
-#ifndef STA_CORE_DEBUG_SERIAL_HPP
-#define STA_CORE_DEBUG_SERIAL_HPP
-
-
-#include <sta/config.hpp>
-
-// Determine if module should be enabled
-// Condition 1: STA_DEBUG_SERIAL_UART is defined
-// Condition 2:
-//     STA_DEBUG_SERIAL_FORCE is defined
-//   or
-//     DEBUG is defined but not NDEBUG
-#ifdef STA_DEBUG_SERIAL_UART
-#  ifdef STA_DEBUG_SERIAL_FORCE
-#    define STA_DEBUG_SERIAL_ENABLED
-#  else // !STA_DEBUG_SERIAL_FORCE
-#    if defined(DEBUG) && !defined(NDEBUG)
-#      define STA_DEBUG_SERIAL_ENABLED
-#    endif // DEBUG && !NDEBUG
-#  endif // !STA_DEBUG_SERIAL_FORCE
-#endif // STA_DEBUG_SERIAL_UART
-
-
-#if defined(STA_DEBUG_SERIAL_ENABLED) || defined(DOXYGEN)
-
-#include <sta/printable_uart.hpp>
-
-
-/**
- * @defgroup sta_core_debug Debug Serial
- * @ingroup sta_core
- * @brief Debug serial output.
- */
-
-
-namespace sta
-{
-    /**
-     * @brief %UART print object for debug serial output.
-     *
-     * @ingroup sta_core_debug
-     */
-    extern PrintableUART DebugSerial;
-} // namespace sta
-
-
-/**
- * @brief Print debug output to UART.
- *
- * @param ... See @ref sta::PrintableUART::print
- *
- * @ingroup sta_core_debug
- */
-#  define STA_DEBUG_PRINT(...)      sta::DebugSerial.print(__VA_ARGS__)
-/**
- * @brief Print debug output followed by new-line to UART.
- *
- * @param ... See @ref sta::PrintableUART::println
- *
- * @ingroup sta_core_debug
- */
-#  define STA_DEBUG_PRINTLN(...)    sta::DebugSerial.println(__VA_ARGS__)
-#else // !STA_DEBUG_SERIAL_ENABLED
-#  define STA_DEBUG_PRINT(...)      ((void)0)
-#  define STA_DEBUG_PRINTLN(...)    ((void)0)
-#endif // !STA_DEBUG_SERIAL_ENABLED
-
-
-#endif // STA_CORE_DEBUG_SERIAL_HPP
diff --git a/include/sta/devices/raspi/bus/i2c.hpp b/include/sta/devices/raspi/bus/i2c.hpp
index 597b4bd..d2573ac 100644
--- a/include/sta/devices/raspi/bus/i2c.hpp
+++ b/include/sta/devices/raspi/bus/i2c.hpp
@@ -12,16 +12,16 @@
 
 namespace sta 
 {
-    enum class I2cNode {
+    enum class I2CNode {
         DEV_1,
         DEV_2
     };
 
-    class RaspiI2c : public I2c 
+    class RaspiI2C : public I2C 
     {
     public:
-        RaspiI2c(I2cNode node, Mutex * mutex=nullptr, bool persistent_open=false);
-        ~RaspiI2c();
+        RaspiI2C(I2CNode node, Mutex * mutex=nullptr, bool persistent_open=false);
+        ~RaspiI2C();
 
         void transfer(uint8_t value) override;
         void transfer16(uint16_t value) override;
@@ -29,11 +29,10 @@ namespace sta
         void transfer(const uint8_t * txBuffer, uint8_t * rxBuffer, size_t size) override;
         void receive(uint8_t * buffer, size_t size) override;
 
-        void fill(uint8_t value, size_t count) override;
-
-        void selectAddress(uint16_t address) override; 
         void acquire() override;
         void release() override;
+
+        void fill(uint8_t value, size_t count) override;
     private:
         char * i2cdev_;
         int i2cfd_;
@@ -41,9 +40,9 @@ namespace sta
         const bool persistent_open_;
     };
 
-    class RaspiI2cDevice : public I2cDevice
+    class RaspiI2CDevice : public I2CDevice
     {
-        RaspiI2cDevice(I2c * intf, uint16_t address_10bit, Mutex* mutex=nullptr, bool master=false, bool blocking=true);
+        RaspiI2CDevice(RaspiI2C * intf, uint16_t address_10bit, Mutex* mutex=nullptr, bool master=false, bool blocking=true);
     };
 } // namespace sta
 
diff --git a/src/bus/device.cpp b/src/bus/device.cpp
index fb2365f..7696143 100644
--- a/src/bus/device.cpp
+++ b/src/bus/device.cpp
@@ -1,5 +1,5 @@
 #include <sta/bus/device.hpp>
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 namespace sta
 {
diff --git a/src/bus/i2c/device.cpp b/src/bus/i2c/device.cpp
index 0235b7c..9d152ac 100644
--- a/src/bus/i2c/device.cpp
+++ b/src/bus/i2c/device.cpp
@@ -1,6 +1,6 @@
 #include <sta/bus/i2c/device.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 
 namespace sta
diff --git a/src/bus/interface.cpp b/src/bus/interface.cpp
index a727dee..627e863 100644
--- a/src/bus/interface.cpp
+++ b/src/bus/interface.cpp
@@ -1,6 +1,6 @@
 #include <sta/bus/interface.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 namespace sta
 {
diff --git a/src/bus/spi/device.cpp b/src/bus/spi/device.cpp
index 29c25b2..cca3fa1 100644
--- a/src/bus/spi/device.cpp
+++ b/src/bus/spi/device.cpp
@@ -1,6 +1,6 @@
 #include <sta/bus/spi/device.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 
 namespace sta
diff --git a/src/bus/spi/settings.cpp b/src/bus/spi/settings.cpp
index d0c9246..9fbf8e4 100644
--- a/src/bus/spi/settings.cpp
+++ b/src/bus/spi/settings.cpp
@@ -1,6 +1,6 @@
 #include <sta/bus/spi/settings.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <sta/lang.hpp>
 
 
diff --git a/src/can/iter.cpp b/src/can/iter.cpp
index b5dff60..5288bef 100644
--- a/src/can/iter.cpp
+++ b/src/can/iter.cpp
@@ -1,6 +1,6 @@
 #include <sta/bus/can/iter.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 
 namespace sta
diff --git a/src/assert.cpp b/src/debug/assert.cpp
similarity index 63%
rename from src/assert.cpp
rename to src/debug/assert.cpp
index 3c02c58..2f819ab 100644
--- a/src/assert.cpp
+++ b/src/debug/assert.cpp
@@ -1,22 +1,16 @@
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #ifdef STA_ASSERT_ENABLED
 
-// TODO: This will probably destroy some stuff when working with stm32!
-#ifdef STA_PRINTF_USE_STDLIB
-#   include <sta/debug_printf.hpp>
-#else
-#   include <sta/debug_serial.hpp>
-#endif
-
+#include <sta/debug/debug.hpp>
 #include <sta/lang.hpp>
 
+#include <cstdio>
 
 namespace sta
 {
     STA_WEAK
     void assert_failed(const char * expr, const char * file, uint32_t line)
     {
-        // printf("%s:%d: Assertion failed: %s", file, line, expr)
         STA_DEBUG_PRINT(file);
         STA_DEBUG_PRINT(':');
         STA_DEBUG_PRINT(line);
diff --git a/src/printable.cpp b/src/debug/printing/printable.cpp
similarity index 98%
rename from src/printable.cpp
rename to src/debug/printing/printable.cpp
index 4031032..df6dcba 100644
--- a/src/printable.cpp
+++ b/src/debug/printing/printable.cpp
@@ -1,10 +1,10 @@
-#include <sta/printable.hpp>
+#include <sta/debug/printing/printable.hpp>
 
 #include <cinttypes>
 #include <cstring>
 #include <cstdio>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <sta/lang.hpp>
 
 namespace sta 
diff --git a/src/debug/printing/printable_printf.cpp b/src/debug/printing/printable_printf.cpp
new file mode 100644
index 0000000..07357f5
--- /dev/null
+++ b/src/debug/printing/printable_printf.cpp
@@ -0,0 +1,20 @@
+#include <sta/debug/printing/printable_printf.hpp>
+#include <sta/debug/assert.hpp>
+
+#include <cstdio>
+
+namespace sta
+{
+    PrintablePrintf::PrintablePrintf()
+    {
+
+    }
+
+    void PrintablePrintf::print(const char * str, size_t length)
+    {
+        STA_ASSERT(str != nullptr);
+        STA_ASSERT(length > 0);
+
+        printf("%.*s", length, str);
+    }
+} // namespace sta
diff --git a/src/printable_uart.cpp b/src/debug/printing/printable_uart.cpp
similarity index 85%
rename from src/printable_uart.cpp
rename to src/debug/printing/printable_uart.cpp
index 5a6ae2e..76a63f8 100644
--- a/src/printable_uart.cpp
+++ b/src/debug/printing/printable_uart.cpp
@@ -1,6 +1,6 @@
-#include <sta/printable_uart.hpp>
+#include <sta/debug/printing/printable_uart.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <sta/printf.hpp>
 
 #include <cinttypes>
diff --git a/src/debug_serial.cpp b/src/debug_serial.cpp
deleted file mode 100644
index b6ff7fe..0000000
--- a/src/debug_serial.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include <sta/debug_serial.hpp>
-#ifdef STA_DEBUG_SERIAL_ENABLED
-
-
-#ifdef STA_PLATFORM_STM32
-
-#include <sta/devices/stm32/bus/uart.hpp>
-
-// #include <usart.h>
-
-// Set platform UART alias
-using PlatformUART = sta::STM32UART;
-
-#endif // STA_PLATFORM_STM32
-
-
-namespace
-{
-    // Create platform specific serial interface
-    PlatformUART platformDebugSerial(&STA_DEBUG_SERIAL_UART);
-}
-
-namespace sta
-{
-    // Create debug serial object using platform specific serial interface
-    PrintableUART DebugSerial(&platformDebugSerial);
-} // namespace sta
-
-
-#endif // STA_DEBUG_SERIAL_ENABLED
diff --git a/src/devices/raspi/bus/i2c.cpp b/src/devices/raspi/bus/i2c.cpp
index 7be071f..be270c6 100644
--- a/src/devices/raspi/bus/i2c.cpp
+++ b/src/devices/raspi/bus/i2c.cpp
@@ -2,7 +2,7 @@
 
 #ifdef STA_PLATFORM_RASPI
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 #include <cstdlib>
 #include <string.h>
@@ -16,16 +16,16 @@
 
 namespace sta 
 {
-    RaspiI2c::RaspiI2c(I2cNode node, Mutex * mutex, bool persistent_open)
-        : I2c{mutex}, persistent_open_{persistent_open}
+    RaspiI2C::RaspiI2C(I2CNode node, Mutex * mutex, bool persistent_open)
+        : I2C{mutex}, persistent_open_{persistent_open}
     {
         // Safer version of malloc + strcpy
-        i2cdev_ = strdup(node == I2cNode::DEV_1 ? "/dev/i2c-1" : "/dev/i2c-2");
+        i2cdev_ = strdup(node == I2CNode::DEV_1 ? "/dev/i2c-1" : "/dev/i2c-2");
 
         STA_ASSERT(i2cdev_ != nullptr);
     }
 
-    RaspiI2c::~RaspiI2c()
+    RaspiI2C::~RaspiI2C()
     {
         if (i2cdev_ != NULL ) {
             free(i2cdev_);
@@ -37,35 +37,35 @@ namespace sta
         }
     }
 
-    void RaspiI2c::transfer(uint8_t value)
+    void RaspiI2C::transfer(uint8_t value)
     {
         STA_ASSERT(open_);
 
         write(i2cfd_, &value, 1);
     }
 
-    void RaspiI2c::transfer16(uint16_t value)
+    void RaspiI2C::transfer16(uint16_t value)
     {
         STA_ASSERT(open_);
 
         write(i2cfd_, &value, 2);
     }
 
-    void RaspiI2c::transfer(const uint8_t * buffer, size_t size)
+    void RaspiI2C::transfer(const uint8_t * buffer, size_t size)
     {
         STA_ASSERT(open_);
 
         write(i2cfd_, buffer, size);
     }
 
-    void RaspiI2c::transfer(const uint8_t * txBuffer, uint8_t * rxBuffer, size_t size)
+    void RaspiI2C::transfer(const uint8_t * txBuffer, uint8_t * rxBuffer, size_t size)
     {
         STA_ASSERT(open_);
 
         // TODO: Is this even possible in i2c?
     }
 
-    void RaspiI2c::receive(uint8_t * buffer, size_t size)
+    void RaspiI2C::receive(uint8_t * buffer, size_t size)
     {
         STA_ASSERT(open_);
 
@@ -75,7 +75,7 @@ namespace sta
         }
     }
 
-    void RaspiI2c::fill(uint8_t value, size_t count) 
+    void RaspiI2C::fill(uint8_t value, size_t count) 
     {
         STA_ASSERT(open_);
 
@@ -88,17 +88,9 @@ namespace sta
         delete [] buffer; 
     }
 
-    void RaspiI2c::selectAddress(uint16_t address)
+    void RaspiI2C::acquire()
     {
-        if (ioctl(i2cfd_, I2C_SLAVE, address) < 0)
-        {
-            printf("Failed to send the slave address.");
-        }
-    }
-
-    void RaspiI2c::acquire()
-    {
-        I2c::acquire();
+        I2C::acquire();
 
         if (open_) {
             return;
@@ -110,17 +102,17 @@ namespace sta
         STA_ASSERT(i2cfd_ >= 0);
     }
 
-    void RaspiI2c::release()
+    void RaspiI2C::release()
     {
         if (!persistent_open_ && open_) {
             close(i2cfd_);
         }
 
-        I2c::release();
+        I2C::release();
     }
 
-    RaspiI2cDevice::RaspiI2cDevice(I2c * intf, uint16_t address_10bit, Mutex* mutex, bool master, bool blocking) 
-        : I2cDevice { intf, address_10bit }
+    RaspiI2CDevice::RaspiI2CDevice(RaspiI2C * intf, uint16_t address_10bit, Mutex* mutex, bool master, bool blocking) 
+        : I2CDevice { intf, address_10bit }
     {
         
     }
diff --git a/src/devices/raspi/bus/spi.cpp b/src/devices/raspi/bus/spi.cpp
index 74f71f1..b7eb57b 100644
--- a/src/devices/raspi/bus/spi.cpp
+++ b/src/devices/raspi/bus/spi.cpp
@@ -4,7 +4,7 @@
 
 #include <sta/devices/raspi/gpio_pin.hpp>
 #include <sta/bus/spi/settings.hpp>
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <sta/endian.hpp>
 #include <sta/lang.hpp>
 
diff --git a/src/devices/raspi/delay.cpp b/src/devices/raspi/delay.cpp
index 3bbc5f1..3eb9641 100644
--- a/src/devices/raspi/delay.cpp
+++ b/src/devices/raspi/delay.cpp
@@ -3,7 +3,7 @@
 
 #include <sta/devices/raspi/hal.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <sta/lang.hpp>
 
 
diff --git a/src/devices/raspi/gpio_pin.cpp b/src/devices/raspi/gpio_pin.cpp
index fbfb512..e845997 100644
--- a/src/devices/raspi/gpio_pin.cpp
+++ b/src/devices/raspi/gpio_pin.cpp
@@ -1,7 +1,7 @@
 #include <sta/devices/raspi/gpio_pin.hpp>
 #ifdef STA_RASPI_GPIO_ENABLED
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <sta/lang.hpp>
 
 namespace sta 
diff --git a/src/devices/stm32/bus/i2c.cpp b/src/devices/stm32/bus/i2c.cpp
index 83f4dda..9577eec 100644
--- a/src/devices/stm32/bus/i2c.cpp
+++ b/src/devices/stm32/bus/i2c.cpp
@@ -1,8 +1,10 @@
 #include <sta/devices/stm32/bus/i2c.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 #include <cstring>
 
+#ifdef STA_PLATFORM_STM32
+
 namespace sta
 {
 	STM32I2C::STM32I2C(I2C_HandleTypeDef * handle, Mutex * mutex)
@@ -126,3 +128,5 @@ namespace sta
 	}
 
 } // namespace sta
+
+#endif // STA_PLATFORM_STM32
diff --git a/src/devices/stm32/init.cpp b/src/devices/stm32/init.cpp
index 843c6d0..0f06016 100644
--- a/src/devices/stm32/init.cpp
+++ b/src/devices/stm32/init.cpp
@@ -1,6 +1,6 @@
 #include <sta/devices/stm32/init.hpp>
 
-#include <sta/assert.hpp>
+#include <sta/debug/assert.hpp>
 
 #ifdef STA_STM32_DELAY_US_TIM