diff --git a/Software/Software.ino b/Software/Software.ino
index 4e40a598..7e74440e 100644
--- a/Software/Software.ino
+++ b/Software/Software.ino
@@ -107,11 +107,61 @@ uint8_t inverterAllowsContactorClosing = 1;
 
 // Initialization
 void setup() {
+  init_serial();
+
+  init_CAN();
+
+  init_LED();
+
+  init_contactors();
+
+  init_modbus();
+
+  inform_user_on_inverter();
+
+  inform_user_on_battery();
+}
+
+// Perform main program functions
+void loop() {
+  // Input
+  receive_can();  // Receive CAN messages. Runs as fast as possible
+#ifdef DUAL_CAN
+  receive_can2();
+#endif
+
+  // Process
+  if (millis() - previousMillis10ms >= interval10)  // Every 10ms
+  {
+    previousMillis10ms = millis();
+    handle_LED_state();  // Set the LED color according to state
+#ifdef CONTACTOR_CONTROL
+    handle_contactors();  // Take care of startup precharge/contactor closing
+#endif
+  }
+
+  if (millis() - previousMillisUpdateVal >= intervalUpdateValues)  // Every 4.8s
+  {
+    previousMillisUpdateVal = millis();
+    update_values();  // Update values heading towards inverter. Prepare for sending on CAN, or write directly to Modbus.
+  }
+
+  // Output
+  send_can();  // Send CAN messages
+#ifdef DUAL_CAN
+  send_can2();
+#endif
+}
+
+// Initialization functions
+void init_serial() {
   // Init Serial monitor
   Serial.begin(115200);
   while (!Serial) {}
   Serial.println("__ OK __");
+}
 
+void init_CAN() {
   // CAN pins
   pinMode(CAN_SE_PIN, OUTPUT);
   digitalWrite(CAN_SE_PIN, LOW);
@@ -132,10 +182,14 @@ void setup() {
   settings.mRequestedMode = ACAN2515Settings::NormalMode;       // Select loopback mode
   can.begin(settings, [] { can.isr(); });
 #endif
+}
 
+void init_LED() {
   // Init LED control
   pixels.begin();
+}
 
+void init_contactors() {
   // Init contactor pins
 #ifdef CONTACTOR_CONTROL
   pinMode(POSITIVE_CONTACTOR_PIN, OUTPUT);
@@ -153,7 +207,9 @@ void setup() {
   pinMode(PRECHARGE_PIN, OUTPUT);
   digitalWrite(PRECHARGE_PIN, LOW);
 #endif
+}
 
+void init_modbus() {
   // Set up Modbus RTU Server
   pinMode(RS485_EN_PIN, OUTPUT);
   digitalWrite(RS485_EN_PIN, HIGH);
@@ -178,7 +234,9 @@ void setup() {
   // Start ModbusRTU background task
   MBserver.begin(Serial2);
 #endif
+}
 
+void inform_user_on_inverter() {
   // Inform user what Inverter is used
 #ifdef BYD_CAN
   Serial.println("BYD CAN protocol selected");
@@ -203,7 +261,9 @@ void setup() {
   intervalUpdateValues = 800;          // This protocol also requires the values to be updated faster
   Serial.println("SOLAX CAN protocol selected");
 #endif
+}
 
+void inform_user_on_battery() {
   // Inform user what battery is used
 #ifdef BMW_I3_BATTERY
   Serial.println("BMW i3 battery selected");
@@ -228,37 +288,6 @@ void setup() {
 #endif
 }
 
-// Perform main program functions
-void loop() {
-  // Input
-  receive_can();  // Receive CAN messages. Runs as fast as possible
-#ifdef DUAL_CAN
-  receive_can2();
-#endif
-
-  // Process
-  if (millis() - previousMillis10ms >= interval10)  // Every 10ms
-  {
-    previousMillis10ms = millis();
-    handle_LED_state();  // Set the LED color according to state
-#ifdef CONTACTOR_CONTROL
-    handle_contactors();  // Take care of startup precharge/contactor closing
-#endif
-  }
-
-  if (millis() - previousMillisUpdateVal >= intervalUpdateValues)  // Every 4.8s
-  {
-    previousMillisUpdateVal = millis();
-    update_values();  // Update values heading towards inverter. Prepare for sending on CAN, or write directly to Modbus.
-  }
-
-  // Output
-  send_can();  // Send CAN messages
-#ifdef DUAL_CAN
-  send_can2();
-#endif
-}
-
 // Functions
 void receive_can() {  // This section checks if we have a complete CAN message incoming
   // Depending on which battery/inverter is selected, we forward this to their respective CAN routines