hardware/config.scad

 
 left_support_len = 200;
 left_support_off = 50;
-right_support_len = 200;
-right_support_off = 50;
+right_support_len = left_support_len;
+right_support_off = left_support_off;
 
 echo("distance between bottom support rails", x_axis_rail_len - left_support_off - right_support_off - 40, -left_support_off, -right_support_off);
 
+bottom_support_len = 500;
+bottom_support_off = 80;
+
+use_double_supports = false;
+
 endstop_pcb_width = 16.0;
 endstop_pcb_height = 40.0;
 endstop_pcb_depth = 1.6;
 endstop_pcb_mount_off_w = endstop_mount_slot_length;
 
 use_endstop_mount_v2 = false;
+
+foot_screw_dia = 5.3;
+foot_screw_head_dia = 10.0;
+foot_screw_head_height = 7.0;
+foot_nut_base_dia = foot_screw_head_dia;
+foot_dia_add = 10.0;
+foot_dia = max(foot_screw_dia, foot_screw_head_dia) + foot_dia_add;
+
+carriage_spacer_height = 5;
+carriage_spacer_cut = 4;
+carriage_spacer_cut_x = y_carriage_x / 2 - carriage_spacer_cut;
+carriage_spacer_cut_y = y_carriage_y / 2 - carriage_spacer_cut;

hardware/table.scad

     }
 }
 
+module carriage_spacer() {
+    difference() {
+        cube([y_carriage_x, y_carriage_y, carriage_spacer_height]);
+        
+        translate([y_carriage_x / 2, y_carriage_y / 2, 0])
+        plate_holes(carriage_spacer_height);
+        
+        translate([carriage_spacer_cut_x / 2 * sqrt(2) + y_carriage_x / 2, -1, -1])
+        rotate([0, -90, 45])
+        prism(carriage_spacer_height + 2, carriage_spacer_cut_x, carriage_spacer_cut_x);
+        
+        translate([-carriage_spacer_cut_x / 2 * sqrt(2) + y_carriage_x / 2, y_carriage_y + 1, -1])
+        rotate([0, -90, 225])
+        prism(carriage_spacer_height + 2, carriage_spacer_cut_x, carriage_spacer_cut_x);
+        
+        translate([-1, y_carriage_y / 2 - carriage_spacer_cut_y / 2 * sqrt(2), -1])
+        rotate([0, -90, -45])
+        prism(carriage_spacer_height + 2, carriage_spacer_cut_y, carriage_spacer_cut_y);
+        
+        translate([y_carriage_x + 1, y_carriage_y / 2 + carriage_spacer_cut_y / 2 * sqrt(2), -1])
+        rotate([0, -90, -225])
+        prism(carriage_spacer_height + 2, carriage_spacer_cut_y, carriage_spacer_cut_y);
+    }
+}
+
 // also used as base for x-carriage
 // axis = 0 --> all holes, usable for both
 // axis = 1 --> holes for x-carriage
 
 module y_axis() {
     translate([y_carriage_x / 2, y_axis_animation_position, 20 + y_carriage_h + y_carriage_rail_dist])
-    rotate([0, 180, 0])
-    y_carriage(2);
+    rotate([0, 180, 0]) {
+        y_carriage(2);
+        
+        color("cyan")
+        translate([0, 0, -carriage_spacer_height - 0.01])
+        carriage_spacer();
+    }
     
     color("grey")
     translate([-10, 0, 0])
     translate([0, -y_carriage_y / 2 - y_axis_animation_position, 0])
     y_axis();
     
-    %translate([-plate_x / 2, -plate_y / 2, 20 + y_carriage_h + y_carriage_rail_dist])
+    %translate([-plate_x / 2, -plate_y / 2, 20 + y_carriage_h + y_carriage_rail_dist + carriage_spacer_height])
     plate();
 }
 
     assembly_y_axis_plate();
 }
 
-bottom_support_len = 500;
-bottom_support_off = 80;
-
-use_double_supports = false;
-
 module rail_foot() {
-    
+    difference() {
+        union() {
+            sphere(d = foot_dia);
+            
+            translate([0, 0, foot_dia / 2])
+            sphere(d = foot_nut_base_dia);
+        }
+        
+        translate([-foot_dia / 2 - 1, -foot_dia / 2 - 1, -foot_dia / 2 - 1])
+        cube([foot_dia + 2, foot_dia + 2, foot_dia / 2 + 1]);
+        
+        translate([-foot_nut_base_dia / 2 - 1, -foot_nut_base_dia / 2 - 1, foot_dia / 2])
+        cube([foot_nut_base_dia + 2, foot_nut_base_dia + 2, foot_nut_base_dia / 2 + 1]);
+        
+        translate([0, 0, -1])
+        cylinder(d = foot_screw_dia, h = foot_dia / 2 + 2);
+        
+        translate([0, 0, -1])
+        cylinder(d = foot_screw_head_dia, h = foot_screw_head_height + 1);
+    }
 }
 
 module assembly() {
             translate([x_axis_rail_len - 20 - right_support_off, -right_support_len - 10, -20])
             rail_2020_y(right_support_len, "right support");
             
-            // TODO feet
+            for (i = [0, 1])
+            for (j = [1, -1])
+            translate([i * x_axis_rail_len - (i * 2 - 1) * (right_support_off + 10), j * (right_support_len - 10), -40 - 0]) {
+                %color("grey")
+                cylinder(d = 5.0, h = 25);
+                
+                %color("green")
+                rail_foot();
+            }
         } else {
             color("grey")
             translate([x_axis_rail_len / 2, 0, -40])
             translate([x_axis_rail_len / 2 - bottom_support_off, -bottom_support_len / 2, 0])
             rail_2020_y(bottom_support_len, "bottom supports");
             
-            // TODO feet
+            for (i = [0, 1])
+            for (j = [1, -1])
+            translate([i * x_axis_rail_len - (i * 2 - 1) * (bottom_support_off - 10), j * (bottom_support_len / 2 - 40), -40 - 20]) {
+                %color("grey")
+                cylinder(d = 5.0, h = 25);
+                
+                %color("green")
+                rail_foot();
+            }
         }
     }
 }
 
 module xy_table() {
-    translate([-point_that_reaches_everywhere_x, -point_that_reaches_everywhere_y, -40 + -40 + (y_carriage_rail_dist + y_carriage_h) * -2 + -plate_z])
+    translate([-point_that_reaches_everywhere_x, -point_that_reaches_everywhere_y, -40 + -40 + (y_carriage_rail_dist + y_carriage_h) * -2 + -plate_z - carriage_spacer_height])
     assembly();
 }
 
 //dispenser();
 //rail_2020_x(100);
 //rail_wheel();
+//rail_foot();
 
 //motor_mount(1);
 //motor_mount(2);
 //y_carriage(1);
 //y_carriage(2);
 //x_carriage();
+//carriage_spacer();
 
 //y_axis();
 //x_axis();

include/common.h

 
 #define CLEAR_STORE_INTERRUPTS() uint8_t prev_int_reg = SREG; cli()
 #define RESTORE_INTERRUPTS() SREG = prev_int_reg
+//#define CLEAR_STORE_INTERRUPTS() cli()
+//#define RESTORE_INTERRUPTS() sei()
 
 void async_beep(int time, int freq);
 void blocking_beep(int time, int freq, int repeat = 0);
-void common_run(unsigned long t);
+void common_run(void);
 
 #endif // _COMMON_H_

include/config.h

  ********** Hardware Settings **********
  ***************************************/
 
+#define AXIS_COUNT 4
+#define X_AXIS 0
+#define Y_AXIS 1
+#define Z_AXIS 2
+#define E_AXIS 3
+
 // xy steps per mm
 #define XY_BELT_PITCH 2.0
 #define XY_PULLEY_TEETH 40.0
 #define E_MM_TO_PERCENT(x) ((acos((x * 2.0 / E_AXIS_MAX) - 1.0) - PI) * 100.0 / PI)
 
 // maximum speeds
-#define XY_MAX_SPEED 50.0 // in mm/s
-#define Z_MAX_SPEED 20.0 // in mm/s
-#define E_MAX_SPEED 50.0 // in percent/s
+#define XY_MAX_SPEED 50.0 // in mm/s --> 50mm/s = 2000steps/s
+#define Z_MAX_SPEED 1.2 // in mm/s --> 20mm/s = 32000steps/s
+#define E_MAX_SPEED 30.0 // in percent/s --> 50%/s = 2400steps/s
 
 // homing speeds
 #define XY_FAST_HOME_SPEED 25.0 // in mm/s
 #define E_HOME_BACK_OFF_TIME (E_BACK_OFF_DISTANCE / E_FAST_HOME_SPEED * 1000)
 
 // epsilon around expected endstop position where we dont abort on hits
-#define X_AXIS_EPSILON 20.0 // TODO
-#define Y_AXIS_EPSILON 20.0 // TODO
+// if you need to increase this value, try to reduce play in your axis
+#define X_AXIS_EPSILON 1.0
+#define Y_AXIS_EPSILON 1.0
 #define Z_AXIS_EPSILON 1.0
 #define E_AXIS_EPSILON 1.0
 
-#define ENCODER_RPM_VALUE_FACTOR 10.0
+#define ENCODER_RPM_VALUE_FACTOR 2.0
+#define IDLE_TIMEOUT (5UL * 60UL * 1000UL) // ms, 0 to disable
+
+// TimerOne freq for steppers
+#define XY_MAX_HZ (XY_MAX_SPEED * XY_STEPS_PER_MM)
+#define Z_MAX_HZ (Z_MAX_SPEED * Z_STEPS_PER_MM)
+#define E_MAX_HZ (E_MAX_SPEED * E_STEPS_PER_PERCENT)
+#define MAX_SAMPLE_FREQ 2500
 
 #endif // _CONFIG_H_

include/config_pins.h

 #endif
 
 #if (X_MIN_PIN == -1)
-#define X_HOMING_DIR 1.0
+#define X_HOMING_DIR 1
 #define X_ENDSTOP_PIN X_MAX_PIN
 #else
-#define X_HOMING_DIR -1.0
+#define X_HOMING_DIR -1
 #define X_ENDSTOP_PIN X_MIN_PIN
 #endif
 
 #if (Y_MIN_PIN == -1)
-#define Y_HOMING_DIR 1.0
+#define Y_HOMING_DIR 1
 #define Y_ENDSTOP_PIN Y_MAX_PIN
 #else
-#define Y_HOMING_DIR -1.0
+#define Y_HOMING_DIR -1
 #define Y_ENDSTOP_PIN Y_MIN_PIN
 #endif
 
 #if (Z_MIN_PIN == -1)
-#define Z_HOMING_DIR 1.0
+#define Z_HOMING_DIR 1
 #define Z_ENDSTOP_PIN Z_MAX_PIN
 #else
-#define Z_HOMING_DIR -1.0
+#define Z_HOMING_DIR -1
 #define Z_ENDSTOP_PIN Z_MIN_PIN
 #endif
 
 #if (E_MIN_PIN == -1)
-#define E_HOMING_DIR 1.0
+#define E_HOMING_DIR 1
 #define E_ENDSTOP_PIN E_MAX_PIN
 #else
-#define E_HOMING_DIR -1.0
+#define E_HOMING_DIR -1
 #define E_ENDSTOP_PIN E_MIN_PIN
 #endif
 

include/data.h

 #ifndef _DATA_H_
 #define _DATA_H_
 
-#define DATA_SCHEMA_VERSION 0
+#define DATA_SCHEMA_VERSION 1
 
 struct data_config_options {
+    // in mm/s (%/s for e)
     float speed_x, speed_y, speed_z, speed_e;
+
+    // in mm/s^2 (%/s^2 for e)
     float accel_x, accel_y, accel_z, accel_e;
 };
 
 struct data_config_preset {
+    // number of containers
     uint8_t count_x, count_y;
-    float distance_x, distance_y;
-    float offset_x, offset_y;
-    float top_z, bottom_z;
-    float extrusion;
+
+    // distance between containers
+    float distance_x, distance_y; // in mm
+
+    // offset of first container
+    float offset_x, offset_y; // in mm
+
+    // height for moves between extrusions
+    float move_z; // in mm
+
+    // height while extruding
+    float bottom_z, top_z; // in mm
+
+    float extrusion_length; // in mm
+    float extrusion_bottom_wait; // in s
+    float extrusion_top_wait; // in s
 };
 
 void data_init(void);
+void data_clear(void);
 void data_eeprom_write(void);
 
 bool data_eeprom_read(void);

include/encoder.h

 #define _ENCODER_H_
 
 void encoder_init(void);
+
 void encoder_run(void);
 
+bool encoder_click(void);
+bool kill_switch(void);
+
+// --------------------------------------
+
+void encoder_run_fast(void);
+
+// disable interrupts while reading these
 int encoder_change(void);
 int encoder_rpm(void);
-int encoder_click(void);
-int kill_switch(void);
 
 #endif // _ENCODER_H_

include/statemachine.h

 
 #define STATE_ARRAY_SIZE 42
 
-template <typename T, size_t N>
-void array_print(Array<T, N> *arr);
-
-template <typename T, size_t N>
-void array_insert_at_pos(Array<T, N> *arr, T value, size_t pos);
-
 struct StateMachineInput {
-    StateMachineInput(int c, int e, int r, int k, int m)
-            : click(c), encoder(e), rpm(r), kill(k), motors_done(m) { };
-
-    int click;
+    StateMachineInput(bool c, int e, int r, bool k, bool *m, bool am)
+            : click(c), encoder(e), rpm(r), kill(k), all_motors_done(am) {
+        for (int i = 0; i < AXIS_COUNT; i++) {
+            motor_done[i] = m[i];
+        }
+    };
+
+    bool click;
     int encoder;
     int rpm;
-    int kill;
-    int motors_done;
+    bool kill;
+    bool motor_done[AXIS_COUNT];
+    bool all_motors_done;
 };
 
 class State {
 public:
+    typedef void(*EnterFuncPtr)(void);
+    typedef void(*InFuncPtr)(StateMachineInput smi);
+
     State(State *_parent = NULL);
     State *getParent(void) { return parent; }
 
     void setTitle(const char *_title) { title = _title; }
     const char *getTitle(void) { return title; }
 
-    virtual void enterState(void) = 0;
-    virtual void inState(StateMachineInput smi) = 0;
-
-private:
-    State *parent;
-    State *child;
-    const char *title;
-};
-
-class StateText : public State {
-public:
-    StateText(State *_parent = NULL);
-
-    void setHeading(const char *_heading);
-    void setText(const char *_text);
+    void setHeading(const char *_heading) { heading = _heading; }
+    const char *getHeading(void) { return heading; }
 
-    typedef void(*EnterFuncPtr)(void);
-    typedef void(*InFuncPtr)(StateMachineInput smi);
+    void setText(const char *_text) { text = _text; }
+    const char *getText(void) { return text; }
 
-    void onEnter(EnterFuncPtr func);
-    void whenIn(InFuncPtr func);
+    void onEnter(EnterFuncPtr func) { onEnterFunc = func; }
+    void whenIn(InFuncPtr func) { whenInFunc = func; }
 
     void updateText(void);
 
     virtual void enterState(void);
     virtual void inState(StateMachineInput smi);
 
-private:
+protected:
+    State *parent;
+    State *child;
+    const char *title;
     const char *heading;
     const char *text;
     EnterFuncPtr onEnterFunc;
 public:
     StateValue(State *_parent, T &_value, T _min, T _max);
 
-    void setHeading(const char *_heading);
-    void setText(const char *_text);
+    void setMax(T _max);
 
-    typedef void(*EnterFuncPtr)(void);
     typedef void(*UpdateFuncPtr)(T value);
 
-    void onEnter(EnterFuncPtr func);
     void onLiveUpdate(UpdateFuncPtr func);
     void onUpdate(UpdateFuncPtr func);
 
 
     T &value;
     T min, max;
-    const char *heading;
-    const char *text;
-    EnterFuncPtr onEnterFunc;
     UpdateFuncPtr updateFunc, updateLiveFunc;
 };
 
 
     void setData(size_t index, const char *name, T *value, T min, T max);
 
-    void setHeading(const char *_heading);
-    void setText(const char *_text);
-
-    typedef void(*EnterFuncPtr)(void);
     typedef void(*UpdateFuncPtr)(size_t index, T value);
 
-    void onEnter(EnterFuncPtr func);
     void onLiveUpdate(UpdateFuncPtr func);
     void onUpdate(UpdateFuncPtr func);
 
     const char *heading;
     size_t pos;
     bool editing;
-    EnterFuncPtr onEnterFunc;
     UpdateFuncPtr updateFunc, updateLiveFunc;
 };
 

include/states.h

 State *states_get(void);
 void states_go_to(State *state);
 
-extern StateText sm_error;
+extern State sm_error;
 
 #endif // _STATES_H_

include/steppers.h

 #define _STEPPERS_H_
 
 void steppers_init(void);
-bool steppers_run(void);
+void steppers_run(void);
 
 bool steppers_homed(void);
 char steppers_homing_axis(void);
 void steppers_start_homing(void);
 
+bool stepper_reached_target(int axis);
 float steppers_current_pos(int axis);
 
 // target pos

src/common.cpp

 #include "config.h"
 #include "config_pins.h"
 
+//#define DISABLE_BEEPING
+
 unsigned long last_led_blink_time = 0;
+
+#ifndef DISABLE_BEEPING
 unsigned long beep_stop_time = 0;
+#endif // DISABLE_BEEPING
 
 void async_beep(int time, int freq) {
+#ifndef DISABLE_BEEPING
     beep_stop_time = millis() + time;
     tone(BEEPER, freq);
+#endif // DISABLE_BEEPING
 }
 
 void blocking_beep(int time, int freq, int repeat = 0) {
+#ifndef DISABLE_BEEPING
     for (int i = 0; i <= repeat; i++) {
         tone(BEEPER, freq);
         delay(time);
             delay(time);
         }
     }
+#endif // DISABLE_BEEPING
 }
 
-void common_run(unsigned long t) {
+void common_run(void) {
+    unsigned long t = millis();
+
+#ifndef DISABLE_BEEPING
     // stop async beep
     if ((beep_stop_time > 0)  && (t > beep_stop_time)) {
         noTone(BEEPER);
         beep_stop_time = 0;
     }
+#endif // DISABLE_BEEPING
 
     // blink heartbeat LED
     if ((t - last_led_blink_time) >= LED_BLINK_INTERVAL) {
-        last_led_blink_time = millis();
+        last_led_blink_time = t;
         digitalWrite(LED_PIN, !digitalRead(LED_PIN));
     }
 }

src/data.cpp

 }
 
 static uint32_t data_checksum(struct data_config *data) {
+    uint32_t temp_checksum = data->checksum;
+    data->checksum = 0;
+
+    struct data_config_preset *temp_presets = data->presets;
+    data->presets = NULL;
+
     uint32_t c = 0;
 
+
     uint8_t *t = (uint8_t *)data;
     for (unsigned int i = 0; i < sizeof(struct data_config); i++) {
         c ^= t[i];
     }
 
     for (unsigned int i = 0; i < data->preset_count; i++) {
-        t = (uint8_t *)(&data->presets[i]);
+        t = (uint8_t *)(temp_presets + i);
         for (unsigned int j = 0; j < sizeof(struct data_config_preset); j++) {
             c ^= t[j];
         }
     }
 
+    data->checksum = temp_checksum;
+    data->presets = temp_presets;
+
     return c;
 }
 
 
     // verify checksum
     uint32_t read_checksum = config.checksum;
-    config.checksum = 0;
     uint32_t checksum = data_checksum(&config);
     if (read_checksum == checksum) {
         // verify version
         }
     } else {
         Serial.print(F("checksum read="));
-        Serial.print(config.checksum);
+        Serial.print(read_checksum);
         Serial.print(F(" calc="));
         Serial.println(checksum);
         last_error = "Checksum";
 }
 
 void data_eeprom_write(void) {
-    d.checksum = 0;
     d.checksum = data_checksum(&d);
 
     // write meta-data and settings
 }
 
 void data_init(void) {
+    d.presets = NULL;
+    data_clear();
+
+    Serial.print(F("EEPROM max presets: "));
+    Serial.println(max_presets());
+
+    Serial.print(F("EEPROM read... "));
+    if (!data_eeprom_read()) {
+        Serial.print(last_error);
+        Serial.println(F(" Error"));
+    } else {
+        Serial.println(F("Ok"));
+    }
+}
+
+void data_clear(void) {
+    if (d.presets != NULL) {
+        free(d.presets);
+        d.presets = NULL;
+    }
+
     d.data_schema_version = DATA_SCHEMA_VERSION;
     d.preset_count = 0;
     d.checksum = 0;
     d.options.accel_y = XY_MAX_ACCEL;
     d.options.accel_z = Z_MAX_ACCEL;
     d.options.accel_e = E_MAX_ACCEL;
-
-    d.presets = NULL;
-
-    Serial.print(F("EEPROM max presets: "));
-    Serial.println(max_presets());
-
-    Serial.print(F("EEPROM read... "));
-    if (!data_eeprom_read()) {
-        Serial.print(last_error);
-        Serial.println(F(" Error"));
-    } else {
-        Serial.println(F("Ok"));
-    }
 }
 
 struct data_config_options *data_options(void) {

src/encoder.cpp

 #ifdef KILL_PIN
     kill_state = kill.poll();
 #endif // KILL_PIN
-
-    encoder.tick();
-}
-
-int encoder_change(void) {
-    int new_pos = encoder.getPosition();
-    int diff = new_pos - pos;
-    pos = new_pos;
-
-#ifdef DEBUG_ENCODER
-    if (diff != 0) {
-        Serial.print(F("encoder_change: "));
-        Serial.print(diff);
-        Serial.print(F(" ticks: "));
-        Serial.println(new_pos);
-    }
-#endif // DEBUG_ENCODER
-
-    return diff;
-}
-
-int encoder_rpm(void) {
-    return encoder.getRPM();
 }
 
-int encoder_click(void) {
+bool encoder_click(void) {
     int r = click_state;
 
     // only return 1 once for each click
 #endif // DEBUG_ENCODER
     }
 
-    return r;
+    return r ? true : false;
 }
 
-int kill_switch(void) {
+bool kill_switch(void) {
 #ifdef KILL_PIN
 #ifdef DEBUG_ENCODER
     if (kill_state == 1) {
     }
 #endif // DEBUG_ENCODER
 
-    return kill_state;
+    return kill_state ? true : false;
 #else
-    return 0;
+    return false;
 #endif // KILL_PIN
 }
+
+// --------------------------------------
+
+void encoder_run_fast(void) {
+    encoder.tick();
+}
+
+int encoder_change(void) {
+    int new_pos = encoder.getPosition();
+    int diff = new_pos - pos;
+    pos = new_pos;
+
+#ifdef DEBUG_ENCODER
+    if (diff != 0) {
+        Serial.print(F("encoder_change: "));
+        Serial.print(diff);
+        Serial.print(F(" ticks: "));
+        Serial.println(new_pos);
+    }
+#endif // DEBUG_ENCODER
+
+    return diff;
+}
+
+int encoder_rpm(void) {
+    return encoder.getRPM();
+}

src/main.cpp

 #include "steppers.h"
 #include "states.h"
 
-static volatile bool still_running = false;
+const static float hz = min(max(max(XY_MAX_HZ, Z_MAX_HZ), E_MAX_HZ), MAX_SAMPLE_FREQ);
+const static float us = (1000.0 * 1000.0) / hz;
 
 void print_data(void) {
     Serial.println(F("XY:"));
     Serial.println(F(" percent/s^2"));
 
     Serial.println();
+
+    // ----------------------------------
+
+    Serial.print(F("XY max move freq: "));
+    Serial.print(XY_MAX_HZ);
+    Serial.println(F("hz"));
+    Serial.print(F("Z max move freq: "));
+    Serial.print(Z_MAX_HZ);
+    Serial.println(F("hz"));
+    Serial.print(F("E max move freq: "));
+    Serial.print(E_MAX_HZ);
+    Serial.println(F("hz"));
+
+    Serial.print(F("Sample Freq: "));
+    Serial.print(hz);
+    Serial.print(F("Hz = "));
+    Serial.print(us);
+    Serial.println(F("us"));
 }
 
 void fast_loop(void) {
-    still_running = steppers_run();
-    encoder_run();
+    steppers_run();
+    encoder_run_fast();
 }
 
 void setup() {
     pinMode(BEEPER, OUTPUT);
     blocking_beep(100, 1000);
 
-    // turn on fan connected to D8 for electronics
-    pinMode(FAN_PIN, OUTPUT);
-    digitalWrite(FAN_PIN, HIGH);
-
     Serial.begin(115200);
     Serial.println(F("Initializing Fuellfix v2"));
     Serial.print(F("Version: "));
     Serial.println(F("Init stepper motors"));
     steppers_init();
 
-    print_data();
+    //print_data();
 
     Serial.println(F("ready, showing splash screen"));
     digitalWrite(LED_PIN, LOW);
     blocking_beep(100, 2000);
 
     // wait some time to show splash screen
-    delay(2000);
+    while (millis() < 1500) { }
 
     Serial.println(F("Init state machine"));
     states_init();
     blocking_beep(100, 2000);
     Serial.println(F("starting main loop"));
 
-    Timer1.initialize(1000); // 1000us -> 1kHz
+    Timer1.initialize(us);
     Timer1.attachInterrupt(fast_loop);
 }
 
 void loop() {
-    common_run(millis());
+    common_run();
+    encoder_run();
+
+    bool click = encoder_click();
+    bool kill = kill_switch();
 
     CLEAR_STORE_INTERRUPTS();
 
-    int click = encoder_click();
     int change = encoder_change();
     int rpm = encoder_rpm();
-    int kill = kill_switch();
-    bool motors_done = !still_running;
+
+    bool motor_done[AXIS_COUNT];
+    bool all_done = true;
+    for (int i = 0; i < AXIS_COUNT; i++) {
+        motor_done[i] = stepper_reached_target(i);
+        if (!motor_done[i]) {
+            all_done = false;
+        }
+    }
 
     RESTORE_INTERRUPTS();
 
-    struct StateMachineInput smi(click, change, rpm, kill, motors_done);
+    // allow rudimentary control over serial
+    // with enter and w/s or arrow up/down for encoder
+    // and q/k/space keys for kill
+    static int ser_state = 0;
+    while (Serial.available()) {
+        int c = Serial.read();
+        if (ser_state == 0) {
+            if (c == '\n') {
+                click = 1;
+            } else if ((c == 'q') || (c == 'Q') || (c == 'k') || (c == 'K') || (c == ' ')) {
+                kill = 1;
+            } else if ((c == 'w') || (c == 'W')) {
+                change = 1;
+            } else if ((c == 's') || (c == 'S')) {
+                change = -1;
+            } else if (c == '\e') {
+                ser_state = 1;
+            }
+        } else if (ser_state == 1) {
+            if (c == '[') {
+                ser_state = 2;
+            } else {
+                ser_state = 0;
+            }
+        } else if (ser_state == 2) {
+            if (c == 'A') {
+                change = 1;
+            } else if (c == 'B') {
+                change = -1;
+            }
+            ser_state = 0;
+        }
+    }
+
+    struct StateMachineInput smi(click, change, rpm, kill, motor_done, all_done);
     states_run(smi);
 
     lcd_loop();
-
-    delay(10);
 }

src/sm_menu.cpp

+#include <Arduino.h>
+
+#include "config.h"
+#include "config_pins.h"
+#include "lcd.h"
+#include "states.h"
+
+template <typename T, size_t N>
+void array_print(Array<T, N> *arr) {
+    Serial.print(F("Array length: "));
+    Serial.print(arr->size());
+    Serial.println(F(" contents:"));
+    for (int i = 0; i < arr->size(); i++) {
+        Serial.print(i);
+        Serial.print(F(": "));
+        Serial.println(arr->at(i)->getTitle());
+    }
+}
+
+template <typename T, size_t N>
+void array_insert_at_pos(Array<T, N> *arr, T value, size_t pos) {
+    // make additional space
+    arr->push_back(value);
+
+    if ((pos >= arr->max_size()) || (pos >= arr->size())) {
+        // we can not shift it to the given position
+        return;
+    }
+
+    for (int i = arr->size() - 2; i >= pos; i--) {
+        arr->at(i + 1) = arr->at(i);
+    }
+
+    arr->at(pos) = value;
+}
+
+// --------------------------------------
+
+StateMenu::StateMenu(State *_parent, bool _show_parent) : State(_parent) {
+    show_parent = _show_parent;
+    menuPos = 0;
+    menuOff = 0;
+}
+
+void StateMenu::setChild(State *_child) {
+    children.push_back(_child);
+}
+
+void StateMenu::addChild(State *_child, int pos) {
+    if (pos < 0) {
+        setChild(_child);
+    } else {
+        array_insert_at_pos(&children, _child, pos);
+    }
+}
+
+void StateMenu::enterState(void) {
+    display();
+}
+
+void StateMenu::display(void) {
+    lcd_clear();
+    lcd_set_heading(getTitle());
+
+    int size = children.size();
+    if (show_parent) {
+        size++;
+    }
+
+    for (int i = menuOff; (i < menuOff + lcd_text_lines()) && (i < size); i++) {
+        String s;
+        if (i == menuPos) {
+            s = F("> ");
+        } else {
+            s = F("  ");
+        }
+        if (i == children.size()) {
+            s += getParent()->getTitle();
+        } else {
+            s += children.at(i)->getTitle();
+        }
+        lcd_set_menu_text(i - menuOff, s.c_str());
+    }
+}
+
+void StateMenu::inState(struct StateMachineInput smi) {
+    int size = children.size();
+    if (show_parent) {
+        size++;
+    }
+
+    if (smi.encoder != 0) {
+        menuPos -= smi.encoder;
+
+        while (menuPos < 0) {
+            menuPos += size;
+        }
+
+        while (menuPos >= size) {
+            menuPos -= size;
+        }
+
+        while (menuPos < menuOff) {
+            menuOff--;
+        }
+
+        while (menuPos >= (menuOff + lcd_text_lines())) {
+            menuOff++;
+        }
+
+        display();
+    }
+
+    if (smi.click) {
+        if (menuPos == children.size()) {
+            menuPos = 0;
+            menuOff = 0;
+            states_go_to(getParent());
+        } else {
+            states_go_to(children.at(menuPos));
+        }
+    }
+}
+
+// --------------------------------------
+
+StateDynamicMenu::StateDynamicMenu(State *_parent) : State(_parent) {
+    menuPos = 0;
+    menuOff = 0;
+    prefix = "";
+    countFunc = NULL;
+    getFunc = NULL;
+    callFunc = NULL;
+}
+
+void StateDynamicMenu::dataCount(CountFuncPtr count) {
+    countFunc = count;
+}
+
+void StateDynamicMenu::dataGet(GetFuncPtr get) {
+    getFunc = get;
+}
+
+void StateDynamicMenu::dataCall(CallFuncPtr call) {
+    callFunc = call;
+}
+
+void StateDynamicMenu::display(void) {
+    lcd_clear();
+    lcd_set_heading(getTitle());
+
+    for (int i = menuOff; (i < menuOff + lcd_text_lines()) && (i < count + 1); i++) {
+        String s;
+        if (i == menuPos) {
+            s = F("> ");
+        } else {
+            s = F("  ");
+        }
+        if (i == count) {
+            s += getParent()->getTitle();
+        } else {
+            s += contents.at(i);
+        }
+        lcd_set_menu_text(i - menuOff, s.c_str());
+    }
+}
+
+void StateDynamicMenu::setPrefix(String pre) {
+    prefix = pre;
+}
+
+void StateDynamicMenu::enterState(void) {
+    // cache all entries on entering state
+    if (countFunc != NULL) {
+        count = countFunc();
+    } else {
+        count = 0;
+    }
+    contents.clear();
+    for (int i = 0; i < count; i++) {
+        if (getFunc != NULL) {
+            contents.push_back(prefix + String(getFunc(i)));
+        } else {
+            contents.push_back(prefix + String(i + 1));
+        }
+    }
+
+    display();
+}
+
+void StateDynamicMenu::inState(StateMachineInput smi) {
+    if (smi.encoder != 0) {
+        menuPos -= smi.encoder;
+
+        while (menuPos < 0) {
+            menuPos += count + 1;
+        }
+
+        while (menuPos >= count + 1) {
+            menuPos -= count + 1;
+        }
+
+        while (menuPos < menuOff) {
+            menuOff--;
+        }
+
+        while (menuPos >= (menuOff + lcd_text_lines())) {
+            menuOff++;
+        }
+
+        display();
+    }
+
+    if (smi.click) {
+        if (menuPos == count) {
+            menuPos = 0;
+            menuOff = 0;
+            states_go_to(getParent());
+        } else {
+            if (callFunc != NULL) {
+                callFunc(menuPos);
+            }
+        }
+    }
+}

src/sm_value.cpp

+#include <Arduino.h>
+
+#include "config.h"
+#include "config_pins.h"
+#include "lcd.h"
+#include "states.h"
+
+template <typename T>
+StateValue<T>::StateValue(State *_parent, T &_value, T _min, T _max) : State(_parent), value(_value) {
+    min = _min;
+    max = _max;
+    heading = NULL;
+    text = NULL;
+    onEnterFunc = NULL;
+    updateFunc = NULL;
+    updateLiveFunc = NULL;
+}
+
+template <typename T>
+void StateValue<T>::setMax(T _max) {
+    max = _max;
+}
+
+template <typename T>
+void StateValue<T>::onUpdate(UpdateFuncPtr func) {
+    updateFunc = func;
+}
+
+template <typename T>
+void StateValue<T>::onLiveUpdate(UpdateFuncPtr func) {
+    updateLiveFunc = func;
+}
+
+template <typename T>
+void StateValue<T>::display(void) {
+    lcd_clear();
+    if (heading == NULL) {
+        lcd_set_heading(getTitle());
+    } else {
+        lcd_set_heading(heading);
+    }
+
+    String s = String(min) + F(" .. ") + String(value) + F(" .. ") + String(max);
+
+    if (text != NULL) {
+        s = text + String(F("\n")) + s;
+    }
+
+    lcd_set_text(s.c_str());
+}
+
+
+template <typename T>
+void StateValue<T>::enterState(void) {
+    if (onEnterFunc != NULL) {
+        onEnterFunc();
+    }
+    display();
+}
+
+template <typename T>
+void StateValue<T>::inState(StateMachineInput smi) {
+    if (smi.encoder != 0) {
+        float vf = smi.encoder;
+        vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
+        int v = vf;
+        value -= v;
+        if (value < min) {
+            value = min;
+        }
+        if (value > max) {
+            value = max;
+        }
+        if (updateLiveFunc != NULL) {
+            updateLiveFunc(value);
+        }
+        display();
+    }
+    if (smi.click) {
+        if (updateFunc != NULL) {
+            updateFunc(value);
+        }
+        if (getChild() != NULL) {
+            states_go_to(getChild());
+        } else if (getParent() != NULL) {
+            states_go_to(getParent());
+        }
+    }
+}
+
+template class StateValue<int>;
+template class StateValue<float>;
+
+// --------------------------------------
+
+template <typename T, size_t N>
+StateValues<T, N>::StateValues(State *_parent) : State(_parent) {
+    heading = NULL;
+    onEnterFunc = NULL;
+    updateFunc = NULL;
+    updateLiveFunc = NULL;
+    pos = 0;
+    editing = false;
+}
+
+template <typename T, size_t N>
+void StateValues<T, N>::setData(size_t index, const char *name, T *value, T min, T max) {
+    if (index >= N) {
+        return;
+    }
+    values[index] = value;
+    mins[index] = min;
+    maxs[index] = max;
+    texts[index] = name;
+}
+
+template <typename T, size_t N>
+void StateValues<T, N>::onUpdate(UpdateFuncPtr func) {
+    updateFunc = func;
+}
+
+template <typename T, size_t N>
+void StateValues<T, N>::onLiveUpdate(UpdateFuncPtr func) {
+    updateLiveFunc = func;
+}
+
+template <typename T, size_t N>
+void StateValues<T, N>::display(void) {
+    lcd_clear();
+
+    if (heading == NULL) {
+        lcd_set_heading(getTitle());
+    } else {
+        lcd_set_heading(heading);
+    }
+
+    for (size_t i = 0; i < (N + 1); i++) {
+        String s;
+
+        if (i == pos) {
+            if (editing) {
+                s = F("# ");
+            } else {
+                s = F("> ");
+            }
+        } else {
+            s = F("  ");
+        }
+
+        if (i < N) {
+            s += texts[i] + String(*(values[i])) + F(" (") + String(mins[i]) + F("/") + String(maxs[i]) + F(")");
+        } else {
+            if (getChild() != NULL) {
+                s += F("Continue");
+            } else {
+                s += F("Done");
+            }
+        }
+
+        lcd_set_menu_text(i, s.c_str());
+    }
+}
+
+template <typename T, size_t N>
+void StateValues<T, N>::enterState(void) {
+    pos = 0;
+    if (onEnterFunc != NULL) {
+        onEnterFunc();
+    }
+    display();
+}
+
+template <typename T, size_t N>
+void StateValues<T, N>::inState(StateMachineInput smi) {
+    if (editing) {
+        if (smi.encoder != 0) {
+            float vf = smi.encoder;
+            vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
+            int v = vf;
+            *(values[pos]) -= v;
+
+            if (*(values[pos]) < mins[pos]) {
+                *(values[pos]) = mins[pos];
+            }
+
+            if (*(values[pos]) > maxs[pos]) {
+                *(values[pos]) = maxs[pos];
+            }
+
+            if (updateLiveFunc != NULL) {
+                updateLiveFunc(pos, *(values[pos]));
+            }
+
+            display();
+        }
+        if (smi.click) {
+            editing = false;
+            display();
+        }
+    } else {
+        if (smi.encoder != 0) {
+            int tmp = pos;
+            tmp -= smi.encoder;
+
+            while (tmp < 0) {
+                tmp += N + 1;
+            }
+
+            while (tmp >= (N + 1)) {
+                tmp -= N + 1;
+            }
+
+            pos = tmp;
+
+            display();
+        }
+        if (smi.click) {
+            if (pos < N) {
+                editing = true;
+                display();
+            } else {
+                if (updateFunc != NULL) {
+                    for (size_t i = 0; i < N; i++) {
+                        updateFunc(i, *(values[i]));
+                    }
+                }
+
+                if (getChild() != NULL) {
+                    states_go_to(getChild());
+                } else if (getParent() != NULL) {
+                    states_go_to(getParent());
+                }
+            }
+        }
+    }
+}
+
+template class StateValues<uint8_t, 2>;
+template class StateValues<float, 2>;

src/statemachine.cpp

 #include "lcd.h"
 #include "states.h"
 
-State::State(State *_parent) : parent(_parent), child(NULL), title("no title") {
+State::State(State *_parent)
+        : parent(_parent), child(NULL), title(""), heading(""), text("") {
     if (_parent != NULL) {
         _parent->setChild(this);
     }
-}
-
-// --------------------------------------
 
-StateText::StateText(State *_parent) : State(_parent) {
-    heading = "no heading";
-    text = "text missing";
     onEnterFunc = []() { };
+
     whenInFunc = [](StateMachineInput smi) {
         State *s = states_get();
         if (smi.click && (s != NULL)) {
     };
 }
 
-void StateText::setHeading(const char *_heading) {
-    heading = _heading;
-}
-
-void StateText::setText(const char *_text) {
-    text = _text;
-}
-
-void StateText::onEnter(EnterFuncPtr func) {
-    onEnterFunc = func;
-}
-
-void StateText::whenIn(InFuncPtr func) {
-    whenInFunc = func;
-}
-
-void StateText::updateText(void) {
+void State::updateText(void) {
     lcd_clear();
+
     if (heading != NULL) {
         lcd_set_heading(heading);
     } else if (getTitle() != NULL) {
         lcd_set_heading(getTitle());
     }
+
     if (text != NULL) {
         lcd_set_text(text);
     }
 }
 
-void StateText::enterState(void) {
+void State::enterState(void) {
     if (onEnterFunc != NULL) {
         onEnterFunc();
     }
     updateText();
 }
 
-void StateText::inState(struct StateMachineInput smi) {
+void State::inState(struct StateMachineInput smi) {
     if (whenInFunc != NULL) {
         whenInFunc(smi);
     }
 }
-
-// --------------------------------------
-
-StateMenu::StateMenu(State *_parent, bool _show_parent) : State(_parent) {
-    show_parent = _show_parent;
-    menuPos = 0;
-    menuOff = 0;
-}
-
-void StateMenu::setChild(State *_child) {
-    children.push_back(_child);
-}
-
-void StateMenu::addChild(State *_child, int pos) {
-    if (pos < 0) {
-        setChild(_child);
-    } else {
-        array_insert_at_pos(&children, _child, pos);
-    }
-}
-
-void StateMenu::enterState(void) {
-    display();
-}
-
-void StateMenu::display(void) {
-    lcd_clear();
-    lcd_set_heading(getTitle());
-
-    int size = children.size();
-    if (show_parent) {
-        size++;
-    }
-
-    for (int i = menuOff; (i < menuOff + lcd_text_lines()) && (i < size); i++) {
-        String s;
-        if (i == menuPos) {
-            s = F("> ");
-        } else {
-            s = F("  ");
-        }
-        if (i == children.size()) {
-            s += getParent()->getTitle();
-        } else {
-            s += children.at(i)->getTitle();
-        }
-        lcd_set_menu_text(i - menuOff, s.c_str());
-    }
-}
-
-void StateMenu::inState(struct StateMachineInput smi) {
-    int size = children.size();
-    if (show_parent) {
-        size++;
-    }
-
-    if (smi.encoder != 0) {
-        menuPos -= smi.encoder;
-
-        while (menuPos < 0) {
-            menuPos += size;
-        }
-
-        while (menuPos >= size) {
-            menuPos -= size;
-        }
-
-        while (menuPos < menuOff) {
-            menuOff--;
-        }
-
-        while (menuPos >= (menuOff + lcd_text_lines())) {
-            menuOff++;
-        }
-
-        display();
-    }
-
-    if (smi.click) {
-        if (menuPos == children.size()) {
-            menuPos = 0;
-            menuOff = 0;
-            states_go_to(getParent());
-        } else {
-            states_go_to(children.at(menuPos));
-        }
-    }
-}
-
-// --------------------------------------
-
-StateDynamicMenu::StateDynamicMenu(State *_parent) : State(_parent) {
-    menuPos = 0;
-    menuOff = 0;
-    prefix = "";
-    countFunc = NULL;
-    getFunc = NULL;
-    callFunc = NULL;
-}
-
-void StateDynamicMenu::dataCount(CountFuncPtr count) {
-    countFunc = count;
-}
-
-void StateDynamicMenu::dataGet(GetFuncPtr get) {
-    getFunc = get;
-}
-
-void StateDynamicMenu::dataCall(CallFuncPtr call) {
-    callFunc = call;
-}
-
-void StateDynamicMenu::display(void) {
-    lcd_clear();
-    lcd_set_heading(getTitle());
-
-    for (int i = menuOff; (i < menuOff + lcd_text_lines()) && (i < count + 1); i++) {
-        String s;
-        if (i == menuPos) {
-            s = F("> ");
-        } else {
-            s = F("  ");
-        }
-        if (i == count) {
-            s += getParent()->getTitle();
-        } else {
-            s += contents.at(i);
-        }
-        lcd_set_menu_text(i - menuOff, s.c_str());
-    }
-}
-
-void StateDynamicMenu::setPrefix(String pre) {
-    prefix = pre;
-}
-
-void StateDynamicMenu::enterState(void) {
-    // cache all entries on entering state
-    if (countFunc != NULL) {
-        count = countFunc();
-    } else {
-        count = 0;
-    }
-    contents.clear();
-    for (int i = 0; i < count; i++) {
-        if (getFunc != NULL) {
-            contents.push_back(prefix + String(getFunc(i)));
-        } else {
-            contents.push_back(prefix + String(i + 1));
-        }
-    }
-
-    display();
-}
-
-void StateDynamicMenu::inState(StateMachineInput smi) {
-    if (smi.encoder != 0) {
-        menuPos -= smi.encoder;
-
-        while (menuPos < 0) {
-            menuPos += count + 1;
-        }
-
-        while (menuPos >= count + 1) {
-            menuPos -= count + 1;
-        }
-
-        while (menuPos < menuOff) {
-            menuOff--;
-        }
-
-        while (menuPos >= (menuOff + lcd_text_lines())) {
-            menuOff++;
-        }
-
-        display();
-    }
-
-    if (smi.click) {
-        if (menuPos == count) {
-            menuPos = 0;
-            menuOff = 0;
-            states_go_to(getParent());
-        } else {
-            if (callFunc != NULL) {
-                callFunc(menuPos);
-            }
-        }
-    }
-}
-
-// --------------------------------------
-
-template <typename T>
-StateValue<T>::StateValue(State *_parent, T &_value, T _min, T _max) : State(_parent), value(_value) {
-    min = _min;
-    max = _max;
-    heading = NULL;
-    text = NULL;
-    onEnterFunc = NULL;
-    updateFunc = NULL;
-    updateLiveFunc = NULL;
-}
-
-template <typename T>
-void StateValue<T>::setHeading(const char *_heading) {
-    heading = _heading;
-}
-
-template <typename T>
-void StateValue<T>::setText(const char *_text) {
-    text = _text;
-}
-
-template <typename T>
-void StateValue<T>::onEnter(EnterFuncPtr func) {
-    onEnterFunc = func;
-}
-
-template <typename T>
-void StateValue<T>::onUpdate(UpdateFuncPtr func) {
-    updateFunc = func;
-}
-
-template <typename T>
-void StateValue<T>::onLiveUpdate(UpdateFuncPtr func) {
-    updateLiveFunc = func;
-}
-
-template <typename T>
-void StateValue<T>::display(void) {
-    lcd_clear();
-    if (heading == NULL) {
-        lcd_set_heading(getTitle());
-    } else {
-        lcd_set_heading(heading);
-    }
-
-    String s = String(min) + F(" .. ") + String(value) + F(" .. ") + String(max);
-
-    if (text != NULL) {
-        s = text + String(F("\n")) + s;
-    }
-
-    lcd_set_text(s.c_str());
-}
-
-
-template <typename T>
-void StateValue<T>::enterState(void) {
-    if (onEnterFunc != NULL) {
-        onEnterFunc();
-    }
-    display();
-}
-
-template <typename T>
-void StateValue<T>::inState(StateMachineInput smi) {
-    if (smi.encoder != 0) {
-        float vf = smi.encoder;
-        vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
-        int v = vf;
-        value -= v;
-        if (value < min) {
-            value = min;
-        }
-        if (value > max) {
-            value = max;
-        }
-        if (updateLiveFunc != NULL) {
-            updateLiveFunc(value);
-        }
-        display();
-    }
-    if (smi.click) {
-        if (updateFunc != NULL) {
-            updateFunc(value);
-        }
-        states_go_to(getParent());
-    }
-}
-
-template class StateValue<int>;
-template class StateValue<float>;
-
-// --------------------------------------
-
-template <typename T, size_t N>
-StateValues<T, N>::StateValues(State *_parent) : State(_parent) {
-    heading = NULL;
-    onEnterFunc = NULL;
-    updateFunc = NULL;
-    updateLiveFunc = NULL;
-    pos = 0;
-    editing = false;
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::setData(size_t index, const char *name, T *value, T min, T max) {
-    if (index >= N) {
-        return;
-    }
-    values[index] = value;
-    mins[index] = min;
-    maxs[index] = max;
-    texts[index] = name;
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::setHeading(const char *_heading) {
-    heading = _heading;
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::onEnter(EnterFuncPtr func) {
-    onEnterFunc = func;
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::onUpdate(UpdateFuncPtr func) {
-    updateFunc = func;
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::onLiveUpdate(UpdateFuncPtr func) {
-    updateLiveFunc = func;
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::display(void) {
-    lcd_clear();
-
-    if (heading == NULL) {
-        lcd_set_heading(getTitle());
-    } else {
-        lcd_set_heading(heading);
-    }
-
-    for (size_t i = 0; i < (N + 1); i++) {
-        String s;
-
-        if (i == pos) {
-            if (editing) {
-                s = F("# ");
-            } else {
-                s = F("> ");
-            }
-        } else {
-            s = F("  ");
-        }
-
-        if (i < N) {
-            s += texts[i] + String(*(values[i])) + F(" (") + String(mins[i]) + F("/") + String(maxs[i]) + F(")");
-        } else {
-            if (getChild() != NULL) {
-                s += F("Continue");
-            } else {
-                s += F("Done");
-            }
-        }
-
-        lcd_set_menu_text(i, s.c_str());
-    }
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::enterState(void) {
-    pos = 0;
-    if (onEnterFunc != NULL) {
-        onEnterFunc();
-    }
-    display();
-}
-
-template <typename T, size_t N>
-void StateValues<T, N>::inState(StateMachineInput smi) {
-    if (editing) {
-        if (smi.encoder != 0) {
-            float vf = smi.encoder;
-            vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
-            int v = vf;
-            *(values[pos]) -= v;
-
-            if (*(values[pos]) < mins[pos]) {
-                *(values[pos]) = mins[pos];
-            }
-
-            if (*(values[pos]) > maxs[pos]) {
-                *(values[pos]) = maxs[pos];
-            }
-
-            if (updateLiveFunc != NULL) {
-                updateLiveFunc(pos, *(values[pos]));
-            }
-
-            display();
-        }
-        if (smi.click) {
-            editing = false;
-            display();
-        }
-    } else {
-        if (smi.encoder != 0) {
-            int tmp = pos;
-            tmp -= smi.encoder;
-
-            while (tmp < 0) {
-                tmp += N + 1;
-            }
-
-            while (tmp >= (N + 1)) {
-                tmp -= N + 1;
-            }
-
-            pos = tmp;
-
-            display();
-        }
-        if (smi.click) {
-            if (pos < N) {
-                editing = true;
-                display();
-            } else {
-                if (updateFunc != NULL) {
-                    for (size_t i = 0; i < N; i++) {
-                        updateFunc(i, *(values[i]));
-                    }
-                }
-
-                if (getChild() != NULL) {
-                    states_go_to(getChild());
-                } else if (getParent() != NULL) {
-                    states_go_to(getParent());
-                }
-            }
-        }
-    }
-}
-
-template class StateValues<uint8_t, 2>;
-template class StateValues<float, 2>;
-
-// --------------------------------------
-
-template <typename T, size_t N>
-void array_print(Array<T, N> *arr) {
-    Serial.print(F("Array length: "));
-    Serial.print(arr->size());
-    Serial.println(F(" contents:"));
-    for (int i = 0; i < arr->size(); i++) {
-        Serial.print(i);
-        Serial.print(F(": "));
-        Serial.println(arr->at(i)->getTitle());
-    }
-}
-
-template <typename T, size_t N>
-void array_insert_at_pos(Array<T, N> *arr, T value, size_t pos) {
-    // make additional space
-    arr->push_back(value);
-
-    if ((pos >= arr->max_size()) || (pos >= arr->size())) {
-        // we can not shift it to the given position
-        return;
-    }
-
-    for (int i = arr->size() - 2; i >= pos; i--) {
-        arr->at(i + 1) = arr->at(i);
-    }
-
-    arr->at(pos) = value;
-}

src/states.cpp

 
 // --------------------------------------
 
-StateText sm_ask_homing = StateText();
-StateText sm_do_homing = StateText(&sm_ask_homing);
+int preset_selection = 0, dispense_off = 0,  dispense_count = 0, dispense_index = 0;
+float move_pos_x = 0.0, move_pos_y = 0.0, move_pos_z = 0.0, move_pos_e = 0.0;
+
+State sm_ask_homing = State();
+State sm_do_homing = State(&sm_ask_homing);
 StateMenu sm_menu = StateMenu(&sm_do_homing, false);
 
 StateMenu sm_auto = StateMenu(&sm_menu);
 StateDynamicMenu sm_presets = StateDynamicMenu(&sm_auto);
-// TODO dispense
 
-StateText sm_new_preset = StateText(&sm_auto);
-StateText sm_wiz_move = StateText(&sm_new_preset);
+StateValue<int> sm_disp_off = StateValue<int>(&sm_presets, dispense_off, 1, 1);
+StateValue<int> sm_disp_cnt = StateValue<int>(&sm_disp_off, dispense_count, 1, 1);
+State sm_disp_go = State(&sm_disp_cnt);
+State sm_disp_run_z_travel = State(&sm_disp_go);
+State sm_disp_run_xy_travel = State(&sm_disp_run_z_travel);
+State sm_disp_run_z_disp = State(&sm_disp_run_xy_travel);
+State sm_disp_run = State(&sm_disp_run_z_disp);
+State sm_disp_done = State(&sm_disp_run);
+
+State sm_new_preset = State(&sm_auto);
+State sm_wiz_move = State(&sm_new_preset);
 StateValues<uint8_t, 2> sm_wiz_count = StateValues<uint8_t, 2>(&sm_wiz_move);
 StateValues<float, 2> sm_wiz_first_pos = StateValues<float, 2>(&sm_wiz_count);
 StateValues<float, 2> sm_wiz_last_pos = StateValues<float, 2>(&sm_wiz_first_pos);
 // TODO move z above container
-// TODO move z inside container
+// TODO move z inside bottom container
+// TODO move z inside top container
 // TODO move to extruder depth
-StateText sm_new_preset_done = StateText(&sm_wiz_last_pos);
+State sm_new_preset_done = State(&sm_wiz_last_pos);
 
 StateDynamicMenu sm_mod_preset = StateDynamicMenu(&sm_auto);
 // TODO modify
 StateDynamicMenu sm_del_preset = StateDynamicMenu(&sm_auto);
 // TODO delete
 
-float move_pos_x = 0.0, move_pos_y = 0.0, move_pos_z = 0.0, move_pos_e = 0.0;
 StateMenu sm_move = StateMenu(&sm_menu);
 StateValue<float> sm_move_x = StateValue<float>(&sm_move, move_pos_x, X_AXIS_MIN, X_AXIS_MAX);
 StateValue<float> sm_move_y = StateValue<float>(&sm_move, move_pos_y, Y_AXIS_MIN, Y_AXIS_MAX);
 StateValue<float> sm_move_z = StateValue<float>(&sm_move, move_pos_z, Z_AXIS_MIN, Z_AXIS_MAX);
 StateValue<float> sm_move_e = StateValue<float>(&sm_move, move_pos_e, E_AXIS_MIN, E_AXIS_MAX);
 
+State sm_disable_steppers = State(&sm_menu);
+
 StateMenu sm_config = StateMenu(&sm_menu);
-StateText sm_conf_load = StateText(&sm_config);
-StateText sm_conf_save = StateText(&sm_config);
+State sm_conf_load = State(&sm_config);
+State sm_conf_save = State(&sm_config);
+State sm_conf_clear = State(&sm_config);
 StateValue<float> sm_conf_speed_xy = StateValue<float>(&sm_config, data_options()->speed_x, 1.0, XY_MAX_SPEED);
 StateValue<float> sm_conf_speed_z = StateValue<float>(&sm_config, data_options()->speed_z, 1.0, Z_MAX_SPEED);
 StateValue<float> sm_conf_speed_e = StateValue<float>(&sm_config, data_options()->speed_e, 1.0, E_MAX_SPEED);
 StateValue<float> sm_conf_accel_z = StateValue<float>(&sm_config, data_options()->accel_z, 1.0, Z_MAX_ACCEL);
 StateValue<float> sm_conf_accel_e = StateValue<float>(&sm_config, data_options()->accel_e, 1.0, E_MAX_ACCEL);
 
-StateText sm_error = StateText(NULL);
+State sm_error = State(NULL);
+
+#if defined(ENABLE_MOVEMENT_TEST_STATE) || defined(ENABLE_INPUT_TEST_STATE)
+StateMenu sm_debug = StateMenu(&sm_menu);
+#endif
 
 #ifdef ENABLE_MOVEMENT_TEST_STATE
-StateMenu sm_movement_test = StateMenu(&sm_menu);
-StateText sm_move_x_test = StateText(&sm_movement_test);
-StateText sm_move_y_test = StateText(&sm_movement_test);
+StateMenu sm_movement_test = StateMenu(&sm_debug);
+State sm_move_xy_test = State(&sm_movement_test);
+State sm_move_x_test = State(&sm_movement_test);
+State sm_move_y_test = State(&sm_movement_test);
+State sm_move_z_test = State(&sm_movement_test);
+State sm_move_e_test = State(&sm_movement_test);
 #endif // ENABLE_MOVEMENT_TEST_STATE
 
 #ifdef ENABLE_INPUT_TEST_STATE
-StateText sm_input_test = StateText(&sm_menu);
+State sm_input_test = State(&sm_debug);
 #endif // ENABLE_INPUT_TEST_STATE
 
 // --------------------------------------
 struct data_config_preset wizard_preset = { .count_x = 1, .count_y = 1 };
 float wizard_first_x = 0.0, wizard_first_y = 0.0;
 float wizard_last_x = 0.0, wizard_last_y = 0.0;
+unsigned long last_state_change = 0, last_input = 0;
 
 void states_init(void) {
     // ----------------------------------
 
     sm_do_homing.whenIn([](StateMachineInput smi) {
 #ifndef DISABLE_HOMING_STATE
-        if (smi.motors_done) {
+        if (smi.all_motors_done) {
             if (steppers_homed()) {
 #endif // DISABLE_HOMING_STATE
 
         steppers_move_e(v);
     });
 
+    sm_disable_steppers.setTitle("Disable Steppers");
+    sm_disable_steppers.onEnter([]() {
+        steppers_kill();
+        states_go_to(&sm_ask_homing);
+    });
+
     // ----------------------------------
 
     sm_auto.setTitle("Filling Menu");
 
-    // TODO
+    auto preset_name_func = [](int i) {
+        strbuf = String(i + 1);
+        strbuf += String(F(" ("));
+        strbuf += String(data_preset(i)->count_x);
+        strbuf += String(F("*"));
+        strbuf += String(data_preset(i)->count_y);
+        strbuf += String(F(" / "));
+        strbuf += String(data_preset(i)->distance_x);
+        strbuf += String(F("*"));
+        strbuf += String(data_preset(i)->distance_y);
+        strbuf += String(F(")"));
+        return strbuf.c_str();
+    };
+
     sm_presets.setTitle("Use Preset");
     sm_presets.setPrefix("Preset ");
     sm_presets.dataCount([]() {
         return (int)data_preset_count();
     });
+    sm_presets.dataGet(preset_name_func);
+    sm_presets.dataCall([](int i) {
+        preset_selection = i;
+        states_go_to(&sm_disp_off);
+    });
+
+    sm_disp_off.setTitle("Offset");
+    sm_disp_off.setText("Where to start dispensing?");
+    sm_disp_off.onEnter([]() {
+        dispense_off = 1;
+        sm_disp_off.setMax(data_preset(preset_selection)->count_x * data_preset(preset_selection)->count_y);
+    });
+
+    sm_disp_cnt.setTitle("Count");
+    sm_disp_cnt.setText("How many to fill?");
+    sm_disp_cnt.onEnter([]() {
+        dispense_count = data_preset(preset_selection)->count_x * data_preset(preset_selection)->count_y - (dispense_off - 1);
+        sm_disp_cnt.setMax(dispense_count);
+    });
+
+    sm_disp_go.setTitle("Confirm");
+    sm_disp_go.onEnter([]() {
+        dispense_off -= 1;
+        strbuf = String(F("Filling from ")) + String(dispense_off + 1);
+        strbuf += String(F(" to ")) + String(dispense_off + dispense_count);
+        strbuf += String(F("!\nIs this ok?"));
+        sm_disp_go.setText(strbuf.c_str());
+
+        dispense_index = dispense_off;
+
+        // TODO allow yes/no choice?
+    });
+
+    sm_disp_run_z_travel.setTitle("Z Travel");
+    sm_disp_run_z_travel.onEnter([]() {
+        // TODO move z to travel height
+    });
+    sm_disp_run_z_travel.whenIn([](StateMachineInput smi) {
+        // TODO update status screen
+
+        if (smi.all_motors_done) {
+            states_go_to(&sm_disp_run_xy_travel);
+        }
+    });
+
+    sm_disp_run_xy_travel.setTitle("XY Movement");
+    sm_disp_run_xy_travel.onEnter([]() {
+        int y = dispense_index / data_preset(preset_selection)->count_x;
+        int x = dispense_index % data_preset(preset_selection)->count_x;
+
+        Serial.print(F("xy_travel: index="));
+        Serial.print(dispense_index);
+        Serial.print(F(" x="));
+        Serial.print(x);
+        Serial.print(F(" y="));
+        Serial.println(y);
+
+        float x_pos = data_preset(preset_selection)->offset_x + x * data_preset(preset_selection)->distance_x;
+        float y_pos = data_preset(preset_selection)->offset_y + y * data_preset(preset_selection)->distance_y;
+
+        steppers_move_x(x_pos);
+        steppers_move_y(y_pos);
+    });
+    sm_disp_run_xy_travel.whenIn([](StateMachineInput smi) {
+        // TODO update status screen
+
+        if (smi.all_motors_done) {
+            states_go_to(&sm_disp_run_z_disp);
+        }
+    });
+
+    sm_disp_run_z_disp.setTitle("Z Dispense");
+    sm_disp_run_z_disp.onEnter([]() {
+        // TODO move z to dispension height
+    });
+    sm_disp_run_z_disp.whenIn([](StateMachineInput smi) {
+        // TODO update status screen
+
+        if (smi.all_motors_done) {
+            states_go_to(&sm_disp_run);
+        }
+    });
+
+    sm_disp_run.setTitle("Dispensing");
+    sm_disp_run.onEnter([]() {
+        // TODO start dispensing with e and moving z up
+    });
+    sm_disp_run.whenIn([](StateMachineInput smi) {
+        // TODO update status screen
+
+        if (smi.all_motors_done) {
+            dispense_index++;
+            if (dispense_index >= (dispense_count + dispense_off)) {
+                states_go_to(&sm_disp_done);
+            } else {
+                states_go_to(&sm_disp_run_z_travel);
+            }
+        }
+    });
+
+    sm_disp_done.setTitle("Finished");
+    sm_disp_done.onEnter([]() {
+        // TODO show end screen, "stats"
+    });
+    sm_disp_done.whenIn([](StateMachineInput smi) {
+        if (smi.click) {
+            states_go_to(&sm_auto);
+        }
+    });
 
     sm_new_preset.setTitle("Add new Preset");
     sm_new_preset.setHeading("Preset Wizard");
     });
     sm_wiz_move.whenIn([](StateMachineInput smi) {
 #ifndef DISABLE_HOMING_STATE
-        if (smi.motors_done) {
+        if (smi.all_motors_done) {
             if (steppers_homed()) {
 #endif // DISABLE_HOMING_STATE
                 states_go_to(&sm_wiz_count);
     sm_mod_preset.dataCount([]() {
         return (int)data_preset_count();
     });
+    sm_mod_preset.dataGet(preset_name_func);
 
-    // TODO
     sm_del_preset.setTitle("Delete Preset");
     sm_del_preset.setPrefix("Preset ");
     sm_del_preset.dataCount([]() {
         return (int)data_preset_count();
     });
+    sm_del_preset.dataGet(preset_name_func);
+    sm_del_preset.dataCall([](int i) {
+        data_preset_remove(i);
+        states_go_to(&sm_auto);
+    });
 
     // ----------------------------------
 
         data_eeprom_write();
     });
 
+    sm_conf_clear.setTitle("Clear EEPROM & RAM");
+    sm_conf_clear.setHeading("Clear EEPROM & RAM");
+    sm_conf_clear.setText("EEPROM and RAM Configuration cleared!");
+    sm_conf_clear.onEnter([]() {
+        data_clear();
+        data_eeprom_write();
+
+        // reapply default conf values
+        steppers_set_speed_x(data_options()->speed_x);
+        steppers_set_speed_y(data_options()->speed_y);
+        steppers_set_speed_z(data_options()->speed_z);
+        steppers_set_speed_e(data_options()->speed_e);
+        steppers_set_accel_x(data_options()->accel_x);
+        steppers_set_accel_y(data_options()->accel_y);
+        steppers_set_accel_z(data_options()->accel_z);
+        steppers_set_accel_e(data_options()->accel_e);
+    });
+
     sm_conf_speed_xy.setTitle("XY Speed");
     sm_conf_speed_xy.onUpdate([](float v) {
         steppers_set_speed_x(v);
     sm_error.setChild(&sm_ask_homing);
     sm_error.setTitle("Axis Error");
     sm_error.setHeading("Axis Error");
-    sm_error.setText("Endstop has been hit!");
+    sm_error.setText("An unknown error occured!");
+
+#if defined(ENABLE_MOVEMENT_TEST_STATE) || defined(ENABLE_INPUT_TEST_STATE)
+    sm_debug.setTitle("Debug Menu");
+#endif
 
 #ifdef ENABLE_MOVEMENT_TEST_STATE
     sm_movement_test.setTitle("Movement Test");
 
+    sm_move_xy_test.setTitle("XY Axes");
+    sm_move_xy_test.setHeading("XY Move Test");
+    sm_move_xy_test.setText("Moving axis through full range with selected speed.");
+    sm_move_xy_test.whenIn([](StateMachineInput smi) {
+        static bool sx = false, sy = false;
+        if (smi.motor_done[X_AXIS]) {
+            sx = !sx;
+            steppers_move_x(sx ? (X_AXIS_MAX - X_AXIS_EPSILON) : (X_AXIS_MIN + X_AXIS_EPSILON));
+        }
+        if (smi.motor_done[Y_AXIS]) {
+            sy = !sy;
+            steppers_move_y(sy ? (Y_AXIS_MAX - Y_AXIS_EPSILON) : (Y_AXIS_MIN + Y_AXIS_EPSILON));
+        }
+        if (smi.click) {
+            states_go_to(&sm_movement_test);
+        }
+    });
+
     sm_move_x_test.setTitle("X Axis");
     sm_move_x_test.setHeading("X Move Test");
+    sm_move_x_test.setText("Moving axis through full range with selected speed.");
     sm_move_x_test.whenIn([](StateMachineInput smi) {
         static bool s = false;
-        if (smi.motors_done) {
+        if (smi.motor_done[X_AXIS]) {
             s = !s;
-            steppers_move_x(s ? (X_AXIS_MAX - 10.0) : (X_AXIS_MIN + 10.0));
-            if (smi.click) {
-                states_go_to(&sm_movement_test);
-            }
+            steppers_move_x(s ? (X_AXIS_MAX - X_AXIS_EPSILON) : (X_AXIS_MIN + X_AXIS_EPSILON));
+        }
+        if (smi.click) {
+            states_go_to(&sm_movement_test);
         }
     });
 
     sm_move_y_test.setTitle("Y Axis");
     sm_move_y_test.setHeading("Y Move Test");
+    sm_move_y_test.setText("Moving axis through full range with selected speed.");
     sm_move_y_test.whenIn([](StateMachineInput smi) {
         static bool s = false;
-        if (smi.motors_done) {
+        if (smi.motor_done[Y_AXIS]) {
             s = !s;
-            steppers_move_y(s ? (Y_AXIS_MAX - 10.0) : (Y_AXIS_MIN + 10.0));
-            if (smi.click) {
-                states_go_to(&sm_movement_test);
-            }
+            steppers_move_y(s ? (Y_AXIS_MAX - Y_AXIS_EPSILON) : (Y_AXIS_MIN + Y_AXIS_EPSILON));
+        }
+        if (smi.click) {
+            states_go_to(&sm_movement_test);
+        }
+    });
+
+    sm_move_z_test.setTitle("Z Axis");
+    sm_move_z_test.setHeading("Z Move Test");
+    sm_move_z_test.setText("Moving axis through full range with selected speed.");
+    sm_move_z_test.whenIn([](StateMachineInput smi) {
+        static bool s = false;
+        if (smi.motor_done[Z_AXIS]) {
+            s = !s;
+            steppers_move_z(s ? (Z_AXIS_MAX - Z_AXIS_EPSILON) : (Z_AXIS_MIN + Z_AXIS_EPSILON));
+        }
+        if (smi.click) {
+            states_go_to(&sm_movement_test);
+        }
+    });
+
+    sm_move_e_test.setTitle("E Axis");
+    sm_move_e_test.setHeading("E Move Test");
+    sm_move_e_test.setText("Moving axis through full range with selected speed.");
+    sm_move_e_test.whenIn([](StateMachineInput smi) {
+        static bool s = false;
+        if (smi.motor_done[E_AXIS]) {
+            s = !s;
+            steppers_move_e(s ? (E_AXIS_MAX - E_AXIS_EPSILON) : (E_AXIS_MIN + E_AXIS_EPSILON));
+        }
+        if (smi.click) {
+            states_go_to(&sm_movement_test);
         }
     });
 #endif // ENABLE_MOVEMENT_TEST_STATE
 
 #ifdef ENABLE_INPUT_TEST_STATE
     sm_input_test.setTitle("Input Test");
-    sm_input_test.setHeading("Input Test");
+    sm_input_test.setHeading("Endstop Test");
 
     sm_input_test.whenIn([](StateMachineInput smi) {
         String s = "Endstops: ";
-        for (int i = 0; i < 4; i++) {
+        for (int i = 0; i < AXIS_COUNT; i++) {
             if (steppers_home_switch(i)) {
                 s += '1';
             } else {
 }
 
 void states_run(StateMachineInput smi) {
+    if (smi.click || smi.kill || (smi.encoder != 0)) {
+        last_input = millis();
+    }
+
     if (smi.click) {
         async_beep(ENCODER_CLICK_BEEP_TIME, ENCODER_CLICK_BEEP_FREQ);
     }
         return;
     }
 
+#if (IDLE_TIMEOUT > 0)
+    unsigned long last_time = max(last_input, last_state_change);
+    if ((millis() - last_time) > IDLE_TIMEOUT) {
+        if (current_state != &sm_ask_homing) {
+            Serial.println(F("Idle timeout, disabling motors"));
+            steppers_kill();
+            states_go_to(&sm_ask_homing);
+            return;
+        }
+    }
+#endif // (IDLE_TIMEOUT > 0)
+
     if (current_state != NULL) {
         current_state->inState(smi);
     }
 }
 
 void states_go_to(State *state) {
+    last_state_change = millis();
+
     current_state = state;
 
     if (current_state != NULL) {

src/steppers.cpp

 static AccelStepper stepper_z(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN, 0, 0, false);
 static AccelStepper stepper_e(AccelStepper::DRIVER, E0_STEP_PIN, E0_DIR_PIN, 0, 0, false);
 
+#define HOME_PHASE_START 0
+#define HOME_PHASE_BACK 1
+#define HOME_PHASE_FINE 2
+
 enum stepper_states {
     step_disabled,
     step_not_homed,
 static volatile stepper_states state = step_disabled;
 static volatile unsigned long steppers_home_move_start_time = 0;
 
+static volatile bool x_finished = true;
+static volatile bool y_finished = true;
+static volatile bool z_finished = true;
+static volatile bool e_finished = true;
+
 void steppers_init(void) {
     pinMode(X_ENDSTOP_PIN, INPUT_PULLUP);
     pinMode(Y_ENDSTOP_PIN, INPUT_PULLUP);
     pinMode(E0_STEP_PIN, OUTPUT);
     pinMode(E0_DIR_PIN, OUTPUT);
 
+    // fan connected to D8 for electronics
+    pinMode(FAN_PIN, OUTPUT);
+    digitalWrite(FAN_PIN, LOW);
+
     stepper_x.setEnablePin(X_ENABLE_PIN);
     stepper_x.setPinsInverted(false, false, true);
     steppers_set_speed_x(data_options()->speed_x);
 }
 
 static void steppers_initiate_home(int axis, int phase) {
+#if 0
     Serial.print(F("steppers_initiate_home("));
     if (axis == 0) {
         Serial.print('X');
     Serial.print(F(", "));
     Serial.print(phase);
     Serial.println(F(")"));
+#endif
 
     CLEAR_STORE_INTERRUPTS();
 
     steppers_home_move_start_time = millis();
 
-    if (axis == 0) {
+    if (axis == X_AXIS) {
         // x
-        if (phase == 0) {
-            state = step_homing_x_fast;
-            stepper_x.setSpeed(X_HOMING_DIR * X_AXIS_MOVEMENT_DIR * XY_FAST_HOME_SPEED * XY_STEPS_PER_MM);
-        } else if (phase == 1) {
+        if (phase == HOME_PHASE_START) {
+            if (steppers_home_switch(axis)) {
+#if (X_MIN_PIN == -1)
+                stepper_x.setCurrentPosition(X_AXIS_MAX * XY_STEPS_PER_MM * X_AXIS_MOVEMENT_DIR);
+#else
+                stepper_x.setCurrentPosition(X_AXIS_MIN * XY_STEPS_PER_MM * X_AXIS_MOVEMENT_DIR);
+#endif
+
+                // TODO order of homing
+                steppers_initiate_home(Y_AXIS, HOME_PHASE_START);
+            } else {
+                state = step_homing_x_fast;
+                stepper_x.setSpeed(X_HOMING_DIR * X_AXIS_MOVEMENT_DIR * XY_FAST_HOME_SPEED * XY_STEPS_PER_MM);
+            }
+        } else if (phase == HOME_PHASE_BACK) {
             state = step_homing_x_back;
             stepper_x.setSpeed(-1.0 * X_HOMING_DIR * X_AXIS_MOVEMENT_DIR * XY_FAST_HOME_SPEED * XY_STEPS_PER_MM);
-        } else if (phase == 2) {
+        } else if (phase == HOME_PHASE_FINE) {
             state = step_homing_x_slow;
             stepper_x.setSpeed(X_HOMING_DIR * X_AXIS_MOVEMENT_DIR * XY_SLOW_HOME_SPEED * XY_STEPS_PER_MM);
         }
-    } else if (axis == 1) {
+    } else if (axis == Y_AXIS) {
         // y
-        if (phase == 0) {
-            state = step_homing_y_fast;
-            stepper_y.setSpeed(Y_HOMING_DIR * Y_AXIS_MOVEMENT_DIR * XY_FAST_HOME_SPEED * XY_STEPS_PER_MM);
-        } else if (phase == 1) {
+        if (phase == HOME_PHASE_START) {
+            if (steppers_home_switch(axis)) {
+
+#if (Y_MIN_PIN == -1)
+                stepper_y.setCurrentPosition(Y_AXIS_MAX * XY_STEPS_PER_MM * Y_AXIS_MOVEMENT_DIR);
+#else
+                stepper_y.setCurrentPosition(Y_AXIS_MIN * XY_STEPS_PER_MM * Y_AXIS_MOVEMENT_DIR);
+#endif
+
+                // TODO order of homing
+                //steppers_initiate_home(Z_AXIS, HOME_PHASE_START);
+                state = step_homed;
+            } else {
+                state = step_homing_y_fast;
+                stepper_y.setSpeed(Y_HOMING_DIR * Y_AXIS_MOVEMENT_DIR * XY_FAST_HOME_SPEED * XY_STEPS_PER_MM);
+            }
+        } else if (phase == HOME_PHASE_BACK) {
             state = step_homing_y_back;
             stepper_y.setSpeed(-1.0 * Y_HOMING_DIR * Y_AXIS_MOVEMENT_DIR * XY_FAST_HOME_SPEED * XY_STEPS_PER_MM);
-        } else if (phase == 2) {
+        } else if (phase == HOME_PHASE_FINE) {
             state = step_homing_y_slow;
             stepper_y.setSpeed(Y_HOMING_DIR * Y_AXIS_MOVEMENT_DIR * XY_SLOW_HOME_SPEED * XY_STEPS_PER_MM);
         }
-    } else if (axis == 2) {
+    } else if (axis == Z_AXIS) {
         // z
-        if (phase == 0) {
+        if (phase == HOME_PHASE_START) {
             state = step_homing_z_fast;
             stepper_z.setSpeed(Z_HOMING_DIR * Z_AXIS_MOVEMENT_DIR * Z_FAST_HOME_SPEED * Z_STEPS_PER_MM);
-        } else if (phase == 1) {
+        } else if (phase == HOME_PHASE_BACK) {
             state = step_homing_z_back;
             stepper_z.setSpeed(-1.0 * Z_HOMING_DIR * Z_AXIS_MOVEMENT_DIR * Z_FAST_HOME_SPEED * Z_STEPS_PER_MM);
-        } else if (phase == 2) {
+        } else if (phase == HOME_PHASE_FINE) {
             state = step_homing_z_slow;
             stepper_z.setSpeed(Z_HOMING_DIR * Z_AXIS_MOVEMENT_DIR * Z_SLOW_HOME_SPEED * Z_STEPS_PER_MM);
         }
-    } else if (axis == 3) {
+    } else if (axis == E_AXIS) {
         // e
-        if (phase == 0) {
+        if (phase == HOME_PHASE_START) {
             state = step_homing_e_fast;
             stepper_e.setSpeed(E_HOMING_DIR * E_AXIS_MOVEMENT_DIR * E_FAST_HOME_SPEED * E_STEPS_PER_PERCENT);
-        } else if (phase == 1) {
+        } else if (phase == HOME_PHASE_BACK) {
             state = step_homing_e_back;
             stepper_e.setSpeed(-1.0 * E_HOMING_DIR * E_AXIS_MOVEMENT_DIR * E_FAST_HOME_SPEED * E_STEPS_PER_PERCENT);
-        } else if (phase == 2) {
+        } else if (phase == HOME_PHASE_FINE) {
             state = step_homing_e_slow;
             stepper_e.setSpeed(E_HOMING_DIR * E_AXIS_MOVEMENT_DIR * E_SLOW_HOME_SPEED * E_STEPS_PER_PERCENT);
         }
     bool r = true;
     CLEAR_STORE_INTERRUPTS();
 
-    if (axis == 0) {
+    if (axis == X_AXIS) {
         r = digitalRead(X_ENDSTOP_PIN) == LOW;
-    } else if (axis == 1) {
+    } else if (axis == Y_AXIS) {
         r = digitalRead(Y_ENDSTOP_PIN) == LOW;
-    } else if (axis == 2) {
+    } else if (axis == Z_AXIS) {
         r = digitalRead(Z_ENDSTOP_PIN) == LOW;
-    } else if (axis == 3) {
+    } else if (axis == E_AXIS) {
         r = digitalRead(E_ENDSTOP_PIN) == LOW;
     } else {
         Serial.println(F("home_switch error: invalid axis"));
     return r;
 }
 
+bool stepper_reached_target(int axis) {
+    if (axis == X_AXIS) {
+        return x_finished;
+    } else if (axis == Y_AXIS) {
+        return y_finished;
+    } else if (axis == Z_AXIS) {
+        return z_finished;
+    } else if (axis == E_AXIS) {
+        return e_finished;
+    } else {
+        Serial.println(F("stepper_reached_target error: invalid axis"));
+        return false;
+    }
+}
+
 float steppers_current_pos(int axis) {
     float r = 0.0;
     CLEAR_STORE_INTERRUPTS();
 
-    if (axis == 0) {
+    if (axis == X_AXIS) {
         float v = stepper_x.currentPosition();
         r = v / XY_STEPS_PER_MM / X_AXIS_MOVEMENT_DIR;
-    } else if (axis == 1) {
+    } else if (axis == Y_AXIS) {
         float v = stepper_y.currentPosition();
         r = v / XY_STEPS_PER_MM / Y_AXIS_MOVEMENT_DIR;
-    } else if (axis == 2) {
+    } else if (axis == Z_AXIS) {
         float v = stepper_z.currentPosition();
         r = v / Z_STEPS_PER_MM / Z_AXIS_MOVEMENT_DIR;
-    } else if (axis == 3) {
+    } else if (axis == E_AXIS) {
         float v = stepper_e.currentPosition();
         r = E_PERCENT_TO_MM(v / E_STEPS_PER_PERCENT / E_AXIS_MOVEMENT_DIR);
     } else {
     return r;
 }
 
-bool steppers_run(void) {
-    if (state == step_homing_x_fast) {
-        if (steppers_home_switch(0)) {
-            steppers_initiate_home(0, 1);
+// TODO split into separate run funcs for running and homing
+// TODO only enable timer1 when either homing or moving
+// TODO disable timer1 when no longer homing/moving
+// TODO (when timer is disabled, still call in normal loop?)
+
+void steppers_run(void) {
+    if ((state == step_homed) || (state == step_not_homed)) {
+        for (int i = 0; i < AXIS_COUNT; i++) {
+#if 0
+            Serial.print(i);
+            Serial.print(": ");
+            Serial.println(steppers_current_pos(i));
+#endif
+
+            if (steppers_home_switch(i)) {
+                float compare = 0.0, epsilon = 0.0;
+                if (i == X_AXIS) {
+                    epsilon = X_AXIS_EPSILON;
+    #if (X_HOMING_DIR == 1)
+                    compare = X_AXIS_MAX;
+    #else
+                    compare = X_AXIS_MIN;
+    #endif
+                } else if (i == Y_AXIS) {
+                    epsilon = Y_AXIS_EPSILON;
+    #if (Y_HOMING_DIR == 1)
+                    compare = Y_AXIS_MAX;
+    #else
+                    compare = Y_AXIS_MIN;
+    #endif
+                } else if (i == Z_AXIS) {
+                    epsilon = Z_AXIS_EPSILON;
+    #if (Z_HOMING_DIR == 1)
+                    compare = Z_AXIS_MAX;
+    #else
+                    compare = Z_AXIS_MIN;
+    #endif
+                } else if (i == E_AXIS) {
+                    epsilon = E_AXIS_EPSILON;
+    #if (E_HOMING_DIR == 1)
+                    compare = E_AXIS_MAX;
+    #else
+                    compare = E_AXIS_MIN;
+    #endif
+                }
+
+                if (fabs(steppers_current_pos(i) - compare) > epsilon) {
+#if 0
+                    Serial.print(F("Endstop hit at "));
+                    Serial.println(steppers_current_pos(i));
+#endif
+
+                    steppers_kill();
+
+                    if (i == X_AXIS) {
+                        sm_error.setText("Enstop hit on X axis");
+                    } else if (i == Y_AXIS) {
+                        sm_error.setText("Enstop hit on Y axis");
+                    } else if (i == Z_AXIS) {
+                        sm_error.setText("Enstop hit on Z axis");
+                    } else if (i == E_AXIS) {
+                        sm_error.setText("Enstop hit on E axis");
+                    } else {
+                        sm_error.setText("Enstop hit on unknown axis");
+                    }
+                    states_go_to(&sm_error);
+                }
+            }
+        }
+
+        x_finished = !stepper_x.run();
+        y_finished = !stepper_y.run();
+        z_finished = !stepper_z.run();
+        e_finished = !stepper_e.run();
+    } else if (state == step_homing_x_fast) {
+        if (steppers_home_switch(X_AXIS)) {
+            steppers_initiate_home(X_AXIS, HOME_PHASE_BACK);
         } else {
             stepper_x.runSpeed();
         }
     } else if (state == step_homing_x_back) {
         unsigned long end_time = steppers_home_move_start_time + XY_HOME_BACK_OFF_TIME;
-        if ((!steppers_home_switch(0)) && (millis() >= end_time)) {
-            steppers_initiate_home(0, 2);
+        if ((!steppers_home_switch(X_AXIS)) && (millis() >= end_time)) {
+            steppers_initiate_home(X_AXIS, HOME_PHASE_FINE);
         } else {
             stepper_x.runSpeed();
         }
     } else if (state == step_homing_x_slow) {
-        if (steppers_home_switch(0)) {
+        if (steppers_home_switch(X_AXIS)) {
             stepper_x.setSpeed(0);
 
 #if (X_MIN_PIN == -1)
             stepper_x.setCurrentPosition(X_AXIS_MIN * XY_STEPS_PER_MM * X_AXIS_MOVEMENT_DIR);
 #endif
 
-            steppers_initiate_home(1, 0);
+            steppers_initiate_home(Y_AXIS, HOME_PHASE_START);
         } else {
             stepper_x.runSpeed();
         }
     } else if (state == step_homing_y_fast) {
-        if (steppers_home_switch(1)) {
-            steppers_initiate_home(1, 1);
+        if (steppers_home_switch(Y_AXIS)) {
+            steppers_initiate_home(Y_AXIS, HOME_PHASE_BACK);
         } else {
             stepper_y.runSpeed();
         }
     } else if (state == step_homing_y_back) {
         unsigned long end_time = steppers_home_move_start_time + XY_HOME_BACK_OFF_TIME;
-        if ((!steppers_home_switch(1)) && (millis() >= end_time)) {
-            steppers_initiate_home(1, 2);
+        if ((!steppers_home_switch(Y_AXIS)) && (millis() >= end_time)) {
+            steppers_initiate_home(Y_AXIS, HOME_PHASE_FINE);
         } else {
             stepper_y.runSpeed();
         }
     } else if (state == step_homing_y_slow) {
-        if (steppers_home_switch(1)) {
+        if (steppers_home_switch(Y_AXIS)) {
             stepper_y.setSpeed(0);
 
 #if (Y_MIN_PIN == -1)
             stepper_y.setCurrentPosition(Y_AXIS_MIN * XY_STEPS_PER_MM * Y_AXIS_MOVEMENT_DIR);
 #endif
 
-            //steppers_initiate_home(3, 0);
+            //steppers_initiate_home(E_AXIS, HOME_PHASE_START);
             // TODO
             state = step_homed;
-            return false;
         } else {
             stepper_y.runSpeed();
         }
     } else if (state == step_homing_z_fast) {
-        if (steppers_home_switch(2)) {
-            steppers_initiate_home(2, 1);
+        if (steppers_home_switch(Z_AXIS)) {
+            steppers_initiate_home(Z_AXIS, HOME_PHASE_BACK);
         } else {
             stepper_z.runSpeed();
         }
     } else if (state == step_homing_z_back) {
         unsigned long end_time = steppers_home_move_start_time + Z_HOME_BACK_OFF_TIME;
-        if ((!steppers_home_switch(2)) && (millis() >= end_time)) {
-            steppers_initiate_home(2, 2);
+        if ((!steppers_home_switch(Z_AXIS)) && (millis() >= end_time)) {
+            steppers_initiate_home(Z_AXIS, HOME_PHASE_FINE);
         } else {
             stepper_z.runSpeed();
         }
     } else if (state == step_homing_z_slow) {
-        if (steppers_home_switch(2)) {
+        if (steppers_home_switch(Z_AXIS)) {
             stepper_z.setSpeed(0);
 
 #if (Z_MIN_PIN == -1)
             stepper_z.setCurrentPosition(Z_AXIS_MIN * Z_STEPS_PER_MM * Z_AXIS_MOVEMENT_DIR);
 #endif
 
-            steppers_initiate_home(0, 0);
+            steppers_initiate_home(X_AXIS, HOME_PHASE_START);
         } else {
             stepper_z.runSpeed();
         }
     } else if (state == step_homing_e_fast) {
-        if (steppers_home_switch(3)) {
-            steppers_initiate_home(3, 1);
+        if (steppers_home_switch(E_AXIS)) {
+            steppers_initiate_home(E_AXIS, HOME_PHASE_BACK);
         } else {
             stepper_e.runSpeed();
         }
     } else if (state == step_homing_e_back) {
         unsigned long end_time = steppers_home_move_start_time + E_HOME_BACK_OFF_TIME;
-        if ((!steppers_home_switch(3)) && (millis() >= end_time)) {
-            steppers_initiate_home(3, 2);
+        if ((!steppers_home_switch(E_AXIS)) && (millis() >= end_time)) {
+            steppers_initiate_home(E_AXIS, HOME_PHASE_FINE);
         } else {
             stepper_e.runSpeed();
         }
     } else if (state == step_homing_e_slow) {
-        if (steppers_home_switch(3)) {
+        if (steppers_home_switch(E_AXIS)) {
             stepper_e.setSpeed(0);
 
 #if (E_MIN_PIN == -1)
 #endif
 
             state = step_homed;
-            return false;
         } else {
             stepper_e.runSpeed();
         }
-    } else if (state != step_disabled) {
-        if (state == step_homed) {
-            for (int i = 0; i < 4; i++) {
-                Serial.print(i);
-                Serial.print(": ");
-                Serial.println(steppers_current_pos(i));
-
-                float compare = 0.0, epsilon = 0.0;
-                if (i == 0) {
-                    epsilon = X_AXIS_EPSILON;
-#if (X_MIN_PIN == -1)
-                    compare = X_AXIS_MAX;
-#else
-                    compare = X_AXIS_MIN;
-#endif
-                } else if (i == 1) {
-                    epsilon = Y_AXIS_EPSILON;
-#if (Y_MIN_PIN == -1)
-                    compare = Y_AXIS_MAX;
-#else
-                    compare = Y_AXIS_MIN;
-#endif
-                } else if (i == 2) {
-                    epsilon = Z_AXIS_EPSILON;
-#if (Z_MIN_PIN == -1)
-                    compare = Z_AXIS_MAX;
-#else
-                    compare = Z_AXIS_MIN;
-#endif
-                } else if (i == 3) {
-                    epsilon = E_AXIS_EPSILON;
-#if (E_MIN_PIN == -1)
-                    compare = E_AXIS_MAX;
-#else
-                    compare = E_AXIS_MIN;
-#endif
-                }
-
-                if (fabs(steppers_current_pos(i) - compare) > epsilon) {
-                    if (steppers_home_switch(i)) {
-                        Serial.print(F("Endstop hit at "));
-                        Serial.println(steppers_current_pos(i));
-
-                        steppers_kill();
-
-                        if (i == 0) {
-                            sm_error.setText("Enstop hit on X axis");
-                        } else if (i == 1) {
-                            sm_error.setText("Enstop hit on Y axis");
-                        } else if (i == 2) {
-                            sm_error.setText("Enstop hit on Z axis");
-                        } else if (i == 3) {
-                            sm_error.setText("Enstop hit on E axis");
-                        } else {
-                            sm_error.setText("Enstop hit on unknown axis");
-                        }
-                        states_go_to(&sm_error);
-                    }
-                }
-            }
-        }
-
-        boolean x = stepper_x.run();
-        boolean y = stepper_y.run();
-        boolean z = stepper_z.run();
-        boolean e = stepper_e.run();
-
-        return x || y || z || e;
     }
-
-    return true;
 }
 
 bool steppers_homed(void) {
 void steppers_start_homing(void) {
     CLEAR_STORE_INTERRUPTS();
 
+    digitalWrite(FAN_PIN, HIGH);
+
     stepper_x.enableOutputs();
     stepper_y.enableOutputs();
     stepper_z.enableOutputs();
     stepper_e.enableOutputs();
 
-    //steppers_initiate_home(2, 0);
+    //steppers_initiate_home(Z_AXIS, HOME_PHASE_START);
     // TODO
-    steppers_initiate_home(0, 0);
+    steppers_initiate_home(X_AXIS, HOME_PHASE_START);
 
     RESTORE_INTERRUPTS();
 }
     if (state == step_disabled) {
         Serial.println(F("Enabling stepper drivers"));
 
+        digitalWrite(FAN_PIN, HIGH);
+
         stepper_x.enableOutputs();
         stepper_y.enableOutputs();
         stepper_z.enableOutputs();
     stepper_z.disableOutputs();
     stepper_e.disableOutputs();
 
-    state = step_not_homed;
+    digitalWrite(FAN_PIN, LOW);
+
+    state = step_disabled;
 
     RESTORE_INTERRUPTS();
 }