diff --git a/.github/workflows/build-esp32-c6-rust.yaml b/.github/workflows/build-esp32-c6-rust.yaml
new file mode 100644
index 0000000..6666774
--- /dev/null
+++ b/.github/workflows/build-esp32-c6-rust.yaml
@@ -0,0 +1,49 @@
+name: Continuous Integration for ESP32-C6
+
+on:
+  push:
+    paths:
+      - "esp32-c6-devkit/**"
+      - "spooky-core/**"
+      - "spooky-embedded/**"
+  workflow_dispatch:
+
+env:
+  CARGO_TERM_COLOR: always
+  GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+jobs:
+  rust-checks:
+    name: Rust Checks
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        project: [ "esp32-c6-devkit" ]
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+
+      - name: Setup Rust (Xtensa)
+        uses: esp-rs/xtensa-toolchain@v1.5
+        with:
+          default: true
+          version: '1.82.0'
+          buildtargets: esp32c6
+          ldproxy: false
+
+      - name: Enable caching
+        uses: Swatinem/rust-cache@v2
+
+      - name: Run checks sequentially within each job
+        run: |
+          cd ${{ matrix.project }}
+          # Format check
+          cargo fmt --all -- --check --color always
+          
+          # Clippy linting (with release mode if desired)
+          cargo clippy --release --all-features --workspace -- -D warnings
+          
+          # Build (Release)
+          cargo build --release
diff --git a/.github/workflows/build-esp32-rust.yaml b/.github/workflows/build-esp32-rust.yaml
deleted file mode 100644
index f23b3c6..0000000
--- a/.github/workflows/build-esp32-rust.yaml
+++ /dev/null
@@ -1,161 +0,0 @@
-name: Continuous Integration
-
-on:
-  push:
-    paths:
-      - "esp32-s3-box/**"
-      - "esp32-s3-box-3/**"
-      - "m5stack-cores3/**"
-      - "spooky-core/**"
-      - "spooky-embedded/**"
-  workflow_dispatch:
-
-env:
-  CARGO_TERM_COLOR: always
-  GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-
-jobs:
-  rust-checks:
-    name: Rust Checks
-    runs-on: ubuntu-latest
-    strategy:
-      fail-fast: false
-      matrix:
-        project: [ "esp32-s3-box", "esp32-s3-box-3", "m5stack-cores3" ]
-
-    steps:
-      - name: Checkout repository
-        uses: actions/checkout@v4
-
-      - name: Setup Rust (Xtensa)
-        uses: esp-rs/xtensa-toolchain@v1.5
-        with:
-          default: true
-          buildtargets: esp32s3
-          ldproxy: false
-
-      - name: Enable caching
-        uses: Swatinem/rust-cache@v2
-
-      - name: Run checks sequentially within each job
-        run: |
-          cd ${{ matrix.project }}
-          # Format check
-          cargo fmt --all -- --check --color always
-          
-          # Clippy linting (with release mode if desired)
-          cargo clippy --release --all-features --workspace -- -D warnings
-          
-          # Build (Release)
-          cargo build --release
-
-
-
-#name: Build ESP32 Rust Binaries in Docker image espressif/idf-rust:all_... and upload to Releases
-#
-#on:
-#  workflow_dispatch:
-#    inputs:
-#      release_tag:
-#        description: "Upload to specific release"
-#        required: true
-#        default: 'v0.6.1'
-#      skip_projects:
-#        description: "Skip projects during build (e.g. esp32-c3-devkit-rust)"
-#        required: false
-#        default: ''
-#
-#jobs:
-#  get_release:
-#    name: Get release
-#    runs-on: ubuntu-latest
-#    outputs:
-#      upload_url: ${{ steps.get_upload_url.outputs.url }}
-#    steps:
-#      - uses: octokit/request-action@v2.x
-#        id: get_release
-#        with:
-#          route: GET /repos/{owner}/{repo}/releases/tags/${{ github.event.inputs.release_tag }}
-#          owner: georgik
-#          repo: esp32-spooky-maze-game
-#        env:
-#          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-#      - name: get upload url
-#        id: get_upload_url
-#        run: |
-#          url=$(echo "$response" | jq -r '.upload_url' | sed 's/{?name,label}//')
-#          echo "::set-output name=url::$url"
-#        env:
-#          response: ${{ steps.get_release.outputs.data }}
-#
-#  build:
-#    runs-on: ubuntu-20.04
-#    container:
-#      image: espressif/idf-rust:all_1.73.0.0
-#      options: --user esp --workdir /home/esp
-#    needs: get_release
-#    steps:
-#      - name: Clone repository with specific branch
-#        run: |
-#          export HOME=/home/esp
-#          cd /home/esp
-#          curl -L https://wokwi.com/ci/install.sh | sh
-#          pwd
-#          git clone --depth 1 --branch ${{ github.ref_name }} https://github.com/georgik/esp32-spooky-maze-game.git project
-#      - name: Build and upload binaries
-#        env:
-#          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-#          WOKWI_CLI_TOKEN: ${{ secrets.WOKWI_CLI_TOKEN }}
-#        run: |
-#          # Workaround GitHub issue with setting HOME in container https://github.com/actions/runner/issues/863
-#          export HOME=/home/esp
-#          cd /home/esp
-#          . /home/esp/.bashrc
-#          . /home/esp/export-esp.sh
-#          # Upload loop for each binary
-#          cd project
-#          for FIRMWARE_DIR in `ls -d esp* m5stack*`; do
-#
-#            # If FIRMWARE_DIR is a substring in SKIP_PROJECTS, skip it
-#            if echo "${{ github.event.inputs.skip_projects }}" | grep -q "${FIRMWARE_DIR}"; then
-#              echo "Skipping $FIRMWARE_DIR"
-#              continue
-#            fi
-#
-#            cd $FIRMWARE_DIR
-#            echo "Building $FIRMWARE_DIR"
-#            VERSION=$(grep '^version =' Cargo.toml | cut -d '"' -f2)
-#            CHIP=$(grep 'hal = { package =' Cargo.toml | cut -d '"' -f2 | cut -d '-' -f1)
-#
-#            cargo espflash save-image --chip ${CHIP} --release --merge --skip-padding spooky-maze-${FIRMWARE_DIR}.bin
-#
-#            # If skip-wokwi-test.toml exists, skip Wokwi test
-#            #if [ -f "skip-wokwi-test.toml" ]; then
-#            #  echo "Skipping Wokwi test for $FIRMWARE_DIR"
-#            #else
-#            #  /home/esp/bin/wokwi-cli --timeout 5000 --timeout-exit-code 0 --screenshot-part "lcd1" --screenshot-time 5000 --screenshot-file "screenshot.png" "."
-#            #fi
-#
-#            asset_path="/home/esp/project/${FIRMWARE_DIR}/spooky-maze-${FIRMWARE_DIR}.bin"
-#            asset_name="spooky-maze-${FIRMWARE_DIR}-${{ github.event.inputs.release_tag }}.bin"
-#            curl \
-#              --request POST \
-#              --header "authorization: Bearer $GITHUB_TOKEN" \
-#              --header "Content-Type: application/octet-stream" \
-#              --data-binary "@$asset_path" \
-#              --url "${{ needs.get_release.outputs.upload_url }}?name=${asset_name}"
-#
-#            # If skip-wokwi-test.toml exists, skip Wokwi test
-#            #if [ ! -f "skip-wokwi-test.toml" ]; then
-#            #    asset_path="/home/esp/project/${FIRMWARE_DIR}/screenshot.png"
-#            #    asset_name="spooky-maze-${FIRMWARE_DIR}-${{ github.event.inputs.release_tag }}.png"
-#            #    curl \
-#            #    --request POST \
-#            #    --header "authorization: Bearer $GITHUB_TOKEN" \
-#            #    --header "Content-Type: application/octet-stream" \
-#            #    --data-binary "@$asset_path" \
-#            #    --url "${{ needs.get_release.outputs.upload_url }}?name=${asset_name}"
-#            #fi
-#
-#            cd -
-#          done
diff --git a/.github/workflows/build-esp32-s3-rust.yaml b/.github/workflows/build-esp32-s3-rust.yaml
new file mode 100644
index 0000000..5e7cb79
--- /dev/null
+++ b/.github/workflows/build-esp32-s3-rust.yaml
@@ -0,0 +1,51 @@
+name: Continuous Integration for ESP32-S3
+
+on:
+  push:
+    paths:
+      - "esp32-s3-box/**"
+      - "esp32-s3-box-3/**"
+      - "m5stack-cores3/**"
+      - "spooky-core/**"
+      - "spooky-embedded/**"
+  workflow_dispatch:
+
+env:
+  CARGO_TERM_COLOR: always
+  GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+jobs:
+  rust-checks:
+    name: Rust Checks
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        project: [ "esp32-s3-box", "esp32-s3-box-3", "m5stack-cores3" ]
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+
+      - name: Setup Rust (Xtensa)
+        uses: esp-rs/xtensa-toolchain@v1.5
+        with:
+          default: true
+          version: '1.82.0'
+          buildtargets: esp32s3
+          ldproxy: false
+
+      - name: Enable caching
+        uses: Swatinem/rust-cache@v2
+
+      - name: Run checks sequentially within each job
+        run: |
+          cd ${{ matrix.project }}
+          # Format check
+          cargo fmt --all -- --check --color always
+          
+          # Clippy linting (with release mode if desired)
+          cargo clippy --release --all-features --workspace -- -D warnings
+          
+          # Build (Release)
+          cargo build --release
diff --git a/esp32-c6-devkit/.cargo/config.toml b/esp32-c6-devkit/.cargo/config.toml
index 20b4800..fc74d88 100644
--- a/esp32-c6-devkit/.cargo/config.toml
+++ b/esp32-c6-devkit/.cargo/config.toml
@@ -1,18 +1,17 @@
 [target.riscv32imac-unknown-none-elf]
 runner = "espflash flash --monitor"
+
+[env]
+ESP_LOG="INFO"
+
+[build]
 rustflags = [
-  "-C", "link-arg=-Tlinkall.x",
   # Required to obtain backtraces (e.g. when using the "esp-backtrace" crate.)
   # NOTE: May negatively impact performance of produced code
   "-C", "force-frame-pointers",
 ]
 
-[env]
-# Use clean build after changing ESP_LOGLEVEL
-ESP_LOGLEVEL="DEBUG"
-
-[build]
 target = "riscv32imac-unknown-none-elf"
 
 [unstable]
-build-std = [ "core", "alloc" ]
+build-std = ["core"]
diff --git a/esp32-c6-devkit/Cargo.toml b/esp32-c6-devkit/Cargo.toml
index 15b73d0..09c59b0 100644
--- a/esp32-c6-devkit/Cargo.toml
+++ b/esp32-c6-devkit/Cargo.toml
@@ -1,30 +1,44 @@
 [package]
 name = "spooky-esp32-c6"
-version = "0.10.0"
+version = "0.11.0"
 authors = ["Juraj Michálek <juraj.michalek@gmail.com>"]
 edition = "2021"
 license = "MIT"
 
-[target.riscv32imac-unknown-none-elf.dependencies]
-hal = { package = "esp32c6-hal", version = "0.7.0" }
-esp-backtrace = { version = "0.9.0", features = ["esp32c6", "panic-handler", "exception-handler", "print-uart"] }
-esp-println = { version = "0.7.0", features = [ "esp32c6", "log" ] }
-
 [dependencies]
-esp-alloc = "0.3.0"
-embedded-graphics = "0.8.0"
-embedded-hal = "0.2"
-display-interface = "0.4"
-display-interface-spi = "0.4"
-log = { version = "0.4.18" }
-mipidsi = "0.7.1"
-panic-halt = "0.2"
-rand = { version = "0.8.5", default-features = false }
-rand_chacha = { version = "0.3.1", default-features = false }
-petgraph = { git = "https://github.com/zendurix/petgraph.git", branch = "better_no_std", default-features = false, features = [
-    "graphmap",
+esp-backtrace = { version = "0.14.2", features = [
+    "esp32c6",
+    "exception-handler",
+    "panic-handler",
+    "println",
+]}
+
+esp-hal = { version = "0.22.0", features = [
+    "esp32c6",
 ] }
-shared-bus = { version = "0.3.0" }
-spooky-core = { path = "../spooky-core", default-features = false, features = [ "static_maze"]}
-spooky-embedded = { path = "../spooky-embedded", default-features = false, features = [ "esp32c6", "static_maze", "resolution_320x240" ] }
-spi-dma-displayinterface = { path = "../spi-dma-displayinterface", features = [ "esp32c6" ] }
+esp-println = { version = "0.12.0", features = ["esp32c6", "log"] }
+log = { version = "0.4.21" }
+critical-section = "1.2.0"
+
+esp-alloc = "0.5.0"
+embedded-graphics = "0.8.0"
+embedded-hal = "1.0.0"
+mipidsi = "0.8.0"
+spooky-core = { path = "../spooky-core", default-features = false, features = [ "static_maze"] }
+spooky-embedded = { path = "../spooky-embedded", default-features = false, features = [ "static_maze", "resolution_320x240" ] }
+esp-display-interface-spi-dma = "0.2.0"
+esp-bsp = { version = "0.3.0", features = [ "esp32c6devkitc1" ] }
+
+[profile.dev]
+# Rust debug is too slow.
+# For debug builds always builds with some optimization
+opt-level = "s"
+
+[profile.release]
+codegen-units = 1        # LLVM can perform better optimizations using a single thread
+debug = 2
+debug-assertions = false
+incremental = false
+lto = 'fat'
+opt-level = 's'
+overflow-checks = false
diff --git a/esp32-c6-devkit/build.rs b/esp32-c6-devkit/build.rs
new file mode 100644
index 0000000..5efe9c9
--- /dev/null
+++ b/esp32-c6-devkit/build.rs
@@ -0,0 +1,3 @@
+fn main() {
+    println!("cargo:rustc-link-arg-bins=-Tlinkall.x");
+}
diff --git a/esp32-c6-devkit/rust-toolchain.toml b/esp32-c6-devkit/rust-toolchain.toml
index f524680..be506bb 100644
--- a/esp32-c6-devkit/rust-toolchain.toml
+++ b/esp32-c6-devkit/rust-toolchain.toml
@@ -1,5 +1,4 @@
 [toolchain]
-channel = "nightly"
-components = ["rustfmt", "rustc-dev"]
-# targets = ["xtensa-esp32s3-none-elf"]
-# targets = ["xtensa-esp32-none-elf", "xtensa-esp32s2-none-elf","xtensa-esp32s3-none-elf"]
+channel    = "stable"
+components = ["rust-src"]
+targets = ["riscv32imac-unknown-none-elf"]
\ No newline at end of file
diff --git a/esp32-c6-devkit/src/main.rs b/esp32-c6-devkit/src/main.rs
index 0ca946c..24a1244 100644
--- a/esp32-c6-devkit/src/main.rs
+++ b/esp32-c6-devkit/src/main.rs
@@ -1,10 +1,14 @@
 #![no_std]
 #![no_main]
 
-#[global_allocator]
-static ALLOCATOR: esp_alloc::EspHeap = esp_alloc::EspHeap::empty();
+use esp_bsp::prelude::*;
+use esp_display_interface_spi_dma::display_interface_spi_dma;
+extern crate alloc;
 
-use spi_dma_displayinterface::spi_dma_displayinterface;
+#[allow(unused_imports)]
+use esp_backtrace as _;
+
+use esp_hal::rng::Rng;
 
 use embedded_graphics::{
     mono_font::{ascii::FONT_8X13, MonoTextStyle},
@@ -12,123 +16,76 @@ use embedded_graphics::{
     text::Text,
     Drawable,
 };
-
+use embedded_hal::delay::DelayNs;
 use log::info;
 
-use hal::{
-    clock::{ClockControl, CpuClock},
+use esp_hal::{
+    delay::Delay,
+    dma::Dma,
     dma::DmaPriority,
-    gdma::Gdma,
-    peripherals::Peripherals,
+    gpio::{Level, Output},
+    // i2c::master::I2c,
     prelude::*,
-    spi::{
-        master::{prelude::*, Spi},
-        SpiMode,
-    },
-    Delay,
-    Rng,
-    IO,
-    adc::{AdcConfig, Attenuation, ADC, ADC1},
+    spi::master::Spi,
 };
 
-use spooky_embedded::{
-    app::app_loop,
-    embedded_display::{LCD_H_RES, LCD_V_RES, LCD_MEMORY_SIZE},
-    controllers::{
-        composites::ladder_composite::LadderCompositeController,
-        ladder::LadderMovementController
-    },
-};
-
-use esp_backtrace as _;
+use spooky_embedded::{app::app_loop, embedded_display::LCD_MEMORY_SIZE};
 
 #[entry]
 fn main() -> ! {
-    let peripherals = Peripherals::take();
-
-    let system = peripherals.SYSTEM.split();
-
-    // With DMA we have sufficient throughput, so we can clock down the CPU to 80MHz
-    let clocks = ClockControl::configure(system.clock_control, CpuClock::Clock80MHz).freeze();
-
-    esp_println::logger::init_logger_from_env();
-
-    info!("About to initialize the SPI LED driver");
-    let io = IO::new(peripherals.GPIO, peripherals.IO_MUX);
-
-    let lcd_sclk = io.pins.gpio6;
-    let lcd_mosi = io.pins.gpio7;
-    let lcd_cs = io.pins.gpio20;
-    let lcd_miso = io.pins.gpio0; // random unused pin
-    let lcd_dc = io.pins.gpio21.into_push_pull_output();
-    let _lcd_backlight = io.pins.gpio4.into_push_pull_output();
-    let lcd_reset = io.pins.gpio3.into_push_pull_output();
-
-    let adc_pin = io.pins.gpio2.into_analog();
-
-    let dma = Gdma::new(peripherals.DMA);
-    let dma_channel = dma.channel0;
-
-    let mut descriptors = [0u32; 8 * 3];
-    let mut rx_descriptors = [0u32; 8 * 3];
-
-    let spi = Spi::new(
-        peripherals.SPI2,
-        40u32.MHz(),
-        SpiMode::Mode0,
-        &clocks
-    ).with_pins(
-        Some(lcd_sclk),
-        Some(lcd_mosi),
-        Some(lcd_miso),
-        Some(lcd_cs),
-    ).with_dma(dma_channel.configure(
-        false,
-        &mut descriptors,
-        &mut rx_descriptors,
-        DmaPriority::Priority0,
-    ));
-
-    let di = spi_dma_displayinterface::new_no_cs(LCD_MEMORY_SIZE, spi, lcd_dc);
-
-    let mut delay = Delay::new(&clocks);
-
-    let mut display = match mipidsi::Builder::ili9341_rgb565(di)
-        .with_display_size(LCD_H_RES, LCD_V_RES)
-        .with_orientation(mipidsi::Orientation::Landscape(true))
-        .with_color_order(mipidsi::ColorOrder::Rgb)
-        .init(&mut delay, Some(lcd_reset)) {
-            Ok(disp) => { disp },
-            Err(_) => { panic!() },
-    };
-
-    info!("Display initialized");
+    // Initialize peripherals
+    let peripherals = esp_hal::init(esp_hal::Config::default());
+    esp_alloc::heap_allocator!(280 * 1024);
+    // esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
+
+    let mut delay = Delay::new();
+
+    info!("Initializing SPI LCD driver for ESP32S3Box");
+
+    // Use the `lcd_i2c_init` macro to initialize I2C for accelerometer
+    // let i2c = i2c_init!(peripherals);
 
+    // Use the `lcd_spi` macro to initialize the SPI interface
+    let spi = lcd_spi!(peripherals);
+
+    info!("SPI ready");
+
+    // Use the `lcd_display_interface` macro to create the display interface
+    let di = lcd_display_interface!(peripherals, spi);
+
+    // ESP32-S3-BOX display initialization workaround: Wait for the display to power up.
+    delay.delay_ns(500_000u32);
+
+    let mut display = lcd_display!(peripherals, di).init(&mut delay).unwrap();
+
+    // Use the `lcd_backlight_init` macro to turn on the backlight
+    lcd_backlight_init!(peripherals);
+
+    info!("Initializing...");
+
+    // Render an "Initializing..." message on the display
     Text::new(
         "Initializing...",
         Point::new(80, 110),
-        MonoTextStyle::new(&FONT_8X13, RgbColor::BLACK),
+        MonoTextStyle::new(&FONT_8X13, RgbColor::WHITE),
     )
     .draw(&mut display)
     .unwrap();
 
+    // Initialize the random number generator
     let mut rng = Rng::new(peripherals.RNG);
-    let mut seed_buffer = [1u8; 32];
-    rng.read(&mut seed_buffer).unwrap();
+    let mut seed_buffer = [0u8; 32];
+    rng.read(&mut seed_buffer);
 
-    info!("Initializing the ADC");
-    let mut adc1_config = AdcConfig::new();
-    let adc_pin = adc1_config.enable_pin(adc_pin, Attenuation::Attenuation11dB);
-
-    let analog = peripherals.APB_SARADC.split();
-    let adc1 = ADC::<ADC1>::adc(analog.adc1, adc1_config).unwrap();
+    // Initialize the movement controllers
+    // let accel_movement_controller = AccelMovementController::new(icm, 0.2);
+    let demo_movement_controller =
+        spooky_core::demo_movement_controller::DemoMovementController::new(seed_buffer);
+    // let movement_controller =
+    //     AccelCompositeController::new(demo_movement_controller, accel_movement_controller);
 
     info!("Entering main loop");
 
-    let ladder_movement_controller = LadderMovementController::new(adc1, adc_pin);
-    let demo_movement_controller = spooky_core::demo_movement_controller::DemoMovementController::new(seed_buffer);
-    let movement_controller = LadderCompositeController::new(demo_movement_controller, ladder_movement_controller);
-
-    app_loop(&mut display, seed_buffer, movement_controller);
-    loop {}
+    // Enter the application loop
+    app_loop(&mut display, seed_buffer, demo_movement_controller);
 }
diff --git a/esp32-s3-box-3/Cargo.toml b/esp32-s3-box-3/Cargo.toml
index b6a6aa3..afa5d06 100644
--- a/esp32-s3-box-3/Cargo.toml
+++ b/esp32-s3-box-3/Cargo.toml
@@ -22,6 +22,7 @@ mipidsi = "0.8.0"
 spooky-core = { path = "../spooky-core" }
 spooky-embedded = { path = "../spooky-embedded", default-features = false, features = [ "dynamic_maze", "resolution_320x240" ] }
 esp-display-interface-spi-dma = "0.2.0"
+esp-bsp = { version = "0.3.0", features = ["esp32s3box3"] }
 
 [profile.dev]
 # Rust debug is too slow.
diff --git a/esp32-s3-box-3/src/main.rs b/esp32-s3-box-3/src/main.rs
index c5d0f36..a3b659e 100644
--- a/esp32-s3-box-3/src/main.rs
+++ b/esp32-s3-box-3/src/main.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 
+use esp_bsp::prelude::*;
 use esp_display_interface_spi_dma::display_interface_spi_dma;
 
 #[allow(unused_imports)]
@@ -36,69 +37,39 @@ use spooky_embedded::{
 };
 
 use icm42670::{Address, Icm42670};
-use mipidsi::models::ILI9342CRgb565;
 
 #[entry]
 fn main() -> ! {
+    // Initialize peripherals
     let peripherals = esp_hal::init(esp_hal::Config::default());
     esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
 
     let mut delay = Delay::new();
 
-    println!("About to initialize the SPI LED driver");
-
-    let lcd_sclk = peripherals.GPIO7;
-    let lcd_mosi = peripherals.GPIO6;
-    let lcd_cs = peripherals.GPIO5;
-    let lcd_dc = Output::new(peripherals.GPIO4, Level::Low);
-    let mut lcd_backlight = Output::new(peripherals.GPIO47, Level::Low);
-    let lcd_reset = Output::new(peripherals.GPIO48, Level::Low);
-
-    let i2c_sda = peripherals.GPIO8;
-    let i2c_scl = peripherals.GPIO18;
-    let i2c = I2c::new(peripherals.I2C0, esp_hal::i2c::master::Config::default())
-        .with_sda(i2c_sda)
-        .with_scl(i2c_scl);
-
-    let dma = Dma::new(peripherals.DMA);
-    let dma_channel = dma.channel0;
-
-    let spi = Spi::new_with_config(
-        peripherals.SPI2,
-        esp_hal::spi::master::Config {
-            frequency: 40u32.MHz(),
-            ..esp_hal::spi::master::Config::default()
-        },
-    )
-    .with_sck(lcd_sclk)
-    .with_mosi(lcd_mosi)
-    .with_cs(lcd_cs)
-    .with_dma(dma_channel.configure(false, DmaPriority::Priority0));
+    println!("Initializing SPI LCD driver for ESP32S3Box");
+
+    // Use the `lcd_i2c_init` macro to initialize I2C for accelerometer
+    let i2c = i2c_init!(peripherals);
+
+    // Use the `lcd_spi` macro to initialize the SPI interface
+    let spi = lcd_spi!(peripherals);
 
     println!("SPI ready");
 
-    let di = display_interface_spi_dma::new_no_cs(LCD_MEMORY_SIZE, spi, lcd_dc);
+    // Use the `lcd_display_interface` macro to create the display interface
+    let di = lcd_display_interface!(peripherals, spi);
 
     // ESP32-S3-BOX display initialization workaround: Wait for the display to power up.
-    // If delay is 250ms, picture will be fuzzy.
-    // If there is no delay, display is blank
-    delay.delay_ms(500u32);
-
-    let mut display = mipidsi::Builder::new(ILI9342CRgb565, di)
-        .display_size(320, 240)
-        .orientation(
-            mipidsi::options::Orientation::new()
-                .flip_vertical()
-                .flip_horizontal(),
-        )
-        .color_order(mipidsi::options::ColorOrder::Bgr)
-        .reset_pin(lcd_reset)
-        .init(&mut delay)
-        .unwrap();
-
-    lcd_backlight.set_high();
+    delay.delay_ns(500_000u32);
+
+    let mut display = lcd_display!(peripherals, di).init(&mut delay).unwrap();
+
+    // Use the `lcd_backlight_init` macro to turn on the backlight
+    lcd_backlight_init!(peripherals);
 
     println!("Initializing...");
+
+    // Render an "Initializing..." message on the display
     Text::new(
         "Initializing...",
         Point::new(80, 110),
@@ -106,14 +77,16 @@ fn main() -> ! {
     )
     .draw(&mut display)
     .unwrap();
-    delay.delay_ms(1500u32);
 
+    // Initialize the accelerometer
     let icm = Icm42670::new(i2c, Address::Primary).unwrap();
 
+    // Initialize the random number generator
     let mut rng = Rng::new(peripherals.RNG);
     let mut seed_buffer = [0u8; 32];
     rng.read(&mut seed_buffer);
 
+    // Initialize the movement controllers
     let accel_movement_controller = AccelMovementController::new(icm, 0.2);
     let demo_movement_controller =
         spooky_core::demo_movement_controller::DemoMovementController::new(seed_buffer);
@@ -121,5 +94,7 @@ fn main() -> ! {
         AccelCompositeController::new(demo_movement_controller, accel_movement_controller);
 
     println!("Entering main loop");
+
+    // Enter the application loop
     app_loop(&mut display, seed_buffer, movement_controller);
 }
diff --git a/esp32-s3-box/Cargo.toml b/esp32-s3-box/Cargo.toml
index ec2146a..4596b72 100644
--- a/esp32-s3-box/Cargo.toml
+++ b/esp32-s3-box/Cargo.toml
@@ -22,6 +22,7 @@ mipidsi = "0.8.0"
 spooky-core = { path = "../spooky-core" }
 spooky-embedded = { path = "../spooky-embedded", default-features = false, features = [ "dynamic_maze", "resolution_320x240" ] }
 esp-display-interface-spi-dma = "0.2.0"
+esp-bsp = { version = "0.3.0", features = ["esp32s3box"] }
 
 [profile.dev]
 # Rust debug is too slow.
diff --git a/esp32-s3-box/src/main.rs b/esp32-s3-box/src/main.rs
index 4c682a5..a3b659e 100644
--- a/esp32-s3-box/src/main.rs
+++ b/esp32-s3-box/src/main.rs
@@ -1,6 +1,7 @@
 #![no_std]
 #![no_main]
 
+use esp_bsp::prelude::*;
 use esp_display_interface_spi_dma::display_interface_spi_dma;
 
 #[allow(unused_imports)]
@@ -39,65 +40,36 @@ use icm42670::{Address, Icm42670};
 
 #[entry]
 fn main() -> ! {
+    // Initialize peripherals
     let peripherals = esp_hal::init(esp_hal::Config::default());
     esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
 
     let mut delay = Delay::new();
 
-    println!("About to initialize the SPI LED driver");
-
-    let lcd_sclk = peripherals.GPIO7;
-    let lcd_mosi = peripherals.GPIO6;
-    let lcd_cs = peripherals.GPIO5;
-    let lcd_dc = Output::new(peripherals.GPIO4, Level::Low);
-    let mut lcd_backlight = Output::new(peripherals.GPIO45, Level::Low);
-    let lcd_reset = Output::new(peripherals.GPIO48, Level::Low);
-
-    let i2c_sda = peripherals.GPIO8;
-    let i2c_scl = peripherals.GPIO18;
-    let i2c = I2c::new(peripherals.I2C0, esp_hal::i2c::master::Config::default())
-        .with_sda(i2c_sda)
-        .with_scl(i2c_scl);
-
-    let dma = Dma::new(peripherals.DMA);
-    let dma_channel = dma.channel0;
-
-    let spi = Spi::new_with_config(
-        peripherals.SPI2,
-        esp_hal::spi::master::Config {
-            frequency: 40u32.MHz(),
-            ..esp_hal::spi::master::Config::default()
-        },
-    )
-    .with_sck(lcd_sclk)
-    .with_mosi(lcd_mosi)
-    .with_cs(lcd_cs)
-    .with_dma(dma_channel.configure(false, DmaPriority::Priority0));
+    println!("Initializing SPI LCD driver for ESP32S3Box");
+
+    // Use the `lcd_i2c_init` macro to initialize I2C for accelerometer
+    let i2c = i2c_init!(peripherals);
+
+    // Use the `lcd_spi` macro to initialize the SPI interface
+    let spi = lcd_spi!(peripherals);
 
     println!("SPI ready");
 
-    let di = display_interface_spi_dma::new_no_cs(LCD_MEMORY_SIZE, spi, lcd_dc);
+    // Use the `lcd_display_interface` macro to create the display interface
+    let di = lcd_display_interface!(peripherals, spi);
 
     // ESP32-S3-BOX display initialization workaround: Wait for the display to power up.
-    // If delay is 250ms, picture will be fuzzy.
-    // If there is no delay, display is blank
     delay.delay_ns(500_000u32);
 
-    let mut display = mipidsi::Builder::new(mipidsi::models::ILI9341Rgb565, di)
-        .display_size(240, 320)
-        .orientation(
-            mipidsi::options::Orientation::new()
-                .flip_vertical()
-                .flip_horizontal(),
-        )
-        .color_order(mipidsi::options::ColorOrder::Bgr)
-        .reset_pin(lcd_reset)
-        .init(&mut delay)
-        .unwrap();
+    let mut display = lcd_display!(peripherals, di).init(&mut delay).unwrap();
 
-    lcd_backlight.set_high();
+    // Use the `lcd_backlight_init` macro to turn on the backlight
+    lcd_backlight_init!(peripherals);
 
     println!("Initializing...");
+
+    // Render an "Initializing..." message on the display
     Text::new(
         "Initializing...",
         Point::new(80, 110),
@@ -106,12 +78,15 @@ fn main() -> ! {
     .draw(&mut display)
     .unwrap();
 
+    // Initialize the accelerometer
     let icm = Icm42670::new(i2c, Address::Primary).unwrap();
 
+    // Initialize the random number generator
     let mut rng = Rng::new(peripherals.RNG);
     let mut seed_buffer = [0u8; 32];
     rng.read(&mut seed_buffer);
 
+    // Initialize the movement controllers
     let accel_movement_controller = AccelMovementController::new(icm, 0.2);
     let demo_movement_controller =
         spooky_core::demo_movement_controller::DemoMovementController::new(seed_buffer);
@@ -119,5 +94,7 @@ fn main() -> ! {
         AccelCompositeController::new(demo_movement_controller, accel_movement_controller);
 
     println!("Entering main loop");
+
+    // Enter the application loop
     app_loop(&mut display, seed_buffer, movement_controller);
 }
diff --git a/m5stack-cores3/Cargo.toml b/m5stack-cores3/Cargo.toml
index 0bee09d..ca919ae 100644
--- a/m5stack-cores3/Cargo.toml
+++ b/m5stack-cores3/Cargo.toml
@@ -28,6 +28,7 @@ spooky-embedded = { path = "../spooky-embedded", default-features = false, featu
 esp-display-interface-spi-dma = "0.2.0"
 shared-bus = { version = "0.3.0" }
 log = "0.4.22"
+esp-bsp = { version = "0.3.0", features = ["m5stackcores3"] }
 
 [profile.dev]
 # Rust debug is too slow.
diff --git a/m5stack-cores3/src/main.rs b/m5stack-cores3/src/main.rs
index c932caf..3c3c7f3 100644
--- a/m5stack-cores3/src/main.rs
+++ b/m5stack-cores3/src/main.rs
@@ -5,6 +5,8 @@ use aw9523::I2CGpioExpanderInterface;
 use axp2101::{Axp2101, I2CPowerManagementInterface};
 use esp_display_interface_spi_dma::display_interface_spi_dma;
 
+use esp_bsp::prelude::*;
+
 #[allow(unused_imports)]
 use esp_backtrace as _;
 
@@ -30,15 +32,8 @@ use esp_hal::{
 };
 
 use log::info;
-use mipidsi::options::ColorInversion;
 use shared_bus::BusManagerSimple;
-use spooky_embedded::{
-    app::app_loop,
-    /*controllers::{
-        accel::AccelMovementController, composites::accel_composite::AccelCompositeController,
-    },*/
-    embedded_display::LCD_MEMORY_SIZE,
-};
+use spooky_embedded::{app::app_loop, embedded_display::LCD_MEMORY_SIZE};
 
 #[entry]
 fn main() -> ! {
@@ -47,69 +42,42 @@ fn main() -> ! {
 
     let mut delay = Delay::new();
 
-    info!("About to initialize the SPI LED driver");
+    println!("Initializing M5Stack CoreS3");
 
-    let lcd_sclk = peripherals.GPIO36;
-    let lcd_mosi = peripherals.GPIO37;
-    let lcd_cs = peripherals.GPIO3;
-    let lcd_dc = Output::new(peripherals.GPIO35, Level::Low);
-    let lcd_reset = Output::new(peripherals.GPIO15, Level::Low);
-
-    let i2c_sda = peripherals.GPIO12;
-    let i2c_scl = peripherals.GPIO11;
-    let i2c_bus = I2c::new(peripherals.I2C0, esp_hal::i2c::master::Config::default())
-        .with_sda(i2c_sda)
-        .with_scl(i2c_scl);
-    let bus = BusManagerSimple::new(i2c_bus);
+    // Initialize I2C shared bus
+    let bus = BusManagerSimple::new(i2c_init!(peripherals));
 
+    // Initialize AXP2101 power management
     info!("Initializing AXP2101");
     let axp_interface = I2CPowerManagementInterface::new(bus.acquire_i2c());
     let mut axp = Axp2101::new(axp_interface);
     axp.init().unwrap();
 
-    info!("Initializing GPIO Expander");
+    // Initialize AW9523 GPIO expander
+    info!("Initializing AW9523");
     let aw_interface = I2CGpioExpanderInterface::new(bus.acquire_i2c());
     let mut aw = aw9523::Aw9523::new(aw_interface);
     aw.init().unwrap();
 
-    let dma = Dma::new(peripherals.DMA);
-    let dma_channel = dma.channel0;
+    println!("Initializing SPI LCD driver for ESP32S3Box");
 
-    let spi = Spi::new_with_config(
-        peripherals.SPI2,
-        esp_hal::spi::master::Config {
-            frequency: 40u32.MHz(),
-            ..esp_hal::spi::master::Config::default()
-        },
-    )
-    .with_sck(lcd_sclk)
-    .with_mosi(lcd_mosi)
-    .with_cs(lcd_cs)
-    .with_dma(dma_channel.configure(false, DmaPriority::Priority0));
+    // Use the `lcd_i2c_init` macro to initialize I2C for accelerometer
+    // let i2c = i2c_init!(peripherals);
 
-    info!("SPI ready");
+    // Use the `lcd_spi` macro to initialize the SPI interface
+    let spi = lcd_spi!(peripherals);
 
-    let di = display_interface_spi_dma::new_no_cs(LCD_MEMORY_SIZE, spi, lcd_dc);
+    println!("SPI ready");
+
+    // Use the `lcd_display_interface` macro to create the display interface
+    let di = lcd_display_interface!(peripherals, spi);
 
     // ESP32-S3-BOX display initialization workaround: Wait for the display to power up.
-    // If delay is 250ms, picture will be fuzzy.
-    // If there is no delay, display is blank
     delay.delay_ns(500_000u32);
 
-    let mut display = mipidsi::Builder::new(mipidsi::models::ILI9341Rgb565, di)
-        .display_size(240, 320)
-        .orientation(
-            mipidsi::options::Orientation::new()
-                .flip_vertical()
-                .flip_horizontal(),
-        )
-        .color_order(mipidsi::options::ColorOrder::Bgr)
-        .invert_colors(ColorInversion::Inverted)
-        .reset_pin(lcd_reset)
-        .init(&mut delay)
-        .unwrap();
-
-    info!("Initializing...");
+    let mut display = lcd_display!(peripherals, di).init(&mut delay).unwrap();
+
+    println!("Initializing display...");
     Text::new(
         "Initializing...",
         Point::new(80, 110),
@@ -118,19 +86,15 @@ fn main() -> ! {
     .draw(&mut display)
     .unwrap();
 
-    // let icm = Icm42670::new(i2c, Address::Primary).unwrap();
-
+    // Initialize RNG
     let mut rng = Rng::new(peripherals.RNG);
     let mut seed_buffer = [0u8; 32];
     rng.read(&mut seed_buffer);
 
-    // let accel_movement_controller = AccelMovementController::new(icm, 0.2);
+    // Create the movement controller
     let demo_movement_controller =
         spooky_core::demo_movement_controller::DemoMovementController::new(seed_buffer);
-    let movement_controller = demo_movement_controller;
-    // let movement_controller =
-    //     AccelCompositeController::new(demo_movement_controller, accel_movement_controller);
 
     println!("Entering main loop");
-    app_loop(&mut display, seed_buffer, movement_controller);
+    app_loop(&mut display, seed_buffer, demo_movement_controller);
 }