updated code for use with first prototype of xy table

include/common.h

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

include/config.h

 #define E_MAX_SPEED 50.0 // in percent/s
 
 // homing speeds
-#define XY_FAST_HOME_SPEED 2.5 // in mm/s
-#define XY_SLOW_HOME_SPEED 1.0 // in mm/s
-#define Z_FAST_HOME_SPEED 2.0 // in mm/s
+#define XY_FAST_HOME_SPEED 25.0 // in mm/s
+#define XY_SLOW_HOME_SPEED 5.0 // in mm/s
+#define Z_FAST_HOME_SPEED 5.0 // in mm/s
 #define Z_SLOW_HOME_SPEED 1.0 // in mm/s
 #define E_FAST_HOME_SPEED 10.0 // in percent/s
-#define E_SLOW_HOME_SPEED 5.0 // in percent/s
+#define E_SLOW_HOME_SPEED 2.0 // in percent/s
 
 // accelerations
 #define XY_MAX_ACCEL 100.0 // in mm/s^2
 #define E_AXIS_MOVEMENT_DIR 1.0
 
 // homing back-off
-#define XY_BACK_OFF_DISTANCE 10.0 // in mm
-#define Z_BACK_OFF_DISTANCE 5.0 // in mm
-#define E_BACK_OFF_DISTANCE 2.0 // in mm
+#define XY_BACK_OFF_DISTANCE 5.0 // in mm
+#define Z_BACK_OFF_DISTANCE 2.0 // in mm
+#define E_BACK_OFF_DISTANCE 1.0 // in mm
 
 #define XY_HOME_BACK_OFF_TIME (XY_BACK_OFF_DISTANCE / XY_FAST_HOME_SPEED * 1000)
 #define Z_HOME_BACK_OFF_TIME (Z_BACK_OFF_DISTANCE / Z_FAST_HOME_SPEED * 1000)
 #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
+#define Z_AXIS_EPSILON 1.0
+#define E_AXIS_EPSILON 1.0
+
+#define ENCODER_RPM_VALUE_FACTOR 10.0
+
 #endif // _CONFIG_H_

include/config_pins.h

 
 #define LED_PIN            13
 
-#define FAN_PIN            9
+#define FAN_PIN            8
 
 #define PS_ON_PIN          12	//ATX , awake=LOW, SLEEP=High
 
 #define HEATER_0_PIN	10  // Extruder Heater
-#define HEATER_1_PIN	8
+#define HEATER_1_PIN	9
 
 #define TEMP_0_PIN		13   // ANALOG NUMBERING
 #define TEMP_1_PIN		14   // ANALOG NUMBERING

include/encoder.h

 void encoder_run(void);
 
 int encoder_change(void);
+int encoder_rpm(void);
 int encoder_click(void);
 int kill_switch(void);
 

include/statemachine.h

 void array_insert_at_pos(Array<T, N> *arr, T value, size_t pos);
 
 struct StateMachineInput {
-    StateMachineInput(int c, int e, int k, int m)
-            : click(c), encoder(e), kill(k), motors_done(m) { };
+    StateMachineInput(int c, int e, int r, int k, int m)
+            : click(c), encoder(e), rpm(r), kill(k), motors_done(m) { };
 
     int click;
     int encoder;
+    int rpm;
     int kill;
     int motors_done;
 };
     void onEnter(EnterFuncPtr func);
     void whenIn(InFuncPtr func);
 
+    void updateText(void);
+
     virtual void enterState(void);
     virtual void inState(StateMachineInput smi);
 
     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;
+
 #endif // _STATES_H_

include/steppers.h

 bool steppers_run(void);
 
 bool steppers_homed(void);
+char steppers_homing_axis(void);
 void steppers_start_homing(void);
 
+float steppers_current_pos(int axis);
+
+// target pos
 float steppers_pos_x(void);
 float steppers_pos_y(void);
 float steppers_pos_z(void);
 void steppers_set_accel_z(float accel);
 void steppers_set_accel_e(float accel);
 
+bool steppers_home_switch(int axis);
+
 #endif // _STEPPERS_H_

platformio.ini

     https://github.com/waspinator/AccelStepper
     https://github.com/mathertel/RotaryEncoder
     https://github.com/janelia-arduino/Array
+    https://github.com/PaulStoffregen/TimerOne
 
 [env:ramps_lcd12864]
 platform = atmelavr
     https://github.com/waspinator/AccelStepper
     https://github.com/mathertel/RotaryEncoder
     https://github.com/janelia-arduino/Array
+    https://github.com/PaulStoffregen/TimerOne

src/debounce.cpp

             if (currentState == LOW) {
                 ret = 1;
             }
+
+            /*
+            Serial.print("Debounce: ");
+            Serial.print(pin);
+            Serial.print(" = ");
+            Serial.println(state);
+            */
         }
     }
 

src/encoder.cpp

     return diff;
 }
 
+int encoder_rpm(void) {
+    return encoder.getRPM();
+}
+
 int encoder_click(void) {
     int r = click_state;
 

src/lcd.cpp

 
 #include <U8g2lib.h>
 
+//#define ENABLE_DYNAMIC_SCROLLING
+
 #define LCD_HEADING_LINES 1
 #define LCD_HEADING_WIDTH 15
 #define LCD_TEXT_LINES 5
     last_scroll_time = millis();
 }
 
+#ifdef ENABLE_DYNAMIC_SCROLLING
+
 void lcd_scroll_multiline(String s[], int n, int w, String &pre, String &post, int &dir) {
     if (dir == 0) {
         // switch directions if done scrolling in current direction
     }
 }
 
+#endif // ENABLE_DYNAMIC_SCROLLING
+
 int lcd_text_lines(void) {
     return LCD_TEXT_LINES;
 }
 }
 
 void lcd_loop(void) {
+#ifdef ENABLE_DYNAMIC_SCROLLING
     if ((millis() - last_scroll_time) >= SCROLL_DELAY) {
         last_scroll_time = millis();
         lcd_scrolling();
     }
+#endif // ENABLE_DYNAMIC_SCROLLING
 
 #ifdef USE_FULL_GRAPHIC_LCD
     if (redraw) {

src/main.cpp

 #include <Arduino.h>
+#include <TimerOne.h>
 
 #include "config.h"
 #include "config_pins.h"
 #include "steppers.h"
 #include "states.h"
 
-void setup() {
-    pinMode(LED_PIN, OUTPUT);
-    digitalWrite(LED_PIN, HIGH);
-
-    pinMode(BEEPER, OUTPUT);
-    blocking_beep(100, 1000);
-    
-    Serial.begin(115200);
-    Serial.println(F("Initializing Fuellfix v2"));
-    Serial.print(F("Version: "));
-    Serial.println(FIRMWARE_VERSION);
-
-    Serial.println(F("Init data"));
-    data_init();
-
-    Serial.println(F("Init encoder"));
-    encoder_init();
-
-    Serial.println(F("Init LCD"));
-    lcd_init();
-
-    Serial.println(F("Init stepper motors"));
-    steppers_init();
-
-    // ----------------------------------
+static volatile bool still_running = false;
 
+void print_data(void) {
     Serial.println(F("XY:"));
 
     Serial.print(F("\t X: "));
     Serial.println(F(" percent/s^2"));
 
     Serial.println();
+}
 
-    // ----------------------------------
+void fast_loop(void) {
+    still_running = steppers_run();
+    encoder_run();
+}
+
+void setup() {
+    pinMode(LED_PIN, OUTPUT);
+    digitalWrite(LED_PIN, HIGH);
+
+    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(FIRMWARE_VERSION);
+
+    Serial.println(F("Init data"));
+    data_init();
+
+    Serial.println(F("Init encoder"));
+    encoder_init();
+
+    Serial.println(F("Init LCD"));
+    lcd_init();
+
+    Serial.println(F("Init stepper motors"));
+    steppers_init();
+
+    print_data();
 
     Serial.println(F("ready, showing splash screen"));
     digitalWrite(LED_PIN, LOW);
 
     blocking_beep(100, 2000);
     Serial.println(F("starting main loop"));
+
+    Timer1.initialize(1000); // 1000us -> 1kHz
+    Timer1.attachInterrupt(fast_loop);
 }
 
 void loop() {
-    unsigned long t = millis();
-    common_run(t);
-    bool still_running = steppers_run();
-    encoder_run();
+    common_run(millis());
+
+    CLEAR_STORE_INTERRUPTS();
 
     int click = encoder_click();
+    int change = encoder_change();
+    int rpm = encoder_rpm();
     int kill = kill_switch();
+    bool motors_done = !still_running;
+
+    RESTORE_INTERRUPTS();
 
-    struct StateMachineInput smi(click, encoder_change(), kill, !still_running);
+    struct StateMachineInput smi(click, change, rpm, kill, motors_done);
     states_run(smi);
 
     lcd_loop();
+
+    delay(10);
 }

src/statemachine.cpp

     whenInFunc = func;
 }
 
-void StateText::enterState(void) {
-    if (onEnterFunc != NULL) {
-        onEnterFunc();
-    }
-
+void StateText::updateText(void) {
     lcd_clear();
     if (heading != NULL) {
         lcd_set_heading(heading);
     }
 }
 
+void StateText::enterState(void) {
+    if (onEnterFunc != NULL) {
+        onEnterFunc();
+    }
+
+    updateText();
+}
+
 void StateText::inState(struct StateMachineInput smi) {
     if (whenInFunc != NULL) {
         whenInFunc(smi);
 template <typename T>
 void StateValue<T>::inState(StateMachineInput smi) {
     if (smi.encoder != 0) {
-        value -= smi.encoder;
+        float vf = smi.encoder;
+        vf *= 1.0 + ((float)smi.rpm / ENCODER_RPM_VALUE_FACTOR);
+        int v = vf;
+        value -= v;
         if (value < min) {
             value = min;
         }
 template <typename T, size_t N>
 StateValues<T, N>::StateValues(State *_parent) : State(_parent) {
     heading = NULL;
+    onEnterFunc = NULL;
     updateFunc = NULL;
     updateLiveFunc = NULL;
     pos = 0;
 }
 
 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>::enterState(void) {
     pos = 0;
+    if (onEnterFunc != NULL) {
+        onEnterFunc();
+    }
     display();
 }
 
 void StateValues<T, N>::inState(StateMachineInput smi) {
     if (editing) {
         if (smi.encoder != 0) {
-            *(values[pos]) -= smi.encoder;
+            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) {
                         updateFunc(i, *(values[i]));
                     }
                 }
+
                 if (getChild() != NULL) {
                     states_go_to(getChild());
                 } else if (getParent() != NULL) {

src/states.cpp

 #include "statemachine.h"
 #include "states.h"
 
+//#define DISABLE_HOMING_STATE
+#define ENABLE_MOVEMENT_TEST_STATE
+#define ENABLE_INPUT_TEST_STATE
+
 // --------------------------------------
 
 StateText sm_ask_homing = StateText();
 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);
+
+#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);
+#endif // ENABLE_MOVEMENT_TEST_STATE
+
+#ifdef ENABLE_INPUT_TEST_STATE
+StateText sm_input_test = StateText(&sm_menu);
+#endif // ENABLE_INPUT_TEST_STATE
+
 // --------------------------------------
 
 State *current_state = NULL;
     sm_do_homing.setHeading("Homing...");
 
     sm_do_homing.onEnter([]() {
-        //steppers_start_homing();
+#ifndef DISABLE_HOMING_STATE
+        steppers_start_homing();
+#endif // DISABLE_HOMING_STATE
     });
 
     sm_do_homing.whenIn([](StateMachineInput smi) {
-        //if (smi.motors_done) {
-        //    if (steppers_homed()) {
+#ifndef DISABLE_HOMING_STATE
+        if (smi.motors_done) {
+            if (steppers_homed()) {
+#endif // DISABLE_HOMING_STATE
+
                 async_beep(HOMING_BEEP_TIME, HOMING_BEEP_FREQ);
                 states_go_to(&sm_menu);
-        //    }
-        //}
 
-        // TODO update text with current axis
+#ifndef DISABLE_HOMING_STATE
+            }
+        }
+#endif // DISABLE_HOMING_STATE
+
+        static char last_axis = ' ';
+        char axis = steppers_homing_axis();
+        if ((axis != ' ') && (axis != last_axis)) {
+            last_axis = axis;
+            strbuf = String(F("Currently homing ")) + axis + String(F(" axis"));
+            sm_do_homing.setText(strbuf.c_str());
+            sm_do_homing.updateText();
+        }
     });
 
     // ----------------------------------
 
     sm_auto.setTitle("Filling Menu");
 
+    // TODO
     sm_presets.setTitle("Use Preset");
     sm_presets.setPrefix("Preset ");
     sm_presets.dataCount([]() {
 
     sm_wiz_move.setTitle("Initial Movement");
     sm_wiz_move.onEnter([]() {
-        //steppers_start_homing();
+        steppers_start_homing();
     });
     sm_wiz_move.whenIn([](StateMachineInput smi) {
-        //if (smi.motors_done) {
-        //    if (steppers_homed()) {
+#ifndef DISABLE_HOMING_STATE
+        if (smi.motors_done) {
+            if (steppers_homed()) {
+#endif // DISABLE_HOMING_STATE
                 states_go_to(&sm_wiz_count);
-        //    }
-        //}
-
-        // TODO update text with current axis
+#ifndef DISABLE_HOMING_STATE
+            }
+        }
+#endif // DISABLE_HOMING_STATE
+
+        static char last_axis = ' ';
+        char axis = steppers_homing_axis();
+        if ((axis != ' ') && (axis != last_axis)) {
+            last_axis = axis;
+            strbuf = String(F("Currently homing ")) + axis + String(F(" axis"));
+            sm_wiz_move.setText(strbuf.c_str());
+            sm_wiz_move.updateText();
+        }
     });
 
     sm_wiz_count.setTitle("Container Count");
     sm_wiz_first_pos.setTitle("First Container Position");
     sm_wiz_first_pos.setData(0, "X: ", &wizard_first_x, X_AXIS_MIN, X_AXIS_MAX);
     sm_wiz_first_pos.setData(1, "Y: ", &wizard_first_y, Y_AXIS_MIN, Y_AXIS_MAX);
+    sm_wiz_first_pos.onEnter([]() {
+        wizard_first_x = steppers_current_pos(0);
+        wizard_first_y = steppers_current_pos(1);
+    });
     sm_wiz_first_pos.onLiveUpdate([](size_t i, float v) {
         if (i == 0) {
             steppers_move_x(v);
     sm_wiz_last_pos.setTitle("Last Container Position");
     sm_wiz_last_pos.setData(0, "X: ", &wizard_last_x, X_AXIS_MIN, X_AXIS_MAX);
     sm_wiz_last_pos.setData(1, "Y: ", &wizard_last_y, Y_AXIS_MIN, Y_AXIS_MAX);
+    sm_wiz_last_pos.onEnter([]() {
+        wizard_last_x = steppers_current_pos(0);
+        wizard_last_y = steppers_current_pos(1);
+    });
     sm_wiz_last_pos.onLiveUpdate([](size_t i, float v) {
         if (i == 0) {
             steppers_move_x(v);
         }
     });
 
+    // TODO
     sm_mod_preset.setTitle("Modify Preset");
     sm_mod_preset.setPrefix("Preset ");
     sm_mod_preset.dataCount([]() {
         return (int)data_preset_count();
     });
 
+    // TODO
     sm_del_preset.setTitle("Delete Preset");
     sm_del_preset.setPrefix("Preset ");
     sm_del_preset.dataCount([]() {
         steppers_set_accel_e(v);
     });
 
+    sm_error.setChild(&sm_ask_homing);
+    sm_error.setTitle("Axis Error");
+    sm_error.setHeading("Axis Error");
+    sm_error.setText("Endstop has been hit!");
+
+#ifdef ENABLE_MOVEMENT_TEST_STATE
+    sm_movement_test.setTitle("Movement Test");
+
+    sm_move_x_test.setTitle("X Axis");
+    sm_move_x_test.setHeading("X Move Test");
+    sm_move_x_test.whenIn([](StateMachineInput smi) {
+        static bool s = false;
+        if (smi.motors_done) {
+            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);
+            }
+        }
+    });
+
+    sm_move_y_test.setTitle("Y Axis");
+    sm_move_y_test.setHeading("Y Move Test");
+    sm_move_y_test.whenIn([](StateMachineInput smi) {
+        static bool s = false;
+        if (smi.motors_done) {
+            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);
+            }
+        }
+    });
+#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.whenIn([](StateMachineInput smi) {
+        String s = "Endstops: ";
+        for (int i = 0; i < 4; i++) {
+            if (steppers_home_switch(i)) {
+                s += '1';
+            } else {
+                s += '0';
+            }
+            if (i < 3) {
+                s += ' ';
+            }
+        }
+        if (s != strbuf) {
+            strbuf = s;
+            sm_input_test.setText(strbuf.c_str());
+            sm_input_test.updateText();
+        }
+    });
+#endif // ENABLE_INPUT_TEST_STATE
+
     // ----------------------------------
 
     states_go_to(&sm_ask_homing);

src/steppers.cpp

 
 #include "config.h"
 #include "config_pins.h"
+#include "common.h"
 #include "lcd.h"
 #include "data.h"
+#include "states.h"
 #include "steppers.h"
 
 static AccelStepper stepper_x(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN, 0, 0, false);
     step_homed
 };
 
-static stepper_states state = step_disabled;
-static unsigned long steppers_home_move_start_time = 0;
+static volatile stepper_states state = step_disabled;
+static volatile unsigned long steppers_home_move_start_time = 0;
 
 void steppers_init(void) {
     pinMode(X_ENDSTOP_PIN, INPUT_PULLUP);
     pinMode(E0_DIR_PIN, OUTPUT);
 
     stepper_x.setEnablePin(X_ENABLE_PIN);
-    stepper_x.setMaxSpeed(XY_MAX_SPEED * XY_STEPS_PER_MM);
+    stepper_x.setPinsInverted(false, false, true);
     steppers_set_speed_x(data_options()->speed_x);
     steppers_set_accel_x(data_options()->accel_x);
 
     stepper_y.setEnablePin(Y_ENABLE_PIN);
+    stepper_y.setPinsInverted(false, false, true);
     steppers_set_speed_y(data_options()->speed_y);
     steppers_set_accel_y(data_options()->accel_y);
 
     stepper_z.setEnablePin(Z_ENABLE_PIN);
+    stepper_z.setPinsInverted(false, false, true);
     steppers_set_speed_z(data_options()->speed_z);
     steppers_set_accel_z(data_options()->accel_z);
 
     stepper_e.setEnablePin(E0_ENABLE_PIN);
+    stepper_e.setPinsInverted(false, false, true);
     steppers_set_speed_e(data_options()->speed_e);
     steppers_set_accel_e(data_options()->accel_e);
 }
 
 static void steppers_initiate_home(int axis, int phase) {
+    Serial.print(F("steppers_initiate_home("));
+    if (axis == 0) {
+        Serial.print('X');
+    } else if (axis == 1) {
+        Serial.print('Y');
+    } else if (axis == 2) {
+        Serial.print('Z');
+    } else if (axis == 3) {
+        Serial.print('E');
+    } else {
+        Serial.print(axis);
+    }
+    Serial.print(F(", "));
+    Serial.print(phase);
+    Serial.println(F(")"));
+
+    CLEAR_STORE_INTERRUPTS();
+
     steppers_home_move_start_time = millis();
 
     if (axis == 0) {
     } else {
         Serial.println(F("home_init error: invalid axis"));
     }
+
+    RESTORE_INTERRUPTS();
 }
 
-static bool steppers_home_switch(int axis) {
+bool steppers_home_switch(int axis) {
+    bool r = true;
+    CLEAR_STORE_INTERRUPTS();
+
     if (axis == 0) {
-        return digitalRead(X_ENDSTOP_PIN) == LOW;
+        r = digitalRead(X_ENDSTOP_PIN) == LOW;
     } else if (axis == 1) {
-        return digitalRead(Y_ENDSTOP_PIN) == LOW;
+        r = digitalRead(Y_ENDSTOP_PIN) == LOW;
     } else if (axis == 2) {
-        return digitalRead(Z_ENDSTOP_PIN) == LOW;
+        r = digitalRead(Z_ENDSTOP_PIN) == LOW;
     } else if (axis == 3) {
-        return digitalRead(E_ENDSTOP_PIN) == LOW;
+        r = digitalRead(E_ENDSTOP_PIN) == LOW;
     } else {
         Serial.println(F("home_switch error: invalid axis"));
     }
-    return true;
+
+    RESTORE_INTERRUPTS();
+    return r;
+}
+
+float steppers_current_pos(int axis) {
+    float r = 0.0;
+    CLEAR_STORE_INTERRUPTS();
+
+    if (axis == 0) {
+        float v = stepper_x.currentPosition();
+        r = v / XY_STEPS_PER_MM / X_AXIS_MOVEMENT_DIR;
+    } else if (axis == 1) {
+        float v = stepper_y.currentPosition();
+        r = v / XY_STEPS_PER_MM / Y_AXIS_MOVEMENT_DIR;
+    } else if (axis == 2) {
+        float v = stepper_z.currentPosition();
+        r = v / Z_STEPS_PER_MM / Z_AXIS_MOVEMENT_DIR;
+    } else if (axis == 3) {
+        float v = stepper_e.currentPosition();
+        r = E_PERCENT_TO_MM(v / E_STEPS_PER_PERCENT / E_AXIS_MOVEMENT_DIR);
+    } else {
+        Serial.println(F("steppers_pos error: invalid axis"));
+    }
+
+    RESTORE_INTERRUPTS();
+    return r;
 }
 
 bool steppers_run(void) {
     } else if (state == step_homing_x_slow) {
         if (steppers_home_switch(0)) {
             stepper_x.setSpeed(0);
+
+#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
+
             steppers_initiate_home(1, 0);
         } else {
             stepper_x.runSpeed();
     } else if (state == step_homing_y_slow) {
         if (steppers_home_switch(1)) {
             stepper_y.setSpeed(0);
-            steppers_initiate_home(3, 0);
+
+#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
+
+            //steppers_initiate_home(3, 0);
+            // TODO
+            state = step_homed;
+            return false;
         } else {
             stepper_y.runSpeed();
         }
     } else if (state == step_homing_z_slow) {
         if (steppers_home_switch(2)) {
             stepper_z.setSpeed(0);
+
+#if (Z_MIN_PIN == -1)
+            stepper_z.setCurrentPosition(Z_AXIS_MAX * Z_STEPS_PER_MM * Z_AXIS_MOVEMENT_DIR);
+#else
+            stepper_z.setCurrentPosition(Z_AXIS_MIN * Z_STEPS_PER_MM * Z_AXIS_MOVEMENT_DIR);
+#endif
+
             steppers_initiate_home(0, 0);
         } else {
             stepper_z.runSpeed();
     } else if (state == step_homing_e_slow) {
         if (steppers_home_switch(3)) {
             stepper_e.setSpeed(0);
+
+#if (E_MIN_PIN == -1)
+            stepper_e.setCurrentPosition(E_MM_TO_PERCENT(E_AXIS_MAX) * E_STEPS_PER_PERCENT * E_AXIS_MOVEMENT_DIR);
+#else
+            stepper_e.setCurrentPosition(E_MM_TO_PERCENT(E_AXIS_MIN) * E_STEPS_PER_PERCENT * E_AXIS_MOVEMENT_DIR);
+#endif
+
             state = step_homed;
             return false;
         } else {
             stepper_e.runSpeed();
         }
     } else if (state != step_disabled) {
-        for (int i = 0; i < 4; i++) {
-            if (steppers_home_switch(i)) {
-                Serial.print(F("ERROR: endstop hit on "));
-                Serial.println(i);
-
-                // TODO proper error handling?
-                lcd_clear();
-                lcd_set_heading("ERROR");
-                lcd_set_text("Endstop hit. Aborting!");
-                while (1) { }
+        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();
 }
 
 bool steppers_homed(void) {
-    return (state == step_homed);
+    CLEAR_STORE_INTERRUPTS();
+    bool r = (state == step_homed);
+    RESTORE_INTERRUPTS();
+    return r;
+}
+
+char steppers_homing_axis(void) {
+    CLEAR_STORE_INTERRUPTS();
+
+    char r = ' ';
+    if ((state == step_homing_x_fast) || (state == step_homing_x_back) || (state == step_homing_x_slow)) {
+        r = 'X';
+    } else if ((state == step_homing_y_fast) || (state == step_homing_y_back) || (state == step_homing_y_slow)) {
+        r = 'Y';
+    } else if ((state == step_homing_z_fast) || (state == step_homing_z_back) || (state == step_homing_z_slow)) {
+        r = 'Z';
+    } else if ((state == step_homing_e_fast) || (state == step_homing_e_back) || (state == step_homing_e_slow)) {
+        r = 'E';
+    }
+
+    RESTORE_INTERRUPTS();
+    return r;
 }
 
 void steppers_start_homing(void) {
-    steppers_initiate_home(2, 0);
+    CLEAR_STORE_INTERRUPTS();
+
+    stepper_x.enableOutputs();
+    stepper_y.enableOutputs();
+    stepper_z.enableOutputs();
+    stepper_e.enableOutputs();
+
+    //steppers_initiate_home(2, 0);
+    // TODO
+    steppers_initiate_home(0, 0);
+
+    RESTORE_INTERRUPTS();
 }
 
 static int steppers_move_axis(AccelStepper &axis, long pos) {
+    CLEAR_STORE_INTERRUPTS();
+
     if (state == step_disabled) {
         Serial.println(F("Enabling stepper drivers"));
 
     }
 
     axis.moveTo(pos);
+
+    RESTORE_INTERRUPTS();
     return 0;
 }
 
 }
 
 float steppers_pos_x(void) {
+    CLEAR_STORE_INTERRUPTS();
     float v = stepper_x.targetPosition();
+    RESTORE_INTERRUPTS();
     return v / XY_STEPS_PER_MM / X_AXIS_MOVEMENT_DIR;
 }
 
 float steppers_pos_y(void) {
+    CLEAR_STORE_INTERRUPTS();
     float v = stepper_y.targetPosition();
+    RESTORE_INTERRUPTS();
     return v / XY_STEPS_PER_MM / Y_AXIS_MOVEMENT_DIR;
 }
 
 float steppers_pos_z(void) {
+    CLEAR_STORE_INTERRUPTS();
     float v = stepper_z.targetPosition();
+    RESTORE_INTERRUPTS();
     return v / Z_STEPS_PER_MM / Z_AXIS_MOVEMENT_DIR;
 }
 
 float steppers_pos_e(void) {
+    CLEAR_STORE_INTERRUPTS();
     float v = stepper_e.targetPosition();
+    RESTORE_INTERRUPTS();
     return E_PERCENT_TO_MM(v / E_STEPS_PER_PERCENT / E_AXIS_MOVEMENT_DIR);
 }
 
 void steppers_kill(void) {
+    CLEAR_STORE_INTERRUPTS();
+
     stepper_x.setCurrentPosition(0);
     stepper_y.setCurrentPosition(0);
     stepper_z.setCurrentPosition(0);
     stepper_e.disableOutputs();
 
     state = step_not_homed;
+
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_speed_x(float speed) {
     if (speed > XY_MAX_SPEED) {
         speed = XY_MAX_SPEED;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_x.setMaxSpeed(speed * XY_STEPS_PER_MM);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_speed_y(float speed) {
     if (speed > XY_MAX_SPEED) {
         speed = XY_MAX_SPEED;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_y.setMaxSpeed(speed * XY_STEPS_PER_MM);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_speed_z(float speed) {
     if (speed > Z_MAX_SPEED) {
         speed = Z_MAX_SPEED;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_z.setMaxSpeed(speed * Z_STEPS_PER_MM);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_speed_e(float speed) {
     if (speed > E_MAX_SPEED) {
         speed = E_MAX_SPEED;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_e.setMaxSpeed(speed * E_STEPS_PER_PERCENT);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_accel_x(float accel) {
     if (accel > XY_MAX_ACCEL) {
         accel = XY_MAX_ACCEL;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_x.setAcceleration(accel * XY_STEPS_PER_MM);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_accel_y(float accel) {
     if (accel > XY_MAX_ACCEL) {
         accel = XY_MAX_ACCEL;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_y.setAcceleration(accel * XY_STEPS_PER_MM);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_accel_z(float accel) {
     if (accel > Z_MAX_ACCEL) {
         accel = Z_MAX_ACCEL;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_z.setAcceleration(accel * Z_STEPS_PER_MM);
+    RESTORE_INTERRUPTS();
 }
 
 void steppers_set_accel_e(float accel) {
     if (accel > E_MAX_ACCEL) {
         accel = E_MAX_ACCEL;
     }
+
+    CLEAR_STORE_INTERRUPTS();
     stepper_e.setAcceleration(accel * E_STEPS_PER_PERCENT);
+    RESTORE_INTERRUPTS();
 }