Add endstops, light_sensor, test, many fixes

- Added endstop and light_sensor module
 - Improved configuration by splitting wiring configs to boards.h
 - Added a unit test for argument parsing
 - Fixed many smaller bugs

README.md

@@ -1,8 +1,8 @@
 # Firmware code for the sangaboard
 
-*This firmware is Work in Progress and has not reached feature parity with the original yet (see below for more details)*
+*This firmware is Work in Progress and while it should now support all features of the original, many features are untested.*
 
-Firmware for the control board of OpenFlexure Microscope. This is a refactor of the [original arduino code](https://gitlab.com/bath_open_instrumentation_group/sangaboard/-/blob/master/arduino_code/README.md) with some improvements to the infrastructure and features.
+Firmware for the control board of OpenFlexure Microscope. This is a refactor of the [original arduino code](https://gitlab.com/bath_open_instrumentation_group/sangaboard/-/blob/master/arduino_code) with some improvements to the infrastructure and features.
 
 ## Notable changes
 This version of the firmware uses PlatformIO rather than Arduino IDE.
@@ -11,6 +11,8 @@ The code is split into the main part which mostly handles command parsing and mo
 
 Motor moves now do not block command processing and a new command `stop` was added which aborts any move in progress.
 
+The firware can be build against different platforms (pi pico, stm32). `boards.h` contains wiring configuration which will need to be adjusted when used with a custom board.
+
 ## Hardware
 
 Currently works on Arduino Nano + Sangaboard v0.2. Other platforms might work with appropriate `platform.ini` and `config.h` changes and will be supported in due course.
@@ -18,14 +20,22 @@ Currently works on Arduino Nano + Sangaboard v0.2. Other platforms might work wi
 ## TODO list
 
 ### Original firmware feature parity
-  - [ ] Implement Endstops module
-  - [ ] Implement Light sensor modules
-  - [ ] Handle Sangaboardv3
+  - [x] Implement Endstops module
+  - [x] Implement Light sensor modules
+  - [x] Handle Sangaboardv3
+
+### Testing
   - [ ] Ensure responses to commands match the original version
+  - [ ] Test endstop modules
+  - [ ] Test Light sensor module
+  - [ ] Test on different platforms
 
 ### Improvements and fixes
   - [ ] Add a CI script
-  - [ ] Add tests (argument parsing)
+  - [x] Add test for argument parsing
+  - [ ] Add more unit tests
   - [ ] Test automatic building and upload
-  - [ ] Support other platforms
-  - [ ] Clean up some of the messier bits
+  - [x] Support other platforms
+  - [ ] Clean up
+  - [ ] Improve handling of unused modules/libraries
+  - [ ] Lower RAM use

platformio.ini

@@ -8,8 +8,35 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:nanoatmega328new]
+[env]
+monitor_speed = 115200
+framework = arduino
+test_build_project_src = true
+lib_deps =
+  adafruit/Adafruit TSL2591 Library @ ^1.3.1  
+  adafruit/Adafruit ADS1X15 @ ^2.1.1
+  adafruit/Adafruit BusIO @ ^1.7.3
+  SPI
+lib_ldf_mode = chain
+
+[env:nano]
 platform = atmelavr
 board = nanoatmega328new
-framework = arduino
-monitor_speed = 115200
+build_flags = -D MCU_NANO -lc
+
+[env:leonardo]
+platform = atmelavr
+board = leonardo
+monitor_speed = 115200
+build_flags = -D MCU_LEONARDO
+
+[env:pico]
+platform = raspberrypi
+board = pico
+build_flags = -D MCU_PICO
+
+[env:bluepill]
+platform = ststm32
+board = bluepill_f103c8
+upload_protocol = stlink
+build_flags = -D MCU_BLUEPILL

src/boards.h

@@ -0,0 +1,41 @@
+#ifdef BOARD_AUTO
+    #ifdef MCU_LEONARDO
+        #define BOARD_SANGABOARDv3
+        #define BOARD_STRING "Sangaboard v0.3"
+    #elif defined(MCU_NANO) || defined(MCU_PICO) || defined(MCU_BLUEPILL)
+        #define BOARD_SANGABOARDv2
+        #define BOARD_STRING "Sangaboard v0.2"
+    #else
+        #error "Invalid build config - must specify MCU type"
+    #endif
+#endif
+
+#if defined(BOARD_SANGABOARDv3)
+    #define BOARD_STRING "Sangaboard v0.3"
+    #define WIRING_MOTOR_X 8, 9, 10, 11
+    #define WIRING_MOTOR_Y 5, 13, 4, 12
+    #define WIRING_MOTOR_Z 6, 7, A5, A4
+    #define ENDSTOPS_INVERT false
+    #define ENDSTOPS_PULLUPS true
+    #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
+    #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
+#elif defined(BOARD_SANGABOARDv2)
+    #define BOARD_STRING "Sangaboard v0.2"
+    #define WIRING_MOTOR_X 13, 12, 11, 10
+    #define WIRING_MOTOR_Y 9, 8, 7, 6
+    #define WIRING_MOTOR_Z 5, 4, 3, 2
+    #define ENDSTOPS_INVERT false
+    #define ENDSTOPS_PULLUPS true
+    #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
+    #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
+#else
+    //custom board
+    #define BOARD_STRING "Custom board"
+    #define WIRING_MOTOR_X 13, 12, 11, 10
+    #define WIRING_MOTOR_Y 9, 8, 7, 6
+    #define WIRING_MOTOR_Z 5, 4, 3, 2
+    #define ENDSTOPS_INVERT false
+    #define ENDSTOPS_PULLUPS true
+    #define WIRING_ENDSTOPS_MIN {A0,A1,A2}
+    #define WIRING_ENDSTOPS_MAX {A3,A4,A5}
+#endif

src/config.h

@@ -1,27 +1,45 @@
 #ifndef CONFIGURED
+    //board choice
+    #define BOARD_AUTO
+    //#define BOARD_SANGABOARDv3
+    //#define BOARD_SANGABOARDv2
+    //#define BOARD_CUSTOM
+    #include <boards.h> //defines wiring
+
     //general settings
     #define MAX_COMMANDS 50
     #define MAX_MODULES 10
+    #define MAX_ARGUMENT_LENGTH 20 //used in argument parsing, takes up ram
     #define VERSION_STRING "Sangaboard Firmware v0.6"
     #define DEBUG_ON
 
     //module choice
     #define HELP
     #define STAGE
+    #define ENDSTOPS
+ //   #define LIGHT_SENSOR
 
     //module configs
     #ifdef STAGE
-        //TODO: Fix this for platformio
-        #ifdef ARDUINO_AVR_LEONARDO
-            #define SANGABOARDv3
-            #define BOARD_STRING "Sangaboard v0.3"
-        #elif ARDUINO_AVR_SANGABOARD
-            #define SANGABOARDv3
-            #define BOARD_STRING "Sangaboard v0.3"
-        #else
-            #define SANGABOARDv2
-            #define BOARD_STRING "Sangaboard v0.2"
+        #define STAGE_N_MOTORS 3
+    #endif
+
+    #ifdef ENDSTOPS
+        #define ENDSTOPS_MIN
+        //#define ENDSTOPS_MAX
+        #if defined(ENDSTOPS_MIN) || defined(ENDSTOPS_MAX)
+            //endstops are closed when the pin is down  by default, this inverts the behaviour
+            //soft endstops trigger when 0/MAX is reached in the direction without endstops
+            #define ENDSTOPS_SOFT
         #endif
     #endif
+
+    #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

src/dummyEEPROM.h

@@ -0,0 +1,24 @@
+
+#include <stdint.h>
+
+struct EEPROMClass
+{
+    //Basic user access methods.
+    uint8_t read(int idx) { return -1; }
+    void write(int idx, uint8_t val) { return; }
+    void update(int idx, uint8_t val) { return; }
+
+    template <typename T>
+    T &get(int idx, T &t)
+    {
+        return t;
+    }
+
+    template <typename T>
+    const T &put(int idx, const T &t)
+    {
+        return t;
+    }
+};
+
+static EEPROMClass EEPROM;

src/main.cpp

@@ -28,6 +28,14 @@
 #include "modules/stage/stage.h"
 #endif
 
+#ifdef ENDSTOPS
+#include "modules/endstops/endstops.h"
+#endif
+
+#ifdef LIGHT_SENSOR
+#include "modules/light_sensor/light_sensor.h"
+#endif
+
 Command registered_commands[MAX_COMMANDS];
 void (*registered_loop_functions[MAX_MODULES])(void);
 
@@ -62,19 +70,24 @@ void handle_command(String received_command)
 #endif
 }
 
-uint8_t parse_arguments(String arguments[], String command, uint8_t max_args)
+uint8_t parse_arguments(char ** arguments, String command, uint8_t max_args)
 {
+    char buffer[MAX_ARGUMENT_LENGTH];
+
     uint8_t from = 0;
+    while (command[from] == ' ' && from < command.length())
+        from++;
+
     uint8_t parsed = 0;
     while (from < command.length() && parsed < max_args)
     {
-        int space = command.indexOf(" ", from + 1);
+        int space = command.indexOf(' ', from);
         if (space == -1)
             space = command.length();
-        arguments[parsed] = command.substring(from, space);
-        arguments[parsed++].trim();
-
-        from = space;
+        memcpy(buffer, &command[from], space - from);
+        buffer[space - from] = '\0';
+        arguments[parsed++] = strdup(buffer);
+        from = space + 1;
     }
 
     return parsed;
@@ -97,7 +110,6 @@ void setup()
     register_module(core_commands, NULL);
 
 #ifdef HELP
-    // D(help_commands_t[0].command);
     help_setup();
 #endif
 
@@ -107,11 +119,11 @@ void setup()
 #endif
 
 #ifdef LIGHT_SENSOR
-    light_sensor_setup(); //TODO
+    light_sensor_setup();
 #endif
 
 #ifdef ENDSTOPS
-    light_sensor_setup(); //TODO
+    endstops_setup();
 #endif
 
     registered_commands[registered_commands_count] = END_COMMAND;

src/main.h

@@ -19,7 +19,7 @@ struct Command
     void (*run)(String);
 };
 
-uint8_t parse_arguments(String[], String, uint8_t);
+uint8_t parse_arguments(char ** arguments, String, uint8_t);
 extern void register_module(const Command commands[], void (*loop_fn)(void));
 
 void get_version(String);

src/modules/endstops/endstops.cpp

@@ -0,0 +1,307 @@
+#include "config.h"
+#ifdef ENDSTOPS
+#include "endstops.h"
+#include "../stage/stage.h"
+#ifdef SUPPORT_EEPROM
+    #include <EEPROM.h>
+#else
+    #include "dummyEEPROM.h"
+#endif
+
+unsigned long retreat_steps = 5000;
+unsigned long home_move_steps = 100000;
+unsigned long axis_max[STAGE_N_MOTORS];
+const int axis_max_eeprom = sizeof(long)*(STAGE_N_MOTORS+2);
+#ifdef ENDSTOPS_MIN
+const int endstops_min_pins[] = WIRING_ENDSTOPS_MIN;
+#endif
+#ifdef ENDSTOPS_MAX
+const int endstops_max_pins[] = WIRING_ENDSTOPS_MAX;
+#endif
+bool endstops_enabled = true;
+
+//we want to be able to abort homing, so all calls are non-blocking
+//we start with a very long move towards the endstops
+//when this is finished we start a move away from the endstops
+//when this is finished we set a very slow speed and start a move
+//back towards the endstops, after which we move away a few thousands
+//steps (the switch has some hysteresis)
+bool homing_initial_move = false;
+bool homing_moving_away = false;
+bool homing_final = false;
+uint8_t homing_axes = 0;
+int8_t homing_direction = 0;
+long previous_step_delay = 0;
+
+boolean endstop_triggered(uint8_t axis, int8_t direction)
+{
+#if defined(ENDSTOPS_MIN)
+    if (direction < 0)
+    {
+        int value = analogRead(endstops_min_pins[axis]);
+        if (ENDSTOPS_INVERT != (value < 100))
+            return true;
+    }
+#elif defined(ENDSTOPS_SOFT)
+    if (direction < 0 && current_pos[axis] < 1)
+        return true;
+#endif
+
+#if defined(ENDSTOPS_MAX)
+    if (direction > 0)
+    {
+        int value = analogRead(endstops_max_pins[axis]);
+        if (ENDSTOPS_INVERT != (value < 100))
+            return true;
+    }
+#elif defined(ENDSTOPS_SOFT)
+    if (direction > 0 && current_pos[axis] >= (signed long) axis_max[axis])
+        return true;
+#endif
+    return false;
+}
+
+void endstops_loop()
+{
+    if (!endstops_enabled)
+        return;
+
+    endstops_check();
+
+    if (homing_initial_move)
+    {
+        if (stage_moving)
+            return;
+        D(F("Homing initial move complete"));
+        homing_initial_move = false;
+        homing_moving_away = true;
+        home_retreat();
+        return;
+    }
+
+    if (homing_moving_away)
+    {
+        if (stage_moving)
+            return;
+        D(F("Homing retreat complete"));
+
+        homing_moving_away = false;
+        homing_final = true;
+        home_final();
+        return;
+    }
+
+    if (homing_final)
+    {
+        if (stage_moving)
+            return;
+        D(F("Homing complete complete"));
+        homing_final = false;
+        min_step_delay = previous_step_delay;
+        home_retreat();
+        Serial.println("done.");
+        return;
+    }
+}
+
+int8_t endstops_check()
+{
+    int8_t endstop_break = 0;
+#if defined(ENDSTOPS_MIN) || defined(ENDSTOPS_MAX)
+    EACH_MOTOR
+    {
+        //TODO: why the second part? isn't move direction enough?
+        if (endstop_triggered(i, move_directions[i]))
+        {
+            endstop_break = move_directions[i] * (i + 1);
+            D(endstop_break);
+            D(move_directions[i]);
+        }
+    }
+
+    if (endstop_break != 0 && stage_moving)
+    {
+        Serial.print(F("Endstop hit:"));
+        Serial.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
+        //the predefined axis_max are the travel distances in steps
+        if (endstop_break < 0)
+        {
+            current_pos[-endstop_break - 1] = 0;
+            //abort move in this direction
+            displacement[-endstop_break - 1] = 0;
+        }
+        else
+        {
+            displacement[endstop_break - 1] = 0;
+#if defined(ENDSTOPS_MIN) && defined(ENDSTOPS_MAX)
+            axis_max[endstop_break - 1] = current_pos[endstop_break - 1];
+#elif defined(ENDSTOP_MAX) //we do not do this for ENDSTOPS_SOFT
+            current_pos[endstop_break - 1] = axis_max[endstop_break - 1];
+#else
+            ;
+        }
+#endif
+    }
+#endif //ENDSTOPS_MIN || ENDSTOPS_MAX
+    return endstop_break;
+}
+
+void endstops_status(String command)
+{
+    EACH_MOTOR
+    {
+        if (i > 0)
+            Serial.print(" ");
+        if (endstop_triggered(i, -1))
+            Serial.print("-1");
+        else if (endstop_triggered(i, 1))
+            Serial.print("1");
+        else
+            Serial.print("0");
+    }
+
+    Serial.println();
+}
+
+void print_axes_max(String command)
+{
+    EACH_MOTOR
+    {
+        if (i > 0)
+            Serial.print(" ");
+        Serial.print(axis_max[i]);
+    }
+    Serial.println();
+}
+
+void home_start(uint8_t axes, int8_t direction)
+{
+    homing_axes = axes;
+    homing_direction = direction;
+    homing_initial_move = true;
+    long displacement[3];
+    EACH_MOTOR
+    {
+        if (((homing_axes >> i) & 1) == 1)
+            displacement[i] = homing_direction * home_move_steps;
+        else
+            displacement[i] = 0;
+    }
+
+    start_move(displacement);
+}
+
+void home_retreat()
+{
+    long displacement[3];
+    EACH_MOTOR
+    {
+        if (((homing_axes >> i) & 1) == 1)
+            displacement[i] = -homing_direction * retreat_steps;
+        else
+            displacement[i] = 0;
+    }
+    start_move(displacement);
+}
+
+void home_final()
+{
+    long displacement[3];
+    EACH_MOTOR
+    {
+        if (((homing_axes >> i) & 1) == 1)
+            displacement[i] = 2 * homing_direction * retreat_steps;
+        else
+            displacement[i] = 0;
+    }
+
+    previous_step_delay = min_step_delay;
+    min_step_delay = 4000;
+    start_move(displacement);
+}
+
+void endstops_home_min(String command)
+{
+#ifndef ENDSTOPS_MIN
+    Serial.println(F("Min endstops not installed"));
+    return;
+#endif
+    if (command.equals("home_min"))
+        return home_start(7, -1);
+    char * arg[1];
+    parse_arguments(arg, command, 1);
+    uint8_t axes = atoi(arg[0]);
+    free(arg[0]);
+    home_start(axes, -1);
+}
+
+void endstops_home_max(String command)
+{
+#ifndef ENDSTOPS_MAX
+    Serial.println(F("Max endstops not installed"));
+    return;
+#endif
+    if (command.equals("home_max"))
+        return home_start(7, 1);
+    char * arg[1];
+    parse_arguments(arg, command, 1);
+    uint8_t axes = atoi(arg[0]);
+    free(arg[0]);
+    home_start(axes, 1);
+}
+
+void endstops_set_max(String command)
+{
+    char * args[3];
+    parse_arguments(args, command, 3);
+    EACH_MOTOR
+    {
+        axis_max[i] = atol(args[i]);
+        free(args[i]);
+    }
+    EEPROM.put(axis_max_eeprom, axis_max);
+
+}
+
+void endstops_setup()
+{
+    #ifdef ENDSTOPS_MIN
+    EACH_MOTOR{
+      if(ENDSTOPS_PULLUPS)
+        pinMode(endstops_min_pins[i], INPUT_PULLUP);
+      else
+        pinMode(endstops_min_pins[i], INPUT);
+    }
+    #endif
+
+    #ifdef ENDSTOPS_MAX
+    EACH_MOTOR{
+      if(ENDSTOPS_PULLUPS)
+        pinMode(endstops_max_pins[i], INPUT_PULLUP);
+      else
+        pinMode(endstops_max_pins[i], INPUT);
+    }
+    #endif
+    EACH_MOTOR
+    {
+        EEPROM.get(axis_max_eeprom+i*sizeof(long), axis_max[i]);
+    }
+    register_module(endstops_commands, endstops_loop);
+}
+
+//endstops? - get triggered endstops in (1,0,-1) format for max, none, min"));
+//      Serial.println(F("home_min <axes?>               - home given (00000zyx byte, e.g. 1 for x) or all axes to their min position"));
+//      Serial.println(F("home_max <axes?>               - home given (00000zyx byte, e.g. 3 for x and y) or all axes to their max position"));
+//      Serial.println(F("max_p?                         - return positions of max endstops"));
+//      Serial.println(F("max <d> <d> <d>
+extern const Command endstops_commands[] = {
+    {"endstops?", endstops_status},
+    {"home_min", endstops_home_min},
+    {"home_max", endstops_home_max},
+    {"max_p?", print_axes_max},
+    {"max", endstops_set_max},
+    END_COMMAND};
+#endif

src/modules/endstops/endstops.h

@@ -0,0 +1,32 @@
+#include "config.h"
+#ifdef ENDSTOPS
+#ifndef ENDSTOPS_H
+#include "config.h"
+#ifndef STAGE
+#error "Endstops module depends on the stage module"
+#endif
+
+#include "main.h"
+#include <Arduino.h>
+
+void endstops_setup();
+void endstops_loop();
+
+void endstops_home_min(String command);
+void endstops_home_max(String command);
+void endstops_max(String command);
+void endstops_status(String command);
+extern void stage_stop(String command);
+
+void home_min(uint8_t axes);
+void home_max(uint8_t axes);
+
+void home_start(uint8_t, int8_t);
+void home_retreat();
+void home_final();
+int8_t endstops_check();
+
+extern const Command endstops_commands[];
+#define ENDSTOPS_H
+#endif
+#endif

src/modules/help/help.cpp

@@ -1,3 +1,5 @@
+#include "config.h"
+#ifdef HELP
 #include "help.h"
 
 void help_setup()
@@ -68,4 +70,5 @@ void help(String command)
     Serial.println(F("<d>                            - a decimal integer."));
     Serial.println("");
     Serial.println("--END--");
-}
+}
+#endif

src/modules/help/help.h

@@ -1,3 +1,5 @@
+#include "config.h"
+#ifdef HELP
 #ifndef HELP_H
 #include <Arduino.h>
 #include "main.h"
@@ -9,4 +11,5 @@ const struct Command help_commands[] = {
     END_COMMAND
 };
 #define HELP_H
+#endif
 #endif

src/modules/light_sensor/light_sensor.cpp

@@ -0,0 +1,214 @@
+#include "config.h"
+#ifdef LIGHT_SENSOR
+#include "light_sensor.h"
+#include "config.h"
+#include "main.h"
+#ifdef LIGHT_SENSOR_ADAFRUIT_TSL2591
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_TSL2591.h>
+#endif
+#ifdef LIGHT_SENSOR_ADAFRUIT_ADS1115
+#include <Wire.h>
+#include <Adafruit_ADS1015.h>
+#endif
+
+const struct Command light_sensor_commands[] = {
+    {"light_sensor_gain", light_sensor_gain},
+    {"light_sensor_gain_values?", light_sensor_gain_values},
+    {"light_sensor_integration_time?", light_sensor_integration_time},
+    {"light_sensor_intensity", light_sensor_intensity},
+    END_COMMAND};
+
+void light_sensor_integration_time(String);
+
+void light_sensor_setup()
+{
+    setup_light_sensor_device();
+    register_module(light_sensor_commands, NULL);
+}
+
+void light_sensor_gain(String command)
+{
+    char *args[1];
+    parse_arguments(args, command, 1);
+    if (args[0][0] != '?')
+    {
+        int gain = atoi(args[0]);
+        free(args[0]);
+        return set_light_sensor_gain(gain);
+    }
+    free(args[0]);
+    return print_light_sensor_gain();
+}
+
+#ifdef LIGHT_SENSOR_ADAFRUIT_TSL2591
+Adafruit_TSL2591 tsl = Adafruit_TSL2591(2591); // pass in a number for the sensor identifier (for your use later)
+
+void setup_light_sensor_device()
+{
+    if (tsl.begin())
+    {
+        tsl.setGain(TSL2591_GAIN_MED);
+        tsl.setTiming(TSL2591_INTEGRATIONTIME_100MS); // shortest integration time (bright light)
+    }
+    else
+    {
+        Serial.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.print(F("light sensor gain "));
+    tsl2591Gain_t gain = tsl.getGain();
+    switch (gain)
+    {
+    case TSL2591_GAIN_LOW:
+        Serial.println(F("1x (Low)"));
+        break;
+    case TSL2591_GAIN_MED:
+        Serial.println(F("25x (Medium)"));
+        break;
+    case TSL2591_GAIN_HIGH:
+        Serial.println(F("428x (High)"));
+        break;
+    case TSL2591_GAIN_MAX:
+        Serial.println(F("9876x (Max)"));
+        break;
+    }
+}
+void light_sensor_gain_values(String command)
+{
+    // Print the allowable gain values of the light sensor
+    Serial.println(F("light sensor gains: 1x, 25x, 428x, 9876x"));
+}
+
+void set_light_sensor_gain(int newgain)
+{
+    // Set the current gain value of the light sensor, and print a confirmation.
+    switch (newgain)
+    {
+    case 1:
+        tsl.setGain(TSL2591_GAIN_LOW);
+        break;
+    case 25:
+        tsl.setGain(TSL2591_GAIN_MED);
+        break;
+    case 428:
+        tsl.setGain(TSL2591_GAIN_HIGH);
+        break;
+    case 9876:
+        tsl.setGain(TSL2591_GAIN_MAX);
+        break;
+    default:
+        Serial.println(F("Error: gain may only be 1, 25, 428, or 9876."));
+        return;
+    }
+    print_light_sensor_gain();
+}
+
+void light_sensor_integration_time(String command)
+{
+    // Print the current integration time in milliseconds.
+    Serial.print(F("light sensor integration time "));
+    Serial.print((tsl.getTiming() + 1) * 100, DEC);
+    Serial.println(F(" ms"));
+}
+
+void light_sensor_intensity(String command)
+{
+    // Print the current light value
+    uint16_t x = tsl.getLuminosity(TSL2591_FULLSPECTRUM);
+    Serial.println(x, DEC);
+}
+#endif // ADAFRUIT_TSL2591
+
+#ifdef LIGHT_SENSOR_ADAFRUIT_ADS1115
+Adafruit_ADS1115 ads; // pass in a number for the sensor identifier (for your use later)
+
+void setup_light_sensor_device()
+{
+    ads.begin();
+    ads.setGain(GAIN_ONE);
+}
+
+void print_light_sensor_gain()
+{
+    // Print the current gain value of the light sensor
+    Serial.print(F("light sensor gain "));
+    adsGain_t gain = ads.getGain() M;
+    switch (gain)
+    {
+    case GAIN_TWOTHIRDS:
+        Serial.println(F("0.66x (specify 0)"));
+        break;
+    case GAIN_ONE:
+        Serial.println(F("1x"));
+        break;
+    case GAIN_TWO:
+        Serial.println(F("2x"));
+        break;
+    case GAIN_FOUR:
+        Serial.println(F("4x"));
+        break;
+    case GAIN_EIGHT:
+        Serial.println(F("8x"));
+        break;
+    case GAIN_SIXTEEN:
+        Serial.println(F("16x"));
+        break;
+    }
+}
+void light_sensor_gain_values(String command)
+{
+    // Print the allowable gain values of the light sensor
+    Serial.println(F("light sensor gains: 0.66x (specify 0), 1x, 2x, 4x, 8x, 16x"));
+}
+
+void set_light_sensor_gain(int newgain)
+{
+    // Set the current gain value of the light sensor, and print a confirmation.
+    switch (newgain)
+    {
+    case 0:
+        ads.setGain(GAIN_TWOTHIRDS);
+        break;
+    case 1:
+        ads.setGain(GAIN_ONE);
+        break;
+    case 2:
+        ads.setGain(GAIN_TWO);
+        break;
+    case 4:
+        ads.setGain(GAIN_FOUR);
+        break;
+    case 8:
+        ads.setGain(GAIN_EIGHT);
+        break;
+    case 16:
+        ads.setGain(GAIN_SIXTEEN);
+        break;
+    default:
+        Serial.println(F("Error: gain may only be 0, 1, 2, 4, 8, 16 (0 means 2/3)."));
+        return;
+    }
+    print_light_sensor_gain();
+}
+
+void light_sensor_integration_time(String command)
+{
+    // Print the current integration time in milliseconds.
+    Serial.println(F("integration time not supported for ADS1115"));
+}
+
+void light_sensor_intensity(String command)
+{
+    // Print the current light value
+    // uint16_t x = ads.readADC_SingleEnded(0); //single ended measurement on pin 0
+    uint16_t x = ads.readADC_Differential_0_1(); //differential measurement on pins 0,1
+    Serial.println(x, DEC);
+}
+#endif // ADAFRUIT_ADS1115
+#endif

src/modules/light_sensor/light_sensor.h

@@ -0,0 +1,18 @@
+#include "config.h"
+#ifdef LIGHT_SENSOR
+#ifndef LIGHT_SENSOR_H
+#include <Arduino.h>
+
+void light_sensor_gain(String);
+void light_sensor_gain_values(String);
+void light_sensor_integration_time(String);
+void light_sensor_intensity(String);
+
+void light_sensor_setup();
+void setup_light_sensor_device();
+
+void print_light_sensor_gain();
+void set_light_sensor_gain(int);
+#define LIGHT_SENSOR_H
+#endif
+#endif

src/modules/stage/stage.cpp

@@ -2,11 +2,13 @@
 #include "config.h"
 #include <limits.h>
 #include <Arduino.h>
-#include <EEPROM.h>
+#ifdef SUPPORT_EEPROM
+    #include <EEPROM.h>
+#else
+    #include "dummyEEPROM.h"
+#endif
 #include "main.h"
 
-#define EACH_MOTOR for (int i = 0; i < n_motors; i++)
-
 // The array below has 3 stepper objects, for X,Y,Z respectively
 const int n_motors = 3;
 long min_step_delay;
@@ -18,22 +20,14 @@ Stepper *motors[n_motors];
 signed long current_pos[n_motors];
 long steps_remaining[n_motors];
 
-bool test_mode = false;
 bool stage_moving = false;
 
 void stage_setup()
 {
     // get the stepoper objects from the motor shield objects
-
-#if defined(SANGABOARDv2)
-    motors[0] = new Stepper(8, 13, 12, 11, 10);
-    motors[1] = new Stepper(8, 9, 8, 7, 6);
-    motors[2] = new Stepper(8, 5, 4, 3, 2);
-#elif defined(SANGABOARDv3)
-    motors[0] = new Stepper(8, 8, 9, 10, 11);
-    motors[1] = new Stepper(8, 5, 13, 4, 12);
-    motors[2] = new Stepper(8, 6, 7, A5, A4);
-#endif
+    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
     {
         motors[i]->setSpeed(10); // as a default set to 10 rpm, though this is ignored...
@@ -84,12 +78,12 @@ void print_position()
 
 unsigned long move_start_time = 0;
 unsigned long distance_moved[n_motors];
-unsigned long displacement[n_motors];
+long displacement[n_motors];
 float final_scaled_t;
 float step_delay[n_motors];
-long move_directions[n_motors];
+int8_t move_directions[n_motors];
 
-void start_move(unsigned long displ[n_motors])
+void start_move(long displ[n_motors])
 {
     // move all the axes in a nice move
     // split displacements into magnitude and direction, and find max. travel
@@ -103,7 +97,7 @@ void start_move(unsigned long displ[n_motors])
     }
     // scale the step delays so the move goes in a straight line, with >=1 motor
     // running at max. speed.
-    EACH_MOTOR if (displacement[i] > 0)
+    EACH_MOTOR {if (displacement[i] > 0)
     {
         step_delay[i] = float(max_steps) / float(displacement[i]) * float(min_step_delay);
     }
@@ -111,7 +105,7 @@ void start_move(unsigned long displ[n_motors])
     {
         step_delay[i] = 9999999999;
     }
-
+    }
     EACH_MOTOR distance_moved[i] = 0;
     move_start_time = micros();
     final_scaled_t = (float)max_steps * min_step_delay; //NB total time taken will be final_scaled_t + 2*ramp_time
@@ -157,7 +151,7 @@ void stage_loop()
     stage_moving = false;
     EACH_MOTOR
     {
-        if (distance_moved[i] < displacement[i])
+        if ((long) distance_moved[i] < displacement[i])
         {
             stage_moving = true; //only if all axes are done are we truly finished.
 
@@ -173,12 +167,13 @@ void stage_loop()
 
 void stage_move_single_axis(uint8_t axis, String command)
 {
-    String args[1];
+    char * args[1];
     parse_arguments(args, command, 1);
-    int move = args[0].toInt();
+    int move = atoi(args[0]);
     EACH_MOTOR displacement[i] = 0;
     displacement[axis] = move;
     start_move(displacement);
+    free(args[0]);
 }
 
 void stage_mrx(String command)
@@ -197,11 +192,12 @@ void stage_mrz(String command)
 
 void stage_mr(String command)
 {
-    String args[3];
+    char * args[3];
     parse_arguments(args, command, 3);
     EACH_MOTOR
     {
-        displacement[i] = args[i].toInt();
+        displacement[i] = atol(args[i]);
+        free(args[i]);
     }
 
     start_move(displacement);
@@ -222,37 +218,40 @@ void stage_p(String command)
 
 void stage_min_step_delay(String command)
 {
-    String args[1];
+    char * args[1];
     parse_arguments(args, command, 1);
-    if (args[0].equals("?"))
+    if (args[0][0] == '?')
     {
         Serial.print("minimum step delay ");
         Serial.println(min_step_delay);
     }
     else
     {
-        min_step_delay = args[0].toInt();
+        min_step_delay = atol(args[0]);
         EEPROM.put(min_step_delay_eeprom, min_step_delay);
         Serial.println("done.");
     }
+    free(args[0]);
 }
 
 void stage_ramp_time(String command)
 {
-    String args[1];
+    char * args[1];
     parse_arguments(args, command, 1);
-    if (args[0].equals("?"))
+    if (args[0][0] == '?')
     {
         Serial.print("ramp_time ");
         Serial.println(ramp_time);
     }
     else
     {
-        ramp_time = args[0].toInt();
+        ramp_time = atol(args[0]);
         EEPROM.put(ramp_time_eeprom, ramp_time);
         Serial.println("done.");
     }
+    free(args[0]);
 }
+
 void stage_zero(String command)
 {
     EACH_MOTOR current_pos[i] = 0;

src/modules/stage/stage.h

@@ -1,8 +1,9 @@
+#ifndef STAGE_H
 #include "StepperF_alt.h"
 #include "main.h"
 #include <Arduino.h>
 
-#define EACH_MOTOR for (int i = 0; i < n_motors; i++)
+#define EACH_MOTOR for (int i = 0; i < STAGE_N_MOTORS; i++)
 #define VER_STRING "Sangaboard Firmware v0.6"
 
 void stage_setup();
@@ -19,6 +20,17 @@ void stage_p(String command);
 void stage_min_step_delay(String command);
 void stage_ramp_time(String command);
 void stage_zero(String command);
+void start_move(long displ[]);
 void stage_stop(String command);
 
-extern const Command stage_commands[];
+extern const Command stage_commands[];
+//we want to expose these for endstops (and potentially other modules)
+extern signed long current_pos[];
+extern long displacement[];
+extern bool stage_moving;
+extern int8_t move_directions[];
+extern unsigned long distance_moved[];
+extern long min_step_delay;
+
+#define STAGE_H
+#endif

test/test_argument_parsing.cpp

@@ -0,0 +1,91 @@
+#include <Arduino.h>
+#include <unity.h>
+#include "main.h"
+
+void test_single_argument_parsing(void)
+{
+    char * expected_single = "3123.123asdf";
+
+    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);
+    free(args[0]);
+
+    char * args2[1];
+    all_args = F("3123.123asdf fail");
+    ret = parse_arguments(args2, all_args, 1);
+    TEST_ASSERT_EQUAL_STRING(expected_single, args2[0]);
+    TEST_ASSERT_EQUAL(1, ret);
+    free(args2[0]);
+
+    char * args3[1];
+    all_args = F(" 3123.123asdf fail");
+    ret = parse_arguments(args3, all_args, 1);
+    TEST_ASSERT_EQUAL_STRING(expected_single, args3[0]);
+    TEST_ASSERT_EQUAL(1, ret);
+    free(args3[0]);
+}
+
+void test_multiple_argument_parsing(void)
+{
+    char * expected[] = {"3123.123asdf", "second", "321./"};
+
+    String all_args = F("3123.123asdf second 321./");
+    char * args[3];
+    int i;
+    uint8_t ret;
+    ret = parse_arguments(args, all_args, 3);
+    TEST_ASSERT_EQUAL(3, ret);
+
+    for (i = 0; i < 3; i++)
+    {
+        TEST_ASSERT_EQUAL_STRING(expected[i], args[i]);
+        free(args[i]);
+    }
+
+    all_args = F("3123.123asdf second 321./ fail");
+    ret = parse_arguments(args, all_args, 3);
+    TEST_ASSERT_EQUAL(3, ret);
+    for (i = 0; i < 3; i++)
+    {
+        TEST_ASSERT_EQUAL_STRING(expected[i], args[i]);
+        free(args[i]);
+    }
+
+    all_args = F(" 3123.123asdf second");
+    ret = parse_arguments(args, all_args, 3);
+    TEST_ASSERT_EQUAL(2, ret);
+
+    for (i = 0; i < ret; i++)
+    {
+        TEST_ASSERT_EQUAL_STRING(expected[i], args[i]);
+        free(args[i]);
+    }
+}
+
+
+void setup() {
+
+    Serial.begin(115200);
+    // NOTE!!! Wait for >2 secs
+    // if board doesn't support software reset via Serial.DTR/RTS
+    delay(2000);
+    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);
+    RUN_TEST(test_single_argument_parsing);
+    RUN_TEST(test_multiple_argument_parsing);
+    RUN_TEST(test_single_argument_parsing);
+    RUN_TEST(test_single_argument_parsing);
+    RUN_TEST(test_multiple_argument_parsing);
+
+    UNITY_END();
+}
+
+void loop() {
+    ;
+}