diff --git a/Platformio/src/HardwareRevX.cpp b/Platformio/src/HardwareRevX.cpp
index 79cb13f7..0886d956 100644
--- a/Platformio/src/HardwareRevX.cpp
+++ b/Platformio/src/HardwareRevX.cpp
@@ -65,18 +65,18 @@ void HardwareRevX::init() {
 
   // Make sure ESP32 is running at full speed
   setCpuFrequencyMhz(240);
+  Serial.begin(115200);
 
   wakeup_reason = getWakeReason();
+  setupTouchScreen();
+
+  slowDisplayWakeup();
+
   initIO();
   restorePreferences();
   setupBacklight();
-
-  // Setup TFT
-  tft.init();
-  tft.initDMA();
-  tft.setRotation(0);
-  tft.fillScreen(TFT_BLACK);
-  tft.setSwapBytes(true);
+  setupTFT();
+  setupIMU();
 
   initLVGL();
 }
@@ -319,4 +319,46 @@ void HardwareRevX::restorePreferences() {
     backlight_brightness = preferences.getUChar("blBrightness");
     currentDevice = preferences.getUChar("currentDevice");
   }
+}
+
+void HardwareRevX::setupTFT() {
+  // Setup TFT
+  tft.init();
+  tft.initDMA();
+  tft.setRotation(0);
+  tft.fillScreen(TFT_BLACK);
+  tft.setSwapBytes(true);
+}
+
+void HardwareRevX::setupTouchScreen() {
+  // Configure i2c pins and set frequency to 400kHz
+  Wire.begin(SDA, SCL, 400000);
+  touch.begin(128); // Initialize touchscreen and set sensitivity threshold
+}
+
+void HardwareRevX::setupIMU() {
+  // Setup hal
+  IMU.settings.accelSampleRate =
+      50; // Hz.  Can be: 0,1,10,25,50,100,200,400,1600,5000 Hz
+  IMU.settings.accelRange = 2; // Max G force readable.  Can be: 2, 4, 8, 16
+  IMU.settings.adcEnabled = 0;
+  IMU.settings.tempEnabled = 0;
+  IMU.settings.xAccelEnabled = 1;
+  IMU.settings.yAccelEnabled = 1;
+  IMU.settings.zAccelEnabled = 1;
+  IMU.begin();
+  uint8_t intDataRead;
+  IMU.readRegister(&intDataRead, LIS3DH_INT1_SRC); // clear interrupt
+}
+
+void HardwareRevX::slowDisplayWakeup() {
+  // Slowly charge the VSW voltage to prevent a brownout
+  // Workaround for hardware rev 1!
+  for (int i = 0; i < 100; i++) {
+    digitalWrite(LCD_EN, HIGH); // LCD Logic off
+    delayMicroseconds(1);
+    digitalWrite(LCD_EN, LOW); // LCD Logic on
+  }
+
+  delay(100); // Wait for the LCD driver to power on
 }
\ No newline at end of file
diff --git a/Platformio/src/HardwareRevX.hpp b/Platformio/src/HardwareRevX.hpp
index ad26c4c2..b230bdf2 100644
--- a/Platformio/src/HardwareRevX.hpp
+++ b/Platformio/src/HardwareRevX.hpp
@@ -3,6 +3,7 @@
 
 #include "HardwareAbstractionInterface.h"
 #include "WiFi.h"
+#include "Wire.h"
 #include "lvgl.h"
 #include <Adafruit_FT6206.h>
 #include <Preferences.h>
@@ -56,6 +57,12 @@ class HardwareRevX : public HardwareAbstractionInterface {
 
   void setupTFT();
 
+  void setupTouchScreen();
+
+  void setupIMU();
+
+  void slowDisplayWakeup();
+
 public:
   static void displayFlushImpl(lv_disp_drv_t *disp, const lv_area_t *area,
                                lv_color_t *color_p) {
diff --git a/Platformio/src/main.cpp b/Platformio/src/main.cpp
index 426ee9bc..ece3567d 100644
--- a/Platformio/src/main.cpp
+++ b/Platformio/src/main.cpp
@@ -1,7 +1,6 @@
 // OMOTE firmware for ESP32
 // 2023 Maximilian Kern
 
-#include "Wire.h"
 #include <IRrecv.h>
 #include <IRremoteESP8266.h>
 #include <IRsend.h>
@@ -100,31 +99,12 @@ void WiFiEvent(WiFiEvent_t event) {
 
 void setup() {
 
-  // --- Startup ---
-
-  Serial.begin(115200);
-
-  // Slowly charge the VSW voltage to prevent a brownout
-  // Workaround for hardware rev 1!
-  for (int i = 0; i < 100; i++) {
-    digitalWrite(LCD_EN, HIGH); // LCD Logic off
-    delayMicroseconds(1);
-    digitalWrite(LCD_EN, LOW); // LCD Logic on
-  }
-
-  delay(100); // Wait for the LCD driver to power on
-
-  // Setup touchscreen
-  Wire.begin(SDA, SCL,
-             400000); // Configure i2c pins and set frequency to 400kHz
-
   hal = HardwareRevX::getInstance();
   hal->init();
 
   auto ui = OmoteUI::getInstance(hal);
   ui->layout_UI();
 
-  hal->touch.begin(128); // Initialize touchscreen and set sensitivity threshold
 #ifdef ENABLE_WIFI
   // Setup WiFi
   WiFi.setHostname("OMOTE"); // define hostname
@@ -133,20 +113,6 @@ void setup() {
   WiFi.setSleep(true);
 #endif
 
-  // Setup hal->IMU
-  hal->IMU.settings.accelSampleRate =
-      50; // Hz.  Can be: 0,1,10,25,50,100,200,400,1600,5000 Hz
-  hal->IMU.settings.accelRange =
-      2; // Max G force readable.  Can be: 2, 4, 8, 16
-  hal->IMU.settings.adcEnabled = 0;
-  hal->IMU.settings.tempEnabled = 0;
-  hal->IMU.settings.xAccelEnabled = 1;
-  hal->IMU.settings.yAccelEnabled = 1;
-  hal->IMU.settings.zAccelEnabled = 1;
-  hal->IMU.begin();
-  uint8_t intDataRead;
-  hal->IMU.readRegister(&intDataRead, LIS3DH_INT1_SRC); // clear interrupt
-
   // Setup IR
   IrSender.begin();
   digitalWrite(IR_VCC, HIGH); // Turn on IR receiver