diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index d6c5426..947eaff 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -15,6 +15,13 @@ stages:
 before_script:
   - "pip install -U platformio"
 
-job:
+build:
   stage: build
-  script: "pio run -e nano -e leonardo -e pico -e bluepill -e esp32"
\ No newline at end of file
+  script:
+    - "pio run -e nano -e leonardo -e pico -e bluepill -e esp32"
+    - mkdir -p builds
+    - cp .pio/build/pico/firmware.uf2 builds/
+    - cp .pio/build/pico/firmware.elf builds/
+  artifacts:
+    paths:
+      - builds/*
\ No newline at end of file
diff --git a/.vscode/extensions.json b/.vscode/extensions.json
index 0f0d740..080e70d 100644
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@@ -1,7 +1,10 @@
-{
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
-    "recommendations": [
-        "platformio.platformio-ide"
-    ]
-}
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}
diff --git a/lib/ComPort/ComPort.cpp b/lib/ComPort/ComPort.cpp
new file mode 100644
index 0000000..94443bb
--- /dev/null
+++ b/lib/ComPort/ComPort.cpp
@@ -0,0 +1,4 @@
+#include <ComPort.h>
+
+// Actually define the variable and default to the USB Serial port
+HardwareSerial* comPort = (HardwareSerial*) &Serial;
\ No newline at end of file
diff --git a/lib/ComPort/ComPort.h b/lib/ComPort/ComPort.h
new file mode 100644
index 0000000..a5af2ac
--- /dev/null
+++ b/lib/ComPort/ComPort.h
@@ -0,0 +1,9 @@
+#ifndef COMPORT_H
+#define COMPORT_H
+
+#include <Arduino.h>
+
+// comPort is a Serial-like object that we can switch.
+extern HardwareSerial* comPort;
+
+#endif
\ No newline at end of file
diff --git a/src/boards.h b/src/boards.h
index ef93337..5f02603 100644
--- a/src/boards.h
+++ b/src/boards.h
@@ -28,9 +28,11 @@
     #define ENDSTOPS_PULLUPS true
     #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
     #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
-    #define SERIAL_PORT Serial
+    #define PRIMARY_SERIAL_PORT Serial
     #define PWM_NUM 0
     #define WIRING_PWM_LEDS {}
+    #define SUPPORT_EEPROM
+    #define STAGE_MAX_MOTORS 3
 #elif defined(BOARD_SANGABOARDv2)
     #define BOARD_STRING "Sangaboard v0.2"
     #define WIRING_MOTOR_X 13, 12, 11, 10
@@ -40,9 +42,11 @@
     #define ENDSTOPS_PULLUPS true
     #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
     #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
-    #define SERIAL_PORT Serial
+    #define PRIMARY_SERIAL_PORT Serial
     #define PWM_NUM 0
     #define WIRING_PWM_LEDS {}
+    #define SUPPORT_EEPROM
+    #define STAGE_MAX_MOTORS 3
 #elif defined(BOARD_CUSTOM_BLUEPILL)
     #define BOARD_STRING "Custom Bluepill board"
     #define WIRING_MOTOR_X PB11, PB10, PB1, PB0
@@ -52,9 +56,10 @@
     #define ENDSTOPS_PULLUPS true
     #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
     #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
-    #define SERIAL_PORT Serial
+    #define PRIMARY_SERIAL_PORT Serial
     #define PWM_NUM 0
     #define WIRING_PWM_LEDS {}
+    #define STAGE_MAX_MOTORS 3
 #elif defined(BOARD_CUSTOM_PICO)
     #define BOARD_STRING "Custom Pico board"
     #define WIRING_MOTOR_X 2, 3, 4, 5
@@ -65,11 +70,14 @@
     #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
     #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
     #define ADDITIONAL_STEPPERS 1
-    #define WIRING_ADDITIONAL_STEPPERS = {{0, 1, 22, 23}}
+    #define WIRING_ADDITIONAL_STEPPER 0, 1, 22, 23
     #define WIRING_STEPSTICK {13,14,15} //enable, step, dir todo: move to boards.h
-    #define SERIAL_PORT Serial
+    #define PRIMARY_SERIAL_PORT Serial
+    #define SECONDARY_SERIAL_PORT Serial2
     #define PWM_NUM 0
     #define WIRING_PWM_LEDS {}
+    #define SUPPORT_EEPROM
+    #define STAGE_MAX_MOTORS 4
 #elif defined(BOARD_SANGABOARDv5)
     #define BOARD_STRING "Sangaboard v0.5"
     #define WIRING_MOTOR_X 2, 3, 4, 5
@@ -80,19 +88,18 @@
     #define WIRING_ENDSTOPS_MIN {26,27,28}
     //#define WIRING_ENDSTOPS_MAX {22,23,0} //not normally used
     #define ADDITIONAL_STEPPERS 1
-    #define WIRING_ADDITIONAL_STEPPERS = {{0, 1, 22, 23}}
+    #define WIRING_ADDITIONAL_STEPPER 0, 1, 22, 23
     #define WIRING_STEPSTICK {23,22,1} //enable, step, dir todo: move to boards.h
     #define WIRING_SERIAL_TX 8
     #define WIRING_SERIAL_RX 9
-    #ifdef USB_SERIAL
-        #define SERIAL_PORT Serial
-    #else
-        #define SERIAL_PORT Serial2
-    #endif
+    #define PRIMARY_SERIAL_PORT Serial
+    #define SECONDARY_SERIAL_PORT Serial2
     #define PWM_NUM 2
     #define WIRING_PWM_LEDS {25, 29}
     #define WIRING_CC_LED 24
     //TODO: 4th motor support
+    #define SUPPORT_EEPROM
+    #define STAGE_MAX_MOTORS 4
 #elif defined(BOARD_CUSTOM_ESP32)
     #define BOARD_STRING "Custom ESP32 board"
     #define WIRING_MOTOR_X 34, 32, 25, 27
@@ -103,11 +110,12 @@
     #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
     #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
     #define WIRING_STEPSTICK {13,12,27}
-    #define SERIAL_PORT Serial
+    #define PRIMARY_SERIAL_PORT Serial
     #define PWM_NUM 0
     #define WIRING_PWM_LEDS {}
+    #define STAGE_MAX_MOTORS 3
 #elif defined(BOARD_CUSTOM)
-    #define BOARD_STRING "Custom board"
+    #define BOARD_STRING "Custom UNO board"
     #define WIRING_MOTOR_X 2, 3, 4, 5
     #define WIRING_MOTOR_Y 2, 3, 4, 5
     #define WIRING_MOTOR_Z 2, 3, 4, 5
@@ -116,9 +124,10 @@
     #define WIRING_ENDSTOPS_MIN {1,2,0}
     //#define WIRING_ENDSTOPS_MAX {22,23,0} //not normally used
     #define ADDITIONAL_STEPPERS 1
-    #define WIRING_ADDITIONAL_STEPPERS = {{0, 1, 22, 23}}
     #define WIRING_STEPSTICK {8,9,10} //enable, step, dir todo: move to boards.h
-    #define SERIAL_PORT Serial
+    #define PRIMARY_SERIAL_PORT Serial
     #define PWM_NUM 0
     #define WIRING_PWM_LEDS {}
+    #define SUPPORT_EEPROM
+    #define STAGE_MAX_MOTORS 3
 #endif
\ No newline at end of file
diff --git a/src/config.h b/src/config.h
index 495ecda..69be789 100644
--- a/src/config.h
+++ b/src/config.h
@@ -3,10 +3,6 @@
     #define BOARD_AUTO
     //#define BOARD_SANGABOARDv5
 
-    //without this, default to HW serial where available
-    //the details are board specific and configured in boards.h
-    #define USB_SERIAL
-
     #include <boards.h> //defines wiring
 
     //general settings
@@ -15,6 +11,7 @@
     #define MAX_ARGUMENT_LENGTH 20 //used in argument parsing, takes up ram
     #define VERSION_STRING "Sangaboard Firmware v1.0.0-dev"
     #define DEBUG_ON
+    #define EMULATED_EEPROM_SIZE 512
 
     //module choice
     #define HELP
@@ -24,11 +21,6 @@
  //   #define STEPSTICK
     #define ILLUMINATION
 
-    //module configs
-    #ifdef STAGE
-        #define STAGE_N_MOTORS 3
-    #endif
-
     #ifdef ENDSTOPS
         #define ENDSTOPS_MIN
         //#define ENDSTOPS_MAX
@@ -42,9 +34,5 @@
     #define LIGHT_SENSOR_ADAFRUIT_TSL2591
     //#define LIGHT_SENSOR_ADAFRUIT_ADS1115
 
-    //config for non-standard platforms
-    #ifndef MCU_PICO
-        #define SUPPORT_EEPROM
-    #endif
     #define CONFIGURED
 #endif
\ No newline at end of file
diff --git a/src/dummyEEPROM.h b/src/dummyEEPROM.h
index 442a610..1802f23 100644
--- a/src/dummyEEPROM.h
+++ b/src/dummyEEPROM.h
@@ -1,4 +1,5 @@
-
+#ifndef dummyEEPROM_h
+#define dummyEEPROM_h
 #include <stdint.h>
 
 struct EEPROMClass
@@ -21,4 +22,5 @@ struct EEPROMClass
     }
 };
 
-static EEPROMClass EEPROM;
\ No newline at end of file
+static EEPROMClass EEPROM;
+#endif
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index c8a19ab..ca0a283 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -2,16 +2,16 @@
  * Sangaboard firmware
  *
  * Refactored from bath_open_instrumentation_group/sangaboard/arduino_code
- * 
+ *
  * This firmware was written by
  * Richard Bowman
  * Julian Stirling
  * Boyko Vodenicharski
  * Filip Ayazi
- * 
+ *
  * Much of the code is based on older code written by
  * James Sharkey and Fergus Riche
- * 
+ *
  * Released under GPL v3, 2021
  */
 
@@ -19,6 +19,12 @@
 #include "config.h"
 #include <Arduino.h>
 #include <stdint.h>
+#include <ComPort.h>
+#ifdef SUPPORT_EEPROM
+#include <EEPROM.h>
+#else
+#include "dummyEEPROM.h"
+#endif
 
 #ifdef HELP
     #include "modules/help/help.h"
@@ -52,7 +58,10 @@ uint8_t registered_loop_fn_count = 0;
 
 const Command core_commands[] = {
     {"version", get_version},
+    {"board", get_board},
     {"list_modules", get_modules},
+    {"eeprom_last_commit", get_eeprom_last_commit},
+    {"system_status", get_system_status},
     END_COMMAND};
 
 void register_module(const Command commands[], void (*loop_fn)(void))
@@ -73,9 +82,11 @@ void handle_command(String received_command)
     }
 
 #ifdef HELP
-    SERIAL_PORT.println(F("Type 'help' for a list of commands."));
+    comPort->print(F("Command not recognised: '"));
+    comPort->print(received_command);
+    comPort->println(F("', Type 'help' for a list of commands."));
 #else
-    SERIAL_PORT.println(F("Invalid command"));
+    comPort->println(F("Invalid command"));
 #endif
 }
 
@@ -104,49 +115,108 @@ uint8_t parse_arguments(char **arguments, String command, uint8_t max_args)
 
 void get_version(String)
 {
-    SERIAL_PORT.println(F(VERSION_STRING));
+    comPort->println(F(VERSION_STRING));
+    return;
+}
+
+void get_board(String)
+{
+    comPort->println(F(BOARD_STRING));
     return;
 }
 
 void get_modules(String)
 {
 #if defined(LIGHTSENSOR) && defined(LIGHT_SENSOR_ADAFRUIT_TSL2591)
-    SERIAL_PORT.println(F("Light Sensor: TSL2591"));
+    comPort->println(F("Light Sensor: TSL2591"));
 #elif defined(LIGHTSENSOR) && defined(LIGHT_SENSOR_ADAFRUIT_ADS1115)
-    SERIAL_PORT.println(F("Light Sensor: ADS1115"));
+    comPort->println(F("Light Sensor: ADS1115"));
 #endif
 
 #if defined(ENDSTOPS)
-    SERIAL_PORT.print("Endstops:");
+    comPort->print("Endstops:");
     #ifdef ENDSTOPS_MIN
-    SERIAL_PORT.print(" min");
+    comPort->print(" min");
     #endif
     #ifdef ENDSTOPS_MAX
-    SERIAL_PORT.print(" max");
+    comPort->print(" max");
     #endif
     #ifdef ENDSTOPS_SOFT
-    SERIAL_PORT.print(" soft");
+    comPort->print(" soft");
     #endif
-    SERIAL_PORT.println();
+    comPort->println();
 #endif
-    SERIAL_PORT.println("--END--");
+#if defined(ILLUMINATION)
+    comPort->println("Illumination:default");
+#endif
+    comPort->println("--END--");
 }
 
-#ifndef UNIT_TEST
+#if defined(SUPPORT_EEPROM) && defined(MCU_PICO)
+unsigned long eeprom_last_commit;
+void eeprom_commit_at_interval(unsigned long interval)
+{
+    unsigned long next_commit = eeprom_last_commit + interval;
+    unsigned long now = millis();
+    // millis() can overflow - this if statement will trigger
+    // every `interval` accounting for overflows.
+    if ( (now > next_commit && next_commit > eeprom_last_commit) // no overflow
+         || (now > next_commit && now < eeprom_last_commit)) // overflow
+    {
+        EEPROM.commit();
+        eeprom_last_commit = millis();
+    }
+}
+#endif
+
+void get_eeprom_last_commit(String)
+{
+#if defined(SUPPORT_EEPROM) && defined(MCU_PICO)
+    comPort->println(eeprom_last_commit);
+#else
+    comPort->println("Not using pico");
+#endif
+    return;
+}
+
+void get_system_status(String)
+{
+    comPort->println(F(BOARD_STRING));
+    comPort->println(F(VERSION_STRING));
+    #ifdef MCU_PICO
+        comPort->print(F("Heap remaining:"));
+        comPort->println(rp2040.getFreeHeap());
+    #endif
+}
+
+#ifndef UNIT_TEST // don't define setup() and loop() for unit testing
 void setup()
 {
+    #if defined(SUPPORT_EEPROM) && defined(MCU_PICO)
+        // On the Pico, EEPROM is emulated.
+        // This means we need to explicitly enable it, and also
+        // call EEPROM.commit() periodically to persist it to flash.
+        EEPROM.begin(EMULATED_EEPROM_SIZE);
+        eeprom_last_commit = millis();
+    #endif
     // initialise serial port
     #ifdef WIRING_SERIAL_TX
         Serial2.setTX(WIRING_SERIAL_TX);
         Serial2.setRX(WIRING_SERIAL_RX);
     #endif
 
-    SERIAL_PORT.begin(115200);
-    while (!SERIAL_PORT)
+    comPort = (HardwareSerial*) &PRIMARY_SERIAL_PORT;
+    PRIMARY_SERIAL_PORT.begin(115200);
+    #ifdef SECONDARY_SERIAL_PORT
+        SECONDARY_SERIAL_PORT.begin(115200);
+        //wait for either port to be ready
+        while (!PRIMARY_SERIAL_PORT && !SECONDARY_SERIAL_PORT)
+    #else
+        while (!PRIMARY_SERIAL_PORT)
+    #endif
         delay(1);
 
     register_module(core_commands, NULL);
-
     #ifdef HELP
     help_setup();
     #endif
@@ -173,17 +243,34 @@ void setup()
     #endif
 
     registered_commands[registered_commands_count] = &end_command;
-    SERIAL_PORT.println(F(VERSION_STRING));
+    comPort->println(F(VERSION_STRING));
 }
 
 void loop()
 {
-    if (SERIAL_PORT.available())
+    bool bytesAvailable = false;
+    if (PRIMARY_SERIAL_PORT.available())
     {
-        handle_command(SERIAL_PORT.readStringUntil('\n'));
+        comPort = (HardwareSerial*) &PRIMARY_SERIAL_PORT;
+        bytesAvailable = true;
     }
+    #if defined(SECONDARY_SERIAL_PORT)
+    else if (SECONDARY_SERIAL_PORT.available())
+    {
+        comPort = (HardwareSerial*) &SECONDARY_SERIAL_PORT;
+        bytesAvailable = true;
+    }
+    #endif
+    if (bytesAvailable)
+    {
+        handle_command(comPort->readStringUntil('\n'));
+    }
+    #if defined(SUPPORT_EEPROM) && defined(MCU_PICO)
+    // On the Pico, emulated EEPROM is only saved to flash when we call commit()
+    eeprom_commit_at_interval(10000); // NB this only writes to flash if something has changed.
+    #endif
 
     for (int i = 0; i < registered_loop_fn_count; i++)
         registered_loop_functions[i]();
 }
-#endif
\ No newline at end of file
+#endif //ndef UNIT_TEST
\ No newline at end of file
diff --git a/src/main.h b/src/main.h
index e69ec23..b4d380b 100644
--- a/src/main.h
+++ b/src/main.h
@@ -1,8 +1,14 @@
 #ifndef MAIN_H
 #include "config.h"
 #include <Arduino.h>
+#include <ComPort.h>
+#ifdef SUPPORT_EEPROM
+#include <EEPROM.h>
+#else
+#include "dummyEEPROM.h"
+#endif
 #ifdef DEBUG_ON
-#define D(x) Serial.println(x)
+#define D(x) comPort->println(x)
 #else
 #define D(x)
 #endif
@@ -21,12 +27,26 @@ struct Command
 
 inline bool check_end_command(Command c) { return strlen(c.command) > 0; }
 inline bool check_end_command(Command * c) {return strlen(c->command) > 0; }
+inline bool read_eeprom_bool(uint16_t address, bool default_value)
+{
+    int8_t temp_value;
+    EEPROM.get(address, temp_value);
+    if (temp_value < 0)
+        return default_value;
+
+    return temp_value != 0;
+}
+
 
 const Command end_command = END_COMMAND;
 uint8_t parse_arguments(char ** arguments, String, uint8_t);
 extern void register_module(const Command commands[], void (*loop_fn)(void));
 
 void get_version(String);
+void get_board(String);
 void get_modules(String);
+void get_eeprom_last_commit(String);
+void get_system_status(String);
+
 #define MAIN_H
 #endif
\ No newline at end of file
diff --git a/src/modules/endstops/endstops.cpp b/src/modules/endstops/endstops.cpp
index 330c1c9..3083fe7 100644
--- a/src/modules/endstops/endstops.cpp
+++ b/src/modules/endstops/endstops.cpp
@@ -2,6 +2,7 @@
 #ifdef ENDSTOPS
 #include "endstops.h"
 #include "../stage/stage.h"
+#include <ComPort.h>
 #ifdef SUPPORT_EEPROM
     #include <EEPROM.h>
 #else
@@ -10,8 +11,8 @@
 
 unsigned long retreat_steps = 5000;
 unsigned long home_move_steps = 100000;
-unsigned long axis_max[STAGE_N_MOTORS];
-const uint8_t axis_max_eeprom = sizeof(long)*(STAGE_N_MOTORS+2);
+unsigned long axis_max[STAGE_MAX_MOTORS];
+const uint8_t axis_max_eeprom = 200+sizeof(long)*(STAGE_MAX_MOTORS+2);
 #ifdef ENDSTOPS_MIN
 const uint8_t endstops_min_pins[] = WIRING_ENDSTOPS_MIN;
 #endif
@@ -99,7 +100,7 @@ void endstops_loop()
         homing_final = false;
         min_step_delay = previous_step_delay;
         home_retreat();
-        SERIAL_PORT.println("done.");
+        comPort->println("done.");
         return;
     }
 }
@@ -121,8 +122,8 @@ int8_t endstops_check()
 
     if (endstop_break != 0 && stage_moving)
     {
-        SERIAL_PORT.print(F("Endstop hit:"));
-        SERIAL_PORT.println(endstop_break);
+        comPort->print(F("Endstop hit:"));
+        comPort->println(endstop_break);
         //if we have both min/max endstops, axis_max is adjusted to the correct value
         //if we only have min, we go from 0 -> predefined axis_max
         //if we only have max, we go from 0 -> predefined axis_max
@@ -154,16 +155,16 @@ void endstops_status(String command)
     EACH_MOTOR
     {
         if (i > 0)
-            SERIAL_PORT.print(" ");
+            comPort->print(" ");
         if (endstop_triggered(i, -1))
-            SERIAL_PORT.print("-1");
+            comPort->print("-1");
         else if (endstop_triggered(i, 1))
-            SERIAL_PORT.print("1");
+            comPort->print("1");
         else
-            SERIAL_PORT.print("0");
+            comPort->print("0");
     }
 
-    SERIAL_PORT.println();
+    comPort->println();
 }
 
 void print_axes_max(String command)
@@ -171,10 +172,10 @@ void print_axes_max(String command)
     EACH_MOTOR
     {
         if (i > 0)
-            SERIAL_PORT.print(" ");
-        SERIAL_PORT.print(axis_max[i]);
+            comPort->print(" ");
+        comPort->print(axis_max[i]);
     }
-    SERIAL_PORT.println();
+    comPort->println();
 }
 
 void home_start(uint8_t axes, int8_t direction)
@@ -226,7 +227,7 @@ void home_final()
 void endstops_home_min(String command)
 {
 #ifndef ENDSTOPS_MIN
-    SERIAL_PORT.println(F("Min endstops not installed"));
+    comPort->println(F("Min endstops not installed"));
     return;
 #endif
     if (command.equals("home_min"))
@@ -241,7 +242,7 @@ void endstops_home_min(String command)
 void endstops_home_max(String command)
 {
 #ifndef ENDSTOPS_MAX
-    SERIAL_PORT.println(F("Max endstops not installed"));
+    comPort->println(F("Max endstops not installed"));
     return;
 #endif
     if (command.equals("home_max"))
@@ -293,10 +294,10 @@ void endstops_setup()
 }
 
 //endstops? - get triggered endstops in (1,0,-1) format for max, none, min"));
-//      SERIAL_PORT.println(F("home_min <axes?>               - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position"));
-//      SERIAL_PORT.println(F("home_max <axes?>               - home given (00000zyx byte, e.g. 3 for x and y) or all axes to their max position"));
-//      SERIAL_PORT.println(F("max_p?                         - return positions of max endstops"));
-//      SERIAL_PORT.println(F("max <d> <d> <d>
+//      comPort->println(F("home_min <axes?>               - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position"));
+//      comPort->println(F("home_max <axes?>               - home given (00000zyx byte, e.g. 3 for x and y) or all axes to their max position"));
+//      comPort->println(F("max_p?                         - return positions of max endstops"));
+//      comPort->println(F("max <d> <d> <d>
 extern const Command endstops_commands[] = {
     {"endstops?", endstops_status},
     {"home_min", endstops_home_min},
diff --git a/src/modules/help/help.cpp b/src/modules/help/help.cpp
index c530481..dbc8027 100644
--- a/src/modules/help/help.cpp
+++ b/src/modules/help/help.cpp
@@ -1,6 +1,7 @@
 #include "config.h"
 #ifdef HELP
 #include "help.h"
+#include <ComPort.h>
 
 void help_setup()
 {
@@ -9,78 +10,78 @@ void help_setup()
 
 void help(String command)
 {
-    SERIAL_PORT.println("");
-    SERIAL_PORT.print("Board: ");
-    SERIAL_PORT.println(F(BOARD_STRING));
+    comPort->println("");
+    comPort->print("Board: ");
+    comPort->println(F(BOARD_STRING));
 
 #ifdef LIGHT_SENSOR
 #if defined ADAFRUIT_TSL2591
-    SERIAL_PORT.println(F("Compiled with Adafruit TSL2591 support"));
+    comPort->println(F("Compiled with Adafruit TSL2591 support"));
 #elif defined ADAFRUIT_ADS1115
-    SERIAL_PORT.println(F("Compiled with Adafruit ADS1115 support"));
+    comPort->println(F("Compiled with Adafruit ADS1115 support"));
 #endif
 #endif //LIGHT_SENSOR
 
 
 #ifdef ENDSTOPS
 #ifdef ENDSTOPS_MIN
-    SERIAL_PORT.println(F("Compiled with min endstops support"));
+    comPort->println(F("Compiled with min endstops support"));
 #endif
 #ifdef ENDSTOPS_MAX
-    SERIAL_PORT.println(F("Compiled with max endstops support"));
+    comPort->println(F("Compiled with max endstops support"));
 #endif
 #endif //ENDSTOPS
 
 
-    SERIAL_PORT.println("");
-    SERIAL_PORT.println(F("Commands (terminated by a newline character):"));
+    comPort->println("");
+    comPort->println(F("Commands (terminated by a newline character):"));
 #ifdef STAGE
-    SERIAL_PORT.println(F("mrx <d>                        - relative move in x"));
-    SERIAL_PORT.println(F("mry <d>                        - relative move in y"));
-    SERIAL_PORT.println(F("mrz <d>                        - relative move in z"));
-    SERIAL_PORT.println(F("mr <d> <d> <d>                 - relative move in all 3 axes"));
-    SERIAL_PORT.println(F("release                        - de-energise all motors"));
-    SERIAL_PORT.println(F("p?                             - print position (3 space-separated integers"));
-    SERIAL_PORT.println(F("ramp_time <d>                  - set the time taken to accelerate/decelerate in us"));
-    SERIAL_PORT.println(F("min_step_delay <d>             - set the minimum time between steps in us."));
-    SERIAL_PORT.println(F("dt <d>                         - set the minimum time between steps in us."));
-    SERIAL_PORT.println(F("ramp_time?                     - get the time taken to accelerate/decelerate in us"));
-    SERIAL_PORT.println(F("min_step_delay?                - get the minimum time between steps in us."));
-    SERIAL_PORT.println(F("zero                           - set the current position to zero."));
-    SERIAL_PORT.println(F("stop                           - stop a move in progress."));
-    SERIAL_PORT.println(F("blocking_moves?                - get blocking moves enabled"));
-    SERIAL_PORT.println(F("blocking_moves <bool>          - enable/disable blocking moves"));
-    SERIAL_PORT.println(F("moving?                        - check if a move is in progress"));
-    SERIAL_PORT.println(F("notify_on_stop                 - respond with stopped when current move finishes"));
+    comPort->println(F("mrx <d>                        - relative move in x"));
+    comPort->println(F("mry <d>                        - relative move in y"));
+    comPort->println(F("mrz <d>                        - relative move in z"));
+    comPort->println(F("mr <d> <d> <d>                 - relative move in all 3 axes"));
+    comPort->println(F("release                        - de-energise all motors"));
+    comPort->println(F("p?                             - print position (3 space-separated integers"));
+    comPort->println(F("ramp_time <d>                  - set the time taken to accelerate/decelerate in us"));
+    comPort->println(F("min_step_delay <d>             - set the minimum time between steps in us."));
+    comPort->println(F("dt <d>                         - set the minimum time between steps in us."));
+    comPort->println(F("ramp_time?                     - get the time taken to accelerate/decelerate in us"));
+    comPort->println(F("min_step_delay?                - get the minimum time between steps in us."));
+    comPort->println(F("zero                           - set the current position to zero."));
+    comPort->println(F("stop                           - stop a move in progress."));
+    comPort->println(F("blocking_moves?                - get blocking moves enabled"));
+    comPort->println(F("blocking_moves <bool>          - enable/disable blocking moves"));
+    comPort->println(F("moving?                        - check if a move is in progress"));
+    comPort->println(F("notify_on_stop                 - respond with stopped when current move finishes"));
 #endif //STAGE
 
 #ifdef LIGHT_SENSOR
-    SERIAL_PORT.println(F("light_sensor_gain <d>          - set the gain of the light sensor"));
-    SERIAL_PORT.println(F("light_sensor_gain?             - get the gain of the light sensor"));
-    SERIAL_PORT.println(F("light_sensor_gain_values?      - get the allowable gain values of the light sensor"));
-    SERIAL_PORT.println(F("light_sensor_integration_time? - get the integration time in milliseconds"));
-    SERIAL_PORT.println(F("light_sensor_intensity?        - read the current value from the full spectrum diode"));
+    comPort->println(F("light_sensor_gain <d>          - set the gain of the light sensor"));
+    comPort->println(F("light_sensor_gain?             - get the gain of the light sensor"));
+    comPort->println(F("light_sensor_gain_values?      - get the allowable gain values of the light sensor"));
+    comPort->println(F("light_sensor_integration_time? - get the integration time in milliseconds"));
+    comPort->println(F("light_sensor_intensity?        - read the current value from the full spectrum diode"));
 #endif //LIGHT_SENSOR
 
 #if defined(ENDSTOPS_MIN) || defined(ENDSTOPS_MAX)
-    SERIAL_PORT.println(F("endstops?                      - get triggered endstops in (1,0,-1) format for max, none, min"));
-    SERIAL_PORT.println(F("home_min <axes?>               - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position"));
-    SERIAL_PORT.println(F("home_max <axes?>               - home given (00000zyx byte, e.g. 3 for x and y) or all axes to their max position"));
-    SERIAL_PORT.println(F("max_p?                         - return positions of max endstops"));
-    SERIAL_PORT.println(F("max <d> <d> <d>                - set maximum positions"));
+    comPort->println(F("endstops?                      - get triggered endstops in (1,0,-1) format for max, none, min"));
+    comPort->println(F("home_min <axes?>               - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position"));
+    comPort->println(F("home_max <axes?>               - home given (00000zyx byte, e.g. 3 for x and y) or all axes to their max position"));
+    comPort->println(F("max_p?                         - return positions of max endstops"));
+    comPort->println(F("max <d> <d> <d>                - set maximum positions"));
 #endif
 
 #ifdef STEPSTICK
-    SERIAL_PORT.println(F("stepstick_move <f> <d>         - move stepper by <d> with max speed <f>"));
-    SERIAL_PORT.println(F("stepstick_release              - release stepstick stepper"));
-    SERIAL_PORT.println(F("stepstick_stop                 - abort stepstick move"));
+    comPort->println(F("stepstick_move <f> <d>         - move stepper by <d> with max speed <f>"));
+    comPort->println(F("stepstick_release              - release stepstick stepper"));
+    comPort->println(F("stepstick_stop                 - abort stepstick move"));
 #endif
-    //SERIAL_PORT.println(F("test_mode <s>                  - set test_mode <on> <off>"));
-    SERIAL_PORT.println(F("version                        - get firmware version string"));
-    SERIAL_PORT.println("");
-    SERIAL_PORT.println("Input Key:");
-    SERIAL_PORT.println(F("<d>                            - a decimal integer."));
-    SERIAL_PORT.println("");
-    SERIAL_PORT.println("--END--");
+    //comPort->println(F("test_mode <s>                  - set test_mode <on> <off>"));
+    comPort->println(F("version                        - get firmware version string"));
+    comPort->println("");
+    comPort->println("Input Key:");
+    comPort->println(F("<d>                            - a decimal integer."));
+    comPort->println("");
+    comPort->println("--END--");
 }
 #endif //HELP
\ No newline at end of file
diff --git a/src/modules/illumination/illumination.cpp b/src/modules/illumination/illumination.cpp
index 231a156..86ee9ab 100644
--- a/src/modules/illumination/illumination.cpp
+++ b/src/modules/illumination/illumination.cpp
@@ -1,29 +1,60 @@
 #include "config.h"
 #ifdef ILLUMINATION
 #include "illumination.h"
+#include <ComPort.h>
 #ifdef SUPPORT_EEPROM
     #include <EEPROM.h>
 #else
     #include "dummyEEPROM.h"
 #endif
 
+const uint16_t eeprom_offset = 100;
+
 const uint8_t pwm_led_pins[] = WIRING_PWM_LEDS;
+const uint16_t cc_value_eeprom = eeprom_offset;
+const uint16_t pwm_frequency_eeprom = cc_value_eeprom + sizeof(uint8_t);
+const uint16_t pwm_values_eeprom = pwm_frequency_eeprom + sizeof(uint16_t);
+
+uint32_t pwm_frequency;
+uint8_t cc_value;
+
+uint16_t pwm_values[PWM_NUM];
+
+
 void illumination_pwm_freq(String command)
 {
     char * args[1];
-    parse_arguments(args, command, 1);
+    uint8_t parsed = parse_arguments(args, command, 1);
     #ifdef MCU_PICO
-        analogWriteFreq(atol(args[0]));//32khz seems sensible
-        analogWriteRange(65535);
+        if (parsed > 0 && !(args[0] == "" || args[0][0] == '?'))
+        {
+            pwm_frequency = atol(args[0]);
+            EEPROM.put(pwm_frequency_eeprom, pwm_frequency);
+            analogWriteFreq(pwm_frequency);
+            analogWriteRange(65535);
+        }
 
-        SERIAL_PORT.print("Frequency set: ");
-        SERIAL_PORT.print(atol(args[0]));
-        SERIAL_PORT.println("Hz");
+        comPort->print("PWM Frequency:");
+        comPort->print(pwm_frequency);
+        comPort->println("Hz");
     #else
-        SERIAL_PORT.println("Feature not supported");
+        comPort->println("Feature not supported");
     #endif
 
-    free(args[0]);
+    if (parsed > 0)
+        free(args[0]);
+}
+
+void illumination_channels(String command)
+{
+    comPort->print("CC:");
+    #ifdef WIRING_CC_LED
+        comPort->print(1);
+    #else
+        comPort->print(0);
+    #endif
+    comPort->print(" PWM:");
+    comPort->println(PWM_NUM);
 }
 
 void illumination_pwm(String command)
@@ -31,11 +62,30 @@ void illumination_pwm(String command)
     #ifdef MCU_PICO
     //PWM frequency is shared between both PWM outputs
     char * args[2];
-    parse_arguments(args, command, 2);
+    uint8_t parsed = parse_arguments(args, command, 2);
+    if (parsed > 0 && args[0][0] == '?')
+    {
+        comPort->print("PWM:");
+        for(int i=0; i<PWM_NUM; i++)
+        {
+            #ifdef MCU_PICO
+                comPort->print(pwm_values[i]/65535.0);
+            #else
+                comPort->print(pwm_values[i]/255.0);
+            #endif
+            comPort->print(";");
+        }
+        comPort->println("");
+        free(args[0]);
+        if (parsed == 2)
+            free(args[1]);
+
+        return;
+    }
     uint8_t index = atoi(args[0]);
     if (index >= PWM_NUM)
     {
-        Serial.println("Invalid index");
+        comPort->println("Invalid index");
     }
     float val = atof(args[1]);
 
@@ -44,26 +94,28 @@ void illumination_pwm(String command)
         digitalWrite(pwm_led_pins[index], LOW);
     }
     #ifdef MCU_PICO
-        uint32_t pwm_val = val * 65535;
+        uint16_t pwm_val = val * 65535;
     #else
-        uint8_t pwm_val = val*255;
+        uint8_t pwm_val = val * 255;
     #endif
 
     analogWrite(pwm_led_pins[index], pwm_val);
-    SERIAL_PORT.print("Illumination set");
-    SERIAL_PORT.println(pwm_val);
+    pwm_values[index] = pwm_val;
+    EEPROM.put(pwm_values_eeprom+index*sizeof(uint16_t), pwm_val);
+    comPort->print("PWM:");
+    comPort->print(index);
+    comPort->print(":");
+    comPort->println(pwm_val);
 
-    free(args[0]);
-    free(args[1]);
+    if (parsed > 0)
+        free(args[0]);
+    if (parsed > 1)
+        free(args[1]);
     #endif
 }
 
 void illumination_setup()
 {
-    #ifdef MCU_PICO
-        analogWriteRange(65535);
-        analogWriteFreq(32000);//32khz seems sensible
-    #endif
     for (uint8_t i = 0; i < PWM_NUM; i++)
     {
         pinMode(pwm_led_pins[i], OUTPUT);
@@ -71,15 +123,32 @@ void illumination_setup()
     }
 
     #ifdef WIRING_CC_LED
+        EEPROM.get(cc_value_eeprom, cc_value);
         pinMode(WIRING_CC_LED, OUTPUT);
+        if (cc_value > 0)
+            cc_set_value(cc_value/32.0);
     #endif
 
+    #ifdef MCU_PICO
+        EEPROM.get(pwm_frequency_eeprom, pwm_frequency);
+        if (pwm_frequency < 1)
+            pwm_frequency = 64000;
+        analogWriteRange(65535);
+        analogWriteFreq(pwm_frequency);//32khz seems sensible
+    #endif
+
+    for(int i = 0; i < PWM_NUM; i++)
+    {
+        EEPROM.get(pwm_values_eeprom+i*sizeof(uint16_t), pwm_values[i]);
+        if (pwm_values[i] > 0)
+            analogWrite(pwm_led_pins[i], pwm_values[i]);
+    }
+
     register_module(illumination_commands, NULL);
 }
 
 #ifdef WIRING_CC_LED
-
-//these functions are useful testing
+//these functions are useful for testing
 float multi_read(uint8_t pin, uint16_t reads)
 {
     float total = 0;
@@ -97,13 +166,13 @@ void cc_test()
     for (float v = 0; v <= 1; v+=0.02)
     {
 
-        Serial.print(v);
-        Serial.print(" ");
+        comPort->print(v);
+        comPort->print(" ");
         cc_set_value(v);
-        Serial.print(" ");
+        comPort->print(" ");
         delay(2000);
         float current = multi_read(A0, 1000)/4095.0*3.3;
-        Serial.println(current*1000);
+        comPort->println(current*1000);
     }
 
     cc_set_value(0);
@@ -113,11 +182,17 @@ void cc_test()
 void cc_set(String command)
 {
     char * args[1];
-    parse_arguments(args, command, 1);
-    float val = atof(args[0]);
+    uint8_t parsed = parse_arguments(args, command, 1);
+    if (parsed > 0 && !(args[0] == "" || args[0][0] == '?'))
+    {
+        float val = atof(args[0]);
+        cc_set_value(val);
+    }
 
-    free(args[0]);
-    cc_set_value(val);
+    comPort->print("CC LED:");
+    comPort->println(cc_value/32.0);
+    if (parsed > 0)
+        free(args[0]);
 }
 
 void cc_set_value(float val)
@@ -129,6 +204,9 @@ void cc_set_value(float val)
             return;
         }
 
+        cc_value = 32 * val;
+        EEPROM.put(cc_value_eeprom, cc_value);
+
         const uint8_t td = 3; // >= 1.5 us
         const uint8_t tup = 2; // 1-75 us
         const uint8_t tdown = 180; //180-300us
@@ -155,7 +233,7 @@ void cc_set_value(float val)
             stepd = tdown;
         }
 
-        //step up
+        //step current
         uint8_t steps_done =0;
         for (uint8_t i=16; i!=setting; i+=dir)
         {
@@ -165,15 +243,15 @@ void cc_set_value(float val)
             delayMicroseconds(td);
             steps_done++;
         }
-        Serial.print(steps_done);
     #else
-        SERIAL_PORT.println("CC LED not supported");
+        comPort->println("CC LED not supported");
     #endif
 }
 
 extern const Command illumination_commands[] = {
     {"led_pwm", illumination_pwm},
-    {"led_freq", illumination_pwm_freq},
-    {"led_cc_set", cc_set},
+    {"led_channels", illumination_channels},
+    {"led_frequency", illumination_pwm_freq},
+    {"led_cc", cc_set},
     END_COMMAND};
 #endif
\ No newline at end of file
diff --git a/src/modules/illumination/illumination.h b/src/modules/illumination/illumination.h
index 2e4ae70..47044ce 100644
--- a/src/modules/illumination/illumination.h
+++ b/src/modules/illumination/illumination.h
@@ -7,6 +7,7 @@
 
 void illumination_pwm(String command);
 void illumination_pwm_freq(String command);
+void illumination_channels(String command);
 
 void cc_set(String command);
 void cc_set_value(float val);
diff --git a/src/modules/light_sensor/light_sensor.cpp b/src/modules/light_sensor/light_sensor.cpp
index 70b89e6..b4f33c1 100644
--- a/src/modules/light_sensor/light_sensor.cpp
+++ b/src/modules/light_sensor/light_sensor.cpp
@@ -1,4 +1,5 @@
 #include "config.h"
+#include <ComPort.h>
 #ifdef LIGHT_SENSOR
 #include "light_sensor.h"
 #include "config.h"
@@ -54,35 +55,35 @@ void setup_light_sensor_device()
     }
     else
     {
-        SERIAL_PORT.println(F("No light sensor found.  NB your board will start up faster if you recompile without light sensor support."));
+        comPort->println(F("No light sensor found.  NB your board will start up faster if you recompile without light sensor support."));
     }
 }
 
 void print_light_sensor_gain()
 {
     // Print the current gain value of the light sensor
-    SERIAL_PORT.print(F("light sensor gain "));
+    comPort->print(F("light sensor gain "));
     tsl2591Gain_t gain = tsl.getGain();
     switch (gain)
     {
     case TSL2591_GAIN_LOW:
-        SERIAL_PORT.println(F("1x (Low)"));
+        comPort->println(F("1x (Low)"));
         break;
     case TSL2591_GAIN_MED:
-        SERIAL_PORT.println(F("25x (Medium)"));
+        comPort->println(F("25x (Medium)"));
         break;
     case TSL2591_GAIN_HIGH:
-        SERIAL_PORT.println(F("428x (High)"));
+        comPort->println(F("428x (High)"));
         break;
     case TSL2591_GAIN_MAX:
-        SERIAL_PORT.println(F("9876x (Max)"));
+        comPort->println(F("9876x (Max)"));
         break;
     }
 }
 void light_sensor_gain_values(String command)
 {
     // Print the allowable gain values of the light sensor
-    SERIAL_PORT.println(F("light sensor gains: 1x, 25x, 428x, 9876x"));
+    comPort->println(F("light sensor gains: 1x, 25x, 428x, 9876x"));
 }
 
 void set_light_sensor_gain(int newgain)
@@ -103,7 +104,7 @@ void set_light_sensor_gain(int newgain)
         tsl.setGain(TSL2591_GAIN_MAX);
         break;
     default:
-        SERIAL_PORT.println(F("Error: gain may only be 1, 25, 428, or 9876."));
+        comPort->println(F("Error: gain may only be 1, 25, 428, or 9876."));
         return;
     }
     print_light_sensor_gain();
@@ -112,16 +113,16 @@ void set_light_sensor_gain(int newgain)
 void light_sensor_integration_time(String command)
 {
     // Print the current integration time in milliseconds.
-    SERIAL_PORT.print(F("light sensor integration time "));
-    SERIAL_PORT.print((tsl.getTiming() + 1) * 100, DEC);
-    SERIAL_PORT.println(F(" ms"));
+    comPort->print(F("light sensor integration time "));
+    comPort->print((tsl.getTiming() + 1) * 100, DEC);
+    comPort->println(F(" ms"));
 }
 
 void light_sensor_intensity(String command)
 {
     // Print the current light value
     uint16_t x = tsl.getLuminosity(TSL2591_FULLSPECTRUM);
-    SERIAL_PORT.println(x, DEC);
+    comPort->println(x, DEC);
 }
 #endif // ADAFRUIT_TSL2591
 
@@ -137,34 +138,34 @@ void setup_light_sensor_device()
 void print_light_sensor_gain()
 {
     // Print the current gain value of the light sensor
-    SERIAL_PORT.print(F("light sensor gain "));
+    comPort->print(F("light sensor gain "));
     adsGain_t gain = ads.getGain();
     switch (gain)
     {
     case GAIN_TWOTHIRDS:
-        SERIAL_PORT.println(F("0.66x (specify 0)"));
+        comPort->println(F("0.66x (specify 0)"));
         break;
     case GAIN_ONE:
-        SERIAL_PORT.println(F("1x"));
+        comPort->println(F("1x"));
         break;
     case GAIN_TWO:
-        SERIAL_PORT.println(F("2x"));
+        comPort->println(F("2x"));
         break;
     case GAIN_FOUR:
-        SERIAL_PORT.println(F("4x"));
+        comPort->println(F("4x"));
         break;
     case GAIN_EIGHT:
-        SERIAL_PORT.println(F("8x"));
+        comPort->println(F("8x"));
         break;
     case GAIN_SIXTEEN:
-        SERIAL_PORT.println(F("16x"));
+        comPort->println(F("16x"));
         break;
     }
 }
 void light_sensor_gain_values(String command)
 {
     // Print the allowable gain values of the light sensor
-    SERIAL_PORT.println(F("light sensor gains: 0.66x (specify 0), 1x, 2x, 4x, 8x, 16x"));
+    comPort->println(F("light sensor gains: 0.66x (specify 0), 1x, 2x, 4x, 8x, 16x"));
 }
 
 void set_light_sensor_gain(int newgain)
@@ -191,7 +192,7 @@ void set_light_sensor_gain(int newgain)
         ads.setGain(GAIN_SIXTEEN);
         break;
     default:
-        SERIAL_PORT.println(F("Error: gain may only be 0, 1, 2, 4, 8, 16 (0 means 2/3)."));
+        comPort->println(F("Error: gain may only be 0, 1, 2, 4, 8, 16 (0 means 2/3)."));
         return;
     }
     print_light_sensor_gain();
@@ -200,7 +201,7 @@ void set_light_sensor_gain(int newgain)
 void light_sensor_integration_time(String command)
 {
     // Print the current integration time in milliseconds.
-    SERIAL_PORT.println(F("integration time not supported for ADS1115"));
+    comPort->println(F("integration time not supported for ADS1115"));
 }
 
 void light_sensor_intensity(String command)
@@ -215,7 +216,7 @@ void light_sensor_intensity(String command)
     uint16_t x = ads.readADC_SingleEnded(ch); //single ended measurement on pin ch (0-3)
     //uint16_t x = ads.readADC_Differential_0_1(); //differential measurement on pins 0,1
     
-    SERIAL_PORT.println(x, DEC);
+    comPort->println(x, DEC);
 }
 #endif // ADAFRUIT_ADS1115
 #endif
\ No newline at end of file
diff --git a/src/modules/stage/stage.cpp b/src/modules/stage/stage.cpp
index 329cf86..a8f0b90 100644
--- a/src/modules/stage/stage.cpp
+++ b/src/modules/stage/stage.cpp
@@ -8,18 +8,24 @@
 #include "dummyEEPROM.h"
 #endif
 #include "main.h"
+#include <ComPort.h>
+
+#if MAX_STEPPERS > 3 && !defined(WIRING_ADDITIONAL_STEPPER)
+#error "Define WIRING_ADDITIONAL_STEPPER to support 4th stepper"
+#endif
 
 // The array below has 3 stepper objects, for X,Y,Z respectively
-const uint8_t n_motors = STAGE_N_MOTORS;
+uint8_t n_motors = 3;
+const uint8_t stage_position_eeprom = 0;
 long min_step_delay = -1;
-const uint8_t min_step_delay_eeprom = sizeof(long) * n_motors;
+const uint8_t min_step_delay_eeprom = stage_position_eeprom + sizeof(long) * STAGE_MAX_MOTORS;
 long ramp_time = -1;
-const uint8_t ramp_time_eeprom = sizeof(long) * (n_motors + 1);
-const uint8_t axis_max_eeprom = sizeof(long) * (n_motors + 2);
-const uint8_t non_blocking_moves_eeprom = sizeof(long) * (n_motors + 3);
-Stepper *motors[n_motors];
-signed long current_pos[n_motors];
-long steps_remaining[n_motors];
+const uint8_t ramp_time_eeprom = min_step_delay_eeprom + sizeof(long);
+const uint8_t axis_max_eeprom = ramp_time_eeprom + sizeof(long);
+const uint8_t non_blocking_moves_eeprom = axis_max_eeprom + sizeof(long);
+Stepper *motors[STAGE_MAX_MOTORS];
+signed long current_pos[STAGE_MAX_MOTORS];
+long steps_remaining[STAGE_MAX_MOTORS];
 
 bool stage_moving = false;
 bool notify_on_stop = false;
@@ -31,7 +37,15 @@ void stage_setup()
     motors[0] = new Stepper(8, WIRING_MOTOR_X);
     motors[1] = new Stepper(8, WIRING_MOTOR_Y);
     motors[2] = new Stepper(8, WIRING_MOTOR_Z);
-    EACH_MOTOR
+    #if STAGE_MAX_MOTORS > 3
+        //TODO: support >4 stepppers
+        for (int i = 0; i < STAGE_MAX_MOTORS - 3; i++)
+        {
+            motors[i+3] = new Stepper(8, WIRING_ADDITIONAL_STEPPER);
+        }
+    #endif
+
+    EACH_MAX_MOTOR
     {
         motors[i]->setSpeed(10); // as a default set to 10 rpm, though this is ignored...
         steps_remaining[i] = 0;
@@ -53,7 +67,7 @@ void stage_setup()
     }
 
     //using non-blocking moves here to have blocking moves by default
-    EEPROM.get(non_blocking_moves_eeprom, non_blocking_moves);
+    non_blocking_moves = read_eeprom_bool(non_blocking_moves_eeprom, false);
 
     register_module(stage_commands, stage_loop);
 }
@@ -76,20 +90,20 @@ void print_position()
     EACH_MOTOR
     {
         if (i > 0)
-            SERIAL_PORT.print(" ");
-        SERIAL_PORT.print(current_pos[i]);
+            comPort->print(" ");
+        comPort->print(current_pos[i]);
     }
-    SERIAL_PORT.println();
+    comPort->println();
 }
 
 unsigned long move_start_time = 0;
-unsigned long distance_moved[n_motors];
-long displacement[n_motors];
+unsigned long distance_moved[STAGE_MAX_MOTORS];
+long displacement[STAGE_MAX_MOTORS];
 float final_scaled_t;
-float step_delay[n_motors];
-int8_t move_directions[n_motors];
+float step_delay[STAGE_MAX_MOTORS];
+int8_t move_directions[STAGE_MAX_MOTORS];
 
-void start_move(long displ[n_motors])
+void start_move(long displ[STAGE_MAX_MOTORS])
 {
     // move all the axes in a nice move
     // split displacements into magnitude and direction, and find max. travel
@@ -124,7 +138,7 @@ void start_move(long displ[n_motors])
     }
     else
     {
-        SERIAL_PORT.println("done.");
+        comPort->println("done.");
     }
 }
 
@@ -180,10 +194,11 @@ void stage_loop()
             }
         }
     }
+    EEPROM.put(0, current_pos); // ensure our position is held in EEPROM so it persists
 
     if (!stage_moving && notify_on_stop)
     {
-        SERIAL_PORT.println("stopped");
+        comPort->println("stopped");
         notify_on_stop = false;
     }
 }
@@ -216,13 +231,10 @@ void stage_mrz(String command)
 
 void stage_mr(String command)
 {
-    char *args[3];
-    parse_arguments(args, command, 3);
+    char *args[STAGE_MAX_MOTORS];
+    parse_arguments(args, command, n_motors);
     EACH_MOTOR
     {
-        if (i>3)
-            continue;
-
         displacement[i] = atol(args[i]);
         free(args[i]);
     }
@@ -236,7 +248,7 @@ void stage_release(String command)
     {
         releaseMotor(i);
     }
-    SERIAL_PORT.println("done");
+    comPort->println("done");
 }
 
 void stage_p(String command)
@@ -250,14 +262,14 @@ void stage_min_step_delay(String command)
     parse_arguments(args, command, 1);
     if (args[0][0] == '?')
     {
-        SERIAL_PORT.print("minimum step delay ");
-        SERIAL_PORT.println(min_step_delay);
+        comPort->print("minimum step delay ");
+        comPort->println(min_step_delay);
     }
     else
     {
         min_step_delay = atol(args[0]);
         EEPROM.put(min_step_delay_eeprom, min_step_delay);
-        SERIAL_PORT.println("done.");
+        comPort->println("done.");
     }
     free(args[0]);
 }
@@ -268,14 +280,14 @@ void stage_ramp_time(String command)
     parse_arguments(args, command, 1);
     if (args[0][0] == '?')
     {
-        SERIAL_PORT.print("ramp_time ");
-        SERIAL_PORT.println(ramp_time);
+        comPort->print("ramp_time ");
+        comPort->println(ramp_time);
     }
     else
     {
         ramp_time = atol(args[0]);
         EEPROM.put(ramp_time_eeprom, ramp_time);
-        SERIAL_PORT.println("done.");
+        comPort->println("done.");
     }
     free(args[0]);
 }
@@ -286,41 +298,54 @@ void update_blocking_moves(String command)
     parse_arguments(args, command, 1);
     if (args[0][0] == '?')
     {
-        SERIAL_PORT.print("blocking_moves ");
-        SERIAL_PORT.println(non_blocking_moves ? "false" : "true");
+        comPort->print("blocking_moves ");
+        comPort->println(non_blocking_moves ? "false" : "true");
     }
     else
     {
         non_blocking_moves = !(args[0][0] == 't');
         EEPROM.put(non_blocking_moves_eeprom, non_blocking_moves);
-        SERIAL_PORT.println("done.");
+        comPort->println("done.");
     }
 }
 
+void stage_n_motors(String command)
+{
+    char *args[1];
+    parse_arguments(args, command, 1);
+    if (args[0][0] != '?')
+        n_motors = atoi(args[0]);
+
+    comPort->print("n_motors ");
+    comPort->println(n_motors);
+
+    free(args[0]);
+}
+
 void stage_zero(String command)
 {
     EACH_MOTOR current_pos[i] = 0;
-    SERIAL_PORT.println(F("position reset to 0 0 0"));
+    comPort->println(F("position reset to 0 0 0"));
     EEPROM.put(0, current_pos);
 }
 
 void stage_stop(String command)
 {
     stage_moving = false;
-    SERIAL_PORT.println("Move aborted");
+    comPort->println("Move aborted");
 }
 
 void is_stage_moving(String command)
 {
     //TODO: should I bother checking for a ?
-    SERIAL_PORT.println(stage_moving ? "true" : "false");
+    comPort->println(stage_moving ? "true" : "false");
 }
 
 void activate_notify_on_stop(String command)
 {
     if (!stage_moving)
     {
-        SERIAL_PORT.println("Error: stage is not moving");
+        comPort->println("Error: stage is not moving");
     }
     else
     {
@@ -345,4 +370,5 @@ extern const Command stage_commands[] = {
     {"moving", is_stage_moving},
     {"notify_on_stop", activate_notify_on_stop},
     {"blocking_moves", update_blocking_moves},
+    {"n_motors", stage_n_motors},
     END_COMMAND};
\ No newline at end of file
diff --git a/src/modules/stage/stage.h b/src/modules/stage/stage.h
index 68b0b16..942a528 100644
--- a/src/modules/stage/stage.h
+++ b/src/modules/stage/stage.h
@@ -3,7 +3,9 @@
 #include "main.h"
 #include <Arduino.h>
 
-#define EACH_MOTOR for (int i = 0; i < STAGE_N_MOTORS; i++)
+#define EACH_MOTOR for (int i = 0; i < n_motors; i++)
+#define EACH_MAX_MOTOR for (int i = 0; i < STAGE_MAX_MOTORS; i++)
+
 
 void stage_setup();
 void stage_loop();
@@ -21,6 +23,7 @@ void stage_ramp_time(String command);
 void stage_zero(String command);
 void start_move(long displ[]);
 void stage_stop(String command);
+void stage_n_motors(String command);
 
 extern const Command stage_commands[];
 //we want to expose these for endstops (and potentially other modules)
diff --git a/src/modules/stepstick/stepstick.cpp b/src/modules/stepstick/stepstick.cpp
index 6a26c8a..d364fd3 100644
--- a/src/modules/stepstick/stepstick.cpp
+++ b/src/modules/stepstick/stepstick.cpp
@@ -1,6 +1,7 @@
 #include "config.h"
 #ifdef STEPSTICK
 #include "stepstick.h"
+#include <ComPort.h>
 #ifdef SUPPORT_EEPROM
     #include <EEPROM.h>
 #else
@@ -16,13 +17,13 @@ bool moving = false;
 void stepstick_release(String command)
 {
     digitalWrite(stepstick_pins[0], HIGH);//start with motor disabled
-    SERIAL_PORT.println("Stepstick released");
+    comPort->println("Stepstick released");
 }
 
 void stepstick_stop(String command)
 {
     stepper.stop();
-    SERIAL_PORT.println("Stepstick stopping");
+    comPort->println("Stepstick stopping");
 }
 
 void stepstick_move(String command)
@@ -37,7 +38,7 @@ void stepstick_move(String command)
     stepper.moveTo(stepper.currentPosition() + distance);
     free(args[0]);
     free(args[1]);
-    SERIAL_PORT.println("Stepstick started");
+    comPort->println("Stepstick started");
     moving = true;
 }
 
@@ -55,7 +56,7 @@ void stepstick_loop()
     if (stepper.distanceToGo() == 0 && moving)
     {
         moving = false;
-        SERIAL_PORT.println("Stepstick stopped");
+        comPort->println("Stepstick stopped");
     }
 }
 
diff --git a/test/test_argument_parsing.cpp b/test/test_argument_parsing.cpp
index f8b01ba..adee2ba 100644
--- a/test/test_argument_parsing.cpp
+++ b/test/test_argument_parsing.cpp
@@ -9,7 +9,7 @@ void test_single_argument_parsing(void)
     String all_args = F("3123.123asdf");
     char * args[1];
     uint8_t ret = 0;
-  
+
     ret = parse_arguments(args, all_args, 1);
     TEST_ASSERT_EQUAL_STRING(expected_single, args[0]);
     TEST_ASSERT_EQUAL(1, ret);
@@ -70,10 +70,11 @@ void test_multiple_argument_parsing(void)
 
 void setup() {
 
+    //TODO: support serial selection in tests
     Serial.begin(115200);
     // NOTE!!! Wait for >2 secs
-    // if board doesn't support software reset via Serial.DTR/RTS
-    delay(2000);
+    // if board doesn't support software reset via comPort->DTR/RTS
+    delay(5000);
     UNITY_BEGIN();    // IMPORTANT LINE!
     //bizzare string issues caused this to fail in specific sequences, so we run it multiple times
     RUN_TEST(test_multiple_argument_parsing);