diff --git a/firmware_rumba_esp/MSerial.ino b/firmware_rumba_esp/MSerial.ino
new file mode 100644
index 0000000..fd8e9c6
--- /dev/null
+++ b/firmware_rumba_esp/MSerial.ino
@@ -0,0 +1,50 @@
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+/*
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+
+void serial_setup(long baud) {
+  UBRR0H = ( baud & 0xFF00) >> 8; // Load upper 8-bits of the baud rate value into the high byte of the UBRR register
+  UBRR0L = ( baud & 0x00FF); // Load lower 8-bits of the baud rate value into the low byte of the UBRR register
+  UCSR0C |= (1 << UCSZ00) | (1 << UCSZ10); // Use 8-bit character sizes
+  //UCSR0B |= (1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0);   // Turn on the transmission, reception, and Receive interrupt
+  UCSR0B |= (1 << RXEN0);
+  interrupts();
+}
+
+
+// serial receive interrupt
+ISR(USART0_RX_vect) {
+}
+*/
+
+
+/**
+ * This file is part of makelangelo-firmware.
+ *
+ * makelangelo-firmware is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * makelangelo-firmware is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with makelangelo-firmware.  If not, see <http://www.gnu.org/licenses/>.
+ */
diff --git a/firmware_rumba_esp/Vector3.h b/firmware_rumba_esp/Vector3.h
new file mode 100644
index 0000000..67abc01
--- /dev/null
+++ b/firmware_rumba_esp/Vector3.h
@@ -0,0 +1,310 @@
+#ifndef VECTOR3_H
+#define VECTOR3_H
+//------------------------------------------------------------------------------
+// makelangelo-firmware - Supports RUMBA 6-axis motor controller
+// dan@marginallycelver.com 2014-01-07
+//------------------------------------------------------------------------------
+// Copyright at end of file.
+// please see http://www.github.com/MarginallyClever/DeltaRobotv8 for more information.
+
+
+#include "Arduino.h"
+
+
+class Vector3 {
+public:
+  // these usions allow the Vector3 to be used as a color component
+  float x;
+  float y;
+  float z;
+
+public:
+  Vector3() {}
+
+
+  Vector3( float xx, float yy, float zz ) {
+    x = xx;
+    y = yy;
+    z = zz;
+  }
+
+
+  Vector3( float v[ 3 ] ) {
+    x = v[ 0 ];
+    y = v[ 1 ];
+    z = v[ 2 ];
+  }
+
+
+  ~Vector3() {};
+
+
+  Vector3 &MakeZero() {
+    x=0;
+    y=0;
+    z=0;
+
+    return *this;
+  }
+
+
+  Vector3 &Set( float xx, float yy, float zz ) {
+    x = xx;
+    y = yy;
+    z = zz;
+
+    return *this;
+  }
+
+
+  Vector3 operator + () const { // Unary negation
+    return Vector3(*this);
+  }
+
+
+  Vector3 operator - () const { // Unary negation
+    return Vector3( -x, -y, -z );
+  }
+
+
+  Vector3 operator *= ( float v ) { // assigned multiply by a float
+    x *= v;
+    y *= v;
+    z *= v;
+
+    return *this;
+  }
+
+
+  Vector3 operator /= ( float t ) { // assigned division by a float
+    float v;
+
+    if( t == 0.0f )
+      v = 0;
+    else
+      v = 1.0f / t;
+
+    x *= v;
+    y *= v;
+    z *= v;
+
+    return *this;
+  }
+
+
+  Vector3 operator -= ( const Vector3 &v ) { // assigned subtraction
+    x -= v.x;
+    y -= v.y;
+    z -= v.z;
+
+    return *this;
+  }
+
+
+  Vector3 operator += ( const Vector3 &v ) { // assigned addition
+    x += v.x;
+    y += v.y;
+    z += v.z;
+
+    return *this;
+  }
+
+
+  Vector3 operator *= ( const Vector3 &v ) { // assigned mult.
+    x *= v.x;
+    y *= v.y;
+    z *= v.z;
+
+    return *this;
+  }
+
+
+  Vector3 operator ^= ( const Vector3 &v ) { // assigned cross product
+    float nx, ny, nz;
+
+    nx = ( y * v.z - z * v.y );
+    ny =-( x * v.z - z * v.x );
+    nz = ( x * v.y - y * v.x );
+    x = nx;
+    y = ny;
+    z = nz;
+
+    return *this;
+  }
+
+/*
+  bool operator == ( const Vector3 &v ) const {
+    return ( abs( x - v.x ) < 0.01f &&
+             abs( y - v.y ) < 0.01f &&
+             abs( z - v.z ) < 0.01f );
+  }*/
+
+/*
+  bool operator != ( const Vector3 &v ) const {
+    return ( abs( x - v.x ) > 0.01f ||
+             abs( y - v.y ) > 0.01f ||
+             abs( z - v.z ) > 0.01f );
+  }*/
+
+
+  // METHODS
+  float Length() const {
+    return (float)sqrt( x*x+y*y+z*z );
+  }
+
+
+  float LengthSquared() const {
+    return x*x+y*y+z*z;
+  }
+
+
+  void Normalize() {
+    float len, iLen;
+
+    len = Length();
+    if( !len ) iLen = 0;
+    else iLen = 1.0f / len;
+
+    x *= iLen;
+    y *= iLen;
+    z *= iLen;
+  }
+
+
+  float NormalizeLength() {
+    float len, iLen;
+
+    len = Length();
+    if( !len ) iLen = 0;
+    else iLen = 1.0f / len;
+
+    x *= iLen;
+    y *= iLen;
+    z *= iLen;
+
+    return len;
+  }
+
+
+  void ClampMin( float min ) { // Clamp to minimum
+    if( x < min ) x = min;
+    if( y < min ) y = min;
+    if( z < min ) z = min;
+  }
+
+
+  void ClampMax( float max ) { // Clamp to maximum
+    if( x > max ) x = max;
+    if( y > max ) y = max;
+    if( z > max ) z = max;
+  }
+
+
+  void Clamp( float min, float max ) { // Clamp to range ]min,max[
+    ClampMin( min );
+    ClampMax( max );
+  }
+
+
+  // Interpolate between *this and v
+  void Interpolate( const Vector3 &v, float a ) {
+    float b( 1.0f - a );
+
+    x = b * x + a * v.x;
+    y = b * y + a * v.y;
+    z = b * z + a * v.z;
+  }
+
+
+  float operator | ( const Vector3 &v ) const { // Dot product
+    return x * v.x + y * v.y + z * v.z;
+  }
+
+
+  Vector3 operator / ( float t ) const { // vector / float
+    if( t == 0.0f )
+      return Vector3( 0, 0, 0 );
+
+    float s( 1.0f / t );
+
+    return Vector3( x * s, y * s, z * s );
+  }
+
+
+  Vector3 operator + ( const Vector3 &b ) const { // vector + vector
+    return Vector3( x + b.x, y + b.y, z + b.z );
+  }
+
+
+  Vector3 operator - ( const Vector3 &b ) const { // vector - vector
+    return Vector3( x - b.x, y - b.y, z - b.z );
+  }
+
+
+  Vector3 operator * ( const Vector3 &b ) const { // vector * vector
+    return Vector3( x * b.x, y * b.y, z * b.z );
+  }
+
+
+  Vector3 operator ^ ( const Vector3 &b ) const { // cross(a,b)
+    float nx, ny, nz;
+
+    nx = y * b.z - z * b.y;
+    ny = z * b.x - x * b.z;
+    nz = x * b.y - y * b.x;
+
+    return Vector3( nx, ny, nz );
+  }
+
+
+  Vector3 operator * ( float s ) const {
+    return Vector3( x * s, y * s, z * s );
+  }
+
+/*
+  void Rotate( Vector3 &axis, float angle ) {
+    float sa = (float)sin( angle );
+    float ca = (float)cos( angle );
+    Vector3 axis2( axis );
+    float m[9];
+
+    axis2.Normalize();
+
+    m[ 0 ] = ca + (1 - ca) * axis2.x * axis2.x;
+    m[ 1 ] = (1 - ca) * axis2.x * axis2.y - sa * axis2.z;
+    m[ 2 ] = (1 - ca) * axis2.z * axis2.x + sa * axis2.y;
+    m[ 3 ] = (1 - ca) * axis2.x * axis2.y + sa * axis2.z;
+    m[ 4 ] = ca + (1 - ca) * axis2.y * axis2.y;
+    m[ 5 ] = (1 - ca) * axis2.y * axis2.z - sa * axis2.x;
+    m[ 6 ] = (1 - ca) * axis2.z * axis2.x - sa * axis2.y;
+    m[ 7 ] = (1 - ca) * axis2.y * axis2.z + sa * axis2.x;
+    m[ 8 ] = ca + (1 - ca) * axis2.z * axis2.z;
+
+    Vector3 src( *this );
+
+    x = m[0] * src.x + m[1] * src.y + m[2] * src.z;
+    y = m[3] * src.x + m[4] * src.y + m[5] * src.z;
+    z = m[6] * src.x + m[7] * src.y + m[8] * src.z;
+  }*/
+};
+
+
+
+/**
+* This file is part of makelangelo-firmware.
+*
+* makelangelo-firmware is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* makelangelo-firmware is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with makelangelo-firmware. If not, see <http://www.gnu.org/licenses/>.
+*/
+#endif
+
diff --git a/firmware_rumba_esp/configure.h b/firmware_rumba_esp/configure.h
new file mode 100644
index 0000000..4222e9d
--- /dev/null
+++ b/firmware_rumba_esp/configure.h
@@ -0,0 +1,325 @@
+#ifndef CONFIGURE_H
+#define CONFIGURE_H
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+
+
+//------------------------------------------------------------------------------
+// Sanity check
+//------------------------------------------------------------------------------
+#if defined(__AVR_ATmega328P__)
+// wrong board type set
+#error This code is not meant for Arduino UNO boards.
+#endif
+
+
+//------------------------------------------------------------------------------
+// CONSTANTS
+//------------------------------------------------------------------------------
+//#define MAKELANGELO_HARDWARE_VERSION 3  // If you have a makelangelo 3+
+#define MAKELANGELO_HARDWARE_VERSION 5  // If you have a makelangelo 5+
+
+#define VERBOSE           (5)  // add to get a lot more serial output.
+
+// machine style
+//#define POLARGRAPH2  // uncomment this line if you use a polargraph like the Makelangelo
+//#define COREXY  // uncomment this line if you use a CoreXY setup.
+#define TRADITIONALXY  // uncomment this line if you use a traditional XY setup.
+//#define ZARPLOTTER  // uncomment this line if you use a 4 motor ZAR plotter
+
+// servo angles for pen control
+#define PEN_UP_ANGLE         (90)
+#define PEN_DOWN_ANGLE       (10)  // Some steppers don't like 0 degrees
+
+// for serial comms
+#define BAUD                 (115200)  // How fast is the Arduino talking?
+#define MAX_BUF              (64)  // What is the longest message Arduino can store?
+
+
+#define MICROSTEPS           (16.0)  // microstepping on this microcontroller
+#define STEPS_PER_TURN       (200 * MICROSTEPS)  // default number of steps per turn * microsteps
+
+#define MAX_FEEDRATE         (100000.0)  // depends on timer interrupt & hardware
+#define MIN_FEEDRATE         (100)
+#define MAX_JERK             (5.0)
+#define DEFAULT_FEEDRATE     (7000.0)  // mm/min? /60 to get mm/sec
+#define DEFAULT_ACCELERATION (700)     // mm/s^2
+
+#define STEP_DELAY           (50)  // delay between steps, in milliseconds, when doing fixed tasks like homing
+
+#define NUM_AXIES            (3)  // x,y,z
+#define NUM_TOOLS            (6)
+#define MAX_SEGMENTS         (32)  // number of line segments to buffer ahead. must be a power of two.
+#define SEGMOD(x)            ((x)&(MAX_SEGMENTS-1))
+
+// for arc directions
+#define ARC_CW               (1)
+#define ARC_CCW              (-1)
+#define SEGMENT_PER_CM_LINE  (2)  // lines are split into segments.  How long are the segments?
+#define SEGMENT_PER_CM_ARC   (3)  // Arcs are split into segments.  How long are the segments?
+
+
+#ifdef HAS_LCD
+#define HAS_SD
+#endif
+
+// SD card settings
+#define SDPOWER            -1
+#define SDSS               53
+#define SDCARDDETECT       49
+
+#define LCD_HEIGHT         4
+#define LCD_WIDTH          20
+
+#define BLEN_C             2
+#define BLEN_B             1
+#define BLEN_A             0
+#define encrot0            0
+#define encrot1            2
+#define encrot2            3
+#define encrot3            1
+
+// Board types.  Don't change this!
+#define BOARD_RUMBA 1
+#define BOARD_RAMPS 2
+#define BOARD_SANGUINOLULU 3
+
+
+#if MAKELANGELO_HARDWARE_VERSION == 5
+#define MOTHERBOARD BOARD_RUMBA
+//#define USE_LIMIT_SWITCH    (1)  // Comment out this line to disable findHome and limit switches
+//#define HAS_SD                   // comment this out if there is no SD card
+//#define HAS_LCD                  // comment this out if there is no SMART LCD controller
+#endif
+#if MAKELANGELO_HARDWARE_VERSION == 3
+#define MOTHERBOARD BOARD_RUMBA
+#define HAS_SD                   // comment this out if there is no SD card
+#define HAS_LCD                  // comment this out if there is no SMART LCD controller
+#endif
+
+
+// Your choice of board
+//#define MOTHERBOARD BOARD_RUMBA
+//#define MOTHERBOARD BOARD_RAMPS
+//#define MOTHERBOARD BOARD_SANGUINOLULU
+
+#if MOTHERBOARD == BOARD_RUMBA
+#define MOTOR_0_DIR_PIN           (D13)
+#define MOTOR_0_STEP_PIN          (D2) // ESP D2 pin
+#define MOTOR_0_ENABLE_PIN        (D8)
+#define MOTOR_0_LIMIT_SWITCH_PIN  (33)
+
+#define MOTOR_1_DIR_PIN           (D12)
+#define MOTOR_1_STEP_PIN          (D15)
+#define MOTOR_1_ENABLE_PIN        (D8)
+#define MOTOR_1_LIMIT_SWITCH_PIN  (33)
+
+// alternate pins in case you want to do something interesting
+#define MOTOR_2_DIR_PIN           (56)
+#define MOTOR_2_STEP_PIN          (57)
+#define MOTOR_2_ENABLE_PIN        (62)
+#define MOTOR_2_LIMIT_SWITCH_PIN  (35)
+
+#define MOTOR_3_DIR_PIN           (22)
+#define MOTOR_3_STEP_PIN          (23)
+#define MOTOR_3_ENABLE_PIN        (24)
+#define MOTOR_3_LIMIT_SWITCH_PIN  (34)
+
+#define MOTOR_4_DIR_PIN           (25)
+#define MOTOR_4_STEP_PIN          (26)
+#define MOTOR_4_ENABLE_PIN        (27)
+#define MOTOR_4_LIMIT_SWITCH_PIN  (33)
+
+#define MOTOR_5_DIR_PIN           (28)
+#define MOTOR_5_STEP_PIN          (29)
+#define MOTOR_5_ENABLE_PIN        (39)
+#define MOTOR_5_LIMIT_SWITCH_PIN  (32)
+
+#define NUM_SERVOS                (1)
+#define SERVO0_PIN                (5)
+
+#define LIMIT_SWITCH_PIN_LEFT     (MOTOR_0_LIMIT_SWITCH_PIN)
+#define LIMIT_SWITCH_PIN_RIGHT    (MOTOR_1_LIMIT_SWITCH_PIN)
+
+// Smart controller settings
+#define BEEPER             44
+#define LCD_PINS_RS        19
+#define LCD_PINS_ENABLE    42
+#define LCD_PINS_D4        18
+#define LCD_PINS_D5        38
+#define LCD_PINS_D6        41
+#define LCD_PINS_D7        40
+
+// Encoder rotation values
+#define BTN_EN1            11
+#define BTN_EN2            12
+#define BTN_ENC            43
+
+#endif
+
+#if MOTHERBOARD == BOARD_RAMPS
+#define MOTOR_0_DIR_PIN           (55)
+#define MOTOR_0_STEP_PIN          (54)
+#define MOTOR_0_ENABLE_PIN        (38)
+#define MOTOR_0_LIMIT_SWITCH_PIN  (3)   /* X min */
+
+#define MOTOR_1_DIR_PIN           (61)
+#define MOTOR_1_STEP_PIN          (60)
+#define MOTOR_1_ENABLE_PIN        (56)
+#define MOTOR_1_LIMIT_SWITCH_PIN  (14)  /* Y min */
+
+// alternate pins in case you want to do something interesting
+#define MOTOR_2_DIR_PIN           (48)
+#define MOTOR_2_STEP_PIN          (46)
+#define MOTOR_2_ENABLE_PIN        (62)
+#define MOTOR_2_LIMIT_SWITCH_PIN  (18)  /* Z Min */
+
+#define MOTOR_3_DIR_PIN           (28)
+#define MOTOR_3_STEP_PIN          (26)
+#define MOTOR_3_ENABLE_PIN        (24)
+#define MOTOR_3_LIMIT_SWITCH_PIN  (2)   /* X Max */
+
+#define MOTOR_4_DIR_PIN           (34)
+#define MOTOR_4_STEP_PIN          (36)
+#define MOTOR_4_ENABLE_PIN        (30)
+#define MOTOR_4_LIMIT_SWITCH_PIN  (15)  /* Y Max */
+
+#define NUM_SERVOS         (4)
+#define SERVO0_PIN         (11)   /* Servo 1 */
+#define SERVO1_PIN         (6)
+#define SERVO2_PIN         (5)
+#define SERVO3_PIN         (4)
+
+// Smart controller settings
+#define BEEPER             37   /* Pin on SMART Adapter */
+#define LCD_PINS_RS        16   /* Pin on SMART Adapter */
+#define LCD_PINS_ENABLE    17   /* Pin on SMART Adapter */
+#define LCD_PINS_D4        23   /* Pin on SMART Adapter */
+#define LCD_PINS_D5        25   /* Pin on SMART Adapter */
+#define LCD_PINS_D6        27   /* Pin on SMART Adapter */
+#define LCD_PINS_D7        29   /* Pin on SMART Adapter */
+
+// Encoder rotation values
+#define BTN_EN1            31   /* Pin on SMART Adapter */
+#define BTN_EN2            33   /* Pin on SMART Adapter */
+#define BTN_ENC            35  /* Pin on SMART Adapter */
+
+#define KILL_PIN    41    /* Pin on SMART Adapter */
+#endif
+
+#if MOTHERBOARD == BOARD_SANGUINOLULU
+#define MOTOR_0_DIR_PIN           (21)
+#define MOTOR_0_STEP_PIN          (15)
+#define MOTOR_0_ENABLE_PIN        (14)
+#define MOTOR_0_LIMIT_SWITCH_PIN  (18)
+
+#define MOTOR_1_DIR_PIN           (23)
+#define MOTOR_1_STEP_PIN          (22)
+#define MOTOR_1_ENABLE_PIN        (14)
+#define MOTOR_1_LIMIT_SWITCH_PIN  (19)
+
+// TODO: if ZARPLOTTER & SANGUINOLULU throw a compile error, not enough motors.
+
+#define NUM_SERVOS         (1)
+#define SERVO0_PIN         (12)
+#endif
+
+
+//------------------------------------------------------------------------------
+// EEPROM MEMORY MAP
+//------------------------------------------------------------------------------
+#define EEPROM_VERSION          7  // Increment EEPROM_VERSION when adding new variables
+#define ADDR_VERSION            0                          // 0..255 (1 byte)
+#define ADDR_UUID               (ADDR_VERSION+1)           // long - 4 bytes
+#define ADDR_PULLEY_DIA1        (ADDR_UUID+4)              // float - 4 bytes
+#define ADDR_PULLEY_DIA2        (ADDR_PULLEY_DIA1+4)       // float - 4 bytes
+#define ADDR_LEFT               (ADDR_PULLEY_DIA2+4)       // float - 4 bytes
+#define ADDR_RIGHT              (ADDR_LEFT+4)              // float - 4 bytes
+#define ADDR_TOP                (ADDR_RIGHT+4)             // float - 4 bytes
+#define ADDR_BOTTOM             (ADDR_TOP+4)               // float - 4 bytes
+#define ADDR_INVL               (ADDR_BOTTOM+4)            // bool - 1 byte
+#define ADDR_INVR               (ADDR_INVL+1)              // bool - 1 byte
+#define ADDR_HOMEX              (ADDR_INVR+1)              // float - 4 bytes
+#define ADDR_HOMEY              (ADDR_HOMEX+4)             // float - 4 bytes
+#define ADDR_CALIBRATION_LEFT   (ADDR_HOMEY+4)             // float - 4 bytes
+#define ADDR_CALIBRATION_RIGHT  (ADDR_CALIBRATION_LEFT+4)  // float - 4 bytes
+
+
+//------------------------------------------------------------------------------
+// TIMERS
+//------------------------------------------------------------------------------
+// for timer interrupt control
+#define CLOCK_FREQ            (16000000L)
+#define MAX_COUNTER           (65536L)
+// time passed with no instruction?  Make sure PC knows we are waiting.
+#define TIMEOUT_OK            (1000)
+
+// optimize code, please
+#define FORCE_INLINE         __attribute__((always_inline)) inline
+
+
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START  noInterrupts();
+  #define CRITICAL_SECTION_END    interrupts();
+#endif //CRITICAL_SECTION_START
+
+
+//------------------------------------------------------------------------------
+// STRUCTURES
+//------------------------------------------------------------------------------
+// for line()
+typedef struct {
+  long step_count;
+  long delta;  // number of steps to move
+  long absdelta;
+  int dir;
+  float delta_normalized;
+} Axis;
+
+
+typedef struct {
+  int step_pin;
+  int dir_pin;
+  int enable_pin;
+  int limit_switch_pin;
+  int limit_switch_state;
+  int reel_in;
+  int reel_out;
+} Motor;
+
+
+typedef struct {
+  Axis a[NUM_AXIES];
+  int steps_total;
+  int steps_taken;
+  int accel_until;
+  int decel_after;
+  unsigned short feed_rate_max;
+  unsigned short feed_rate_start;
+  unsigned short feed_rate_start_max;
+  unsigned short feed_rate_end;
+  char nominal_length_flag;
+  char recalculate_flag;
+  char busy;
+} Segment;
+
+
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+
+extern Segment line_segments[MAX_SEGMENTS];
+extern Segment *working_seg;
+extern volatile int current_segment;
+extern volatile int last_segment;
+extern float acceleration;
+extern Motor motors[NUM_AXIES];
+
+
+#endif // CONFIGURE_H
diff --git a/firmware_rumba_esp/eeprom.ino b/firmware_rumba_esp/eeprom.ino
new file mode 100644
index 0000000..37acc92
--- /dev/null
+++ b/firmware_rumba_esp/eeprom.ino
@@ -0,0 +1,183 @@
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+// Saving config
+#include <EEPROM.h>
+#include <Arduino.h>  // for type definitions
+
+
+//------------------------------------------------------------------------------
+// from http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1234477290/3
+void EEPROM_writeLong(int ee, long value) {
+  byte* p = (byte*)(void*)&value;
+  for (int i = 0; i < sizeof(value); i++)
+  EEPROM.write(ee++, *p++);
+}
+
+
+//------------------------------------------------------------------------------
+// from http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1234477290/3
+float EEPROM_readLong(int ee) {
+  long value = 0;
+  byte* p = (byte*)(void*)&value;
+  for (int i = 0; i < sizeof(value); i++)
+  *p++ = EEPROM.read(ee++);
+  return value;
+}
+
+
+//------------------------------------------------------------------------------
+char loadVersion() {
+  return 0; // no for ESP at the moment
+  return EEPROM.read(ADDR_VERSION);
+}
+
+
+//------------------------------------------------------------------------------
+void loadConfig() {
+  char versionNumber = loadVersion();
+  if( versionNumber != EEPROM_VERSION ) {
+    // If not the current EEPROM_VERSION or the EEPROM_VERSION is sullied (i.e. unknown data)
+    // Update the version number
+    EEPROM.write(ADDR_VERSION,EEPROM_VERSION);
+#if MAKELANGELO_HARDWARE_VERSION == 5
+  //  savePulleyDiameter();
+  //  saveCalibration();
+#endif
+  }
+  
+  // Retrieve stored configuration
+  robot_uid=EEPROM_readLong(ADDR_UUID);
+  adjustPulleyDiameter(10); //(float)EEPROM_readLong(ADDR_PULLEY_DIA1)/10000.0f);   //4 decimal places of percision is enough
+  loadDimensions();
+  loadInversions();
+  loadHome();
+  loadCalibration();
+}
+
+
+//------------------------------------------------------------------------------
+void saveUID() {
+  Serial.println(F("Saving UID."));
+  EEPROM_writeLong(ADDR_UUID,(long)robot_uid);
+}
+
+
+//------------------------------------------------------------------------------
+void savePulleyDiameter() {
+  EEPROM_writeLong(ADDR_PULLEY_DIA1,pulleyDiameter*10000);
+  //EEPROM_writeLong(ADDR_PULLEY_DIA2,pulleyDiameter*10000);
+}
+
+
+//------------------------------------------------------------------------------
+void saveDimensions() {
+  Serial.println(F("Saving dimensions."));
+  EEPROM_writeLong(ADDR_LEFT,limit_left*100);
+  EEPROM_writeLong(ADDR_RIGHT,limit_right*100);
+  EEPROM_writeLong(ADDR_TOP,limit_top*100);
+  EEPROM_writeLong(ADDR_BOTTOM,limit_bottom*100);
+}
+
+
+//------------------------------------------------------------------------------
+void loadDimensions() {
+  limit_left   = (float)EEPROM_readLong(ADDR_LEFT)/100.0f;
+  limit_right  = (float)EEPROM_readLong(ADDR_RIGHT)/100.0f;
+  limit_top    = (float)EEPROM_readLong(ADDR_TOP)/100.0f;
+  limit_bottom = (float)EEPROM_readLong(ADDR_BOTTOM)/100.0f;
+}
+
+
+//------------------------------------------------------------------------------
+void adjustDimensions(float newT,float newB,float newR,float newL) {
+  // round off
+  newT = floor(newT*100)/100.0f;
+  newB = floor(newB*100)/100.0f;
+  newR = floor(newR*100)/100.0f;
+  newL = floor(newL*100)/100.0f;
+
+  if( limit_top    != newT ||
+      limit_bottom != newB ||
+      limit_right  != newR ||
+      limit_left   != newL) {
+        limit_top=newT;
+        limit_bottom=newB;
+        limit_right=newR;
+        limit_left=newL;
+        saveDimensions();
+      }
+}
+
+
+//------------------------------------------------------------------------------
+void saveInversions() {
+  Serial.println(F("Saving inversions."));
+  EEPROM.write(ADDR_INVL,m1i>0?1:0);
+  EEPROM.write(ADDR_INVR,m2i>0?1:0);
+}
+
+
+//------------------------------------------------------------------------------
+void loadInversions() {
+  //Serial.println(F("Loading inversions."));
+  m1i = EEPROM.read(ADDR_INVL)>0?1:-1;
+  m2i = EEPROM.read(ADDR_INVR)>0?1:-1;
+  adjustInversions(m1i,m2i);
+}
+
+
+//------------------------------------------------------------------------------
+void saveHome() {
+  Serial.println(F("Saving home."));
+  EEPROM_writeLong(ADDR_HOMEX,homeX*100);
+  EEPROM_writeLong(ADDR_HOMEY,homeY*100);
+}
+
+
+//------------------------------------------------------------------------------
+void loadHome() {
+  homeX = (float)EEPROM_readLong(ADDR_HOMEX)/100.0f;
+  homeY = (float)EEPROM_readLong(ADDR_HOMEY)/100.0f;
+}
+
+
+//------------------------------------------------------------------------------
+void saveCalibration() {
+  Serial.println(F("Saving calibration."));
+  EEPROM_writeLong(ADDR_CALIBRATION_LEFT,calibrateLeft*100);
+  EEPROM_writeLong(ADDR_CALIBRATION_RIGHT,calibrateRight*100);
+}
+
+
+//------------------------------------------------------------------------------
+void loadCalibration() {
+  calibrateLeft = (float)EEPROM_readLong(ADDR_CALIBRATION_LEFT)/100.0f;
+  calibrateRight = (float)EEPROM_readLong(ADDR_CALIBRATION_RIGHT)/100.0f;
+}
+
+/**
+ * This file is part of makelangelo-firmware.
+ *
+ * makelangelo-firmware is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * makelangelo-firmware is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with makelangelo-firmware.  If not, see <http://www.gnu.org/licenses/>.
+ */
diff --git a/firmware_rumba_esp/firmware_rumba_esp.ino b/firmware_rumba_esp/firmware_rumba_esp.ino
new file mode 100644
index 0000000..cc38728
--- /dev/null
+++ b/firmware_rumba_esp/firmware_rumba_esp.ino
@@ -0,0 +1,1011 @@
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+#include "configure.h"
+
+//#include <SPI.h>  // pkm fix for Arduino 1.5
+
+#include "Vector3.h"
+
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+
+// robot UID
+int robot_uid=0;
+
+// plotter limits, relative to the center of the plotter.
+float limit_top = 0;  // distance to top of drawing area.
+float limit_bottom = 0;  // Distance to bottom of drawing area.
+float limit_right = 0;  // Distance to right of drawing area.
+float limit_left = 0;  // Distance to left of drawing area.
+
+static float homeX=0;
+static float homeY=0;
+
+// length of belt when weights hit limit switch
+float calibrateRight = 101.1;
+float calibrateLeft = 101.1;
+
+// what are the motors called?
+char m1d='L';
+char m2d='R';
+
+// motor inversions
+char m1i=1;
+char m2i=1;
+
+// calculate some numbers to help us find feed_rate
+float pulleyDiameter = 4.0f/PI;  // cm; 20 teeth * 2mm per tooth / PI
+float threadPerStep=4.0f/STEPS_PER_TURN;  // pulleyDiameter * PI / STEPS_PER_TURN
+
+// plotter position.
+float posx, posy, posz;  // pen state
+float feed_rate=DEFAULT_FEEDRATE;
+float acceleration=DEFAULT_ACCELERATION;
+float step_delay;
+
+char absolute_mode=1;  // absolute or incremental programming mode?
+
+// Serial comm reception
+char serialBuffer[MAX_BUF+1];  // Serial buffer
+int sofar;               // Serial buffer progress
+static long last_cmd_time;    // prevent timeouts
+
+
+Vector3 tool_offset[NUM_TOOLS];
+int current_tool=0;
+
+
+long line_number=0;
+
+
+extern long global_steps_0;
+extern long global_steps_1;
+
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+
+
+//------------------------------------------------------------------------------
+// calculate max velocity, threadperstep.
+void adjustPulleyDiameter(float diameter) {
+  Serial.print(F("adjustPulleyDiameter "));
+  Serial.println(diameter);
+  pulleyDiameter = diameter;
+  float circumference = pulleyDiameter*PI;  // circumference
+  threadPerStep = circumference/STEPS_PER_TURN;  // thread per step
+  Serial.print(" threadPerStep="); Serial.println( threadPerStep);
+}
+
+
+//------------------------------------------------------------------------------
+// returns angle of dy/dx as a value from 0...2PI
+float atan3(float dy,float dx) {
+  float a=atan2(dy,dx);
+  if(a<0) a=(PI*2.0)+a;
+  return a;
+}
+
+
+//------------------------------------------------------------------------------
+char readSwitches() {
+#ifdef USE_LIMIT_SWITCH
+  // get the current switch state
+  return ( (digitalRead(LIMIT_SWITCH_PIN_LEFT)==LOW) | (digitalRead(LIMIT_SWITCH_PIN_RIGHT)==LOW) );
+#else
+  return 0;
+#endif  // USE_LIMIT_SWITCH
+}
+
+
+//------------------------------------------------------------------------------
+// feed rate is given in units/min and converted to cm/s
+void setFeedRate(float v1) {
+  if( feed_rate != v1 ) {
+    feed_rate = v1;
+    if(feed_rate > MAX_FEEDRATE) feed_rate = MAX_FEEDRATE;
+    if(feed_rate < MIN_FEEDRATE) feed_rate = MIN_FEEDRATE;
+#ifdef VERBOSE
+    Serial.print(F("F="));
+    Serial.println(feed_rate);
+#endif
+  }
+}
+
+void findStepDelay() {
+  step_delay = 1000000.0f/DEFAULT_FEEDRATE;
+}
+
+
+//------------------------------------------------------------------------------
+// delay in microseconds
+void pause(long us) {
+  delay(us / 1000);
+  delayMicroseconds(us % 1000);
+}
+
+
+//------------------------------------------------------------------------------
+void printFeedRate() {
+  Serial.print(F("F"));
+  Serial.print(feed_rate);
+  Serial.print(F("steps/s"));
+}
+
+
+//------------------------------------------------------------------------------
+// Inverse Kinematics - turns XY coordinates into lengths L1,L2
+void IK(float x, float y, long &l1, long &l2) {
+  threadPerStep=1;
+#ifdef COREXY
+  l1 = lround((x+y) / threadPerStep);
+  l2 = lround((x-y) / threadPerStep);
+#endif
+#ifdef TRADITIONALXY
+  l1 = lround((x) / threadPerStep);
+  l2 = lround((y) / threadPerStep);
+#endif
+#ifdef POLARGRAPH2
+  // find length to M1
+  float dy = y - limit_top;
+  float dx = x - limit_left;
+  l1 = lround( sqrt(dx*dx+dy*dy) / threadPerStep );
+  // find length to M2
+  dx = limit_right - x;
+  l2 = lround( sqrt(dx*dx+dy*dy) / threadPerStep );
+#endif
+}
+
+
+//------------------------------------------------------------------------------
+// Forward Kinematics - turns L1,L2 lengths into XY coordinates
+// use law of cosines: theta = acos((a*a+b*b-c*c)/(2*a*b));
+// to find angle between M1M2 and M1P where P is the plotter position.
+void FK(long l1, long l2,float &x,float &y) {
+   threadPerStep=1;
+#ifdef COREXY
+  l1 *= threadPerStep;
+  l2 *= threadPerStep;
+
+  x = (float)( l1 + l2 ) / 2.0;
+  y = x - (float)l2;
+#endif
+#ifdef TRADITIONALXY
+  x = l1 * threadPerStep;
+  y = l2 * threadPerStep;
+#endif
+#ifdef POLARGRAPH2
+  float a = (float)l1 * threadPerStep;
+  float b = (limit_right-limit_left);
+  float c = (float)l2 * threadPerStep;
+
+  // slow, uses trig
+  // we know law of cosines:   cc = aa + bb -2ab * cos( theta )
+  // or cc - aa - bb = -2ab * cos( theta )
+  // or ( aa + bb - cc ) / ( 2ab ) = cos( theta );
+  // or theta = acos((aa+bb-cc)/(2ab));
+  //x = cos(theta)*l1 + limit_left;
+  //y = sin(theta)*l1 + limit_top;
+  // and we know that cos(acos(i)) = i
+  // and we know that sin(acos(i)) = sqrt(1-i*i)
+  float theta = ((a*a+b*b-c*c)/(2.0*a*b));
+  x = theta * a + limit_left;
+  y = limit_top - (sqrt( 1.0 - theta * theta ) * a);
+#endif
+}
+
+
+//------------------------------------------------------------------------------
+void processConfig() {
+  float newT = parseNumber('T',limit_top);
+  float newB = parseNumber('B',limit_bottom);
+  float newR = parseNumber('R',limit_right);
+  float newL = parseNumber('L',limit_left);
+
+  adjustDimensions(newT,newB,newR,newL);
+
+  // programmatically swap motors
+  char gg=parseNumber('G',m1d);
+  char hh=parseNumber('H',m2d);
+
+  // invert motor direction
+  char i=parseNumber('I',0);
+  char j=parseNumber('J',0);
+  
+  adjustInversions(i,j);
+  
+  // @TODO: check t>b, r>l ?
+  
+  printConfig();
+
+  teleport(posx,posy);
+}
+
+
+//------------------------------------------------------------------------------
+void adjustInversions(int m1,int m2) {
+  //Serial.print(F("Adjusting inversions to "));
+
+  if(m1>0) {
+    motors[0].reel_in  = HIGH;
+    motors[0].reel_out = LOW;
+  } else if(m1<0) {
+    motors[0].reel_in  = LOW;
+    motors[0].reel_out = HIGH;
+  }
+
+  if(m2>0) {
+    motors[1].reel_in  = HIGH;
+    motors[1].reel_out = LOW;
+  } else if(m2<0) {
+    motors[1].reel_in  = LOW;
+    motors[1].reel_out = HIGH;
+  }
+
+  if( m1!=m1i || m2 != m2i) {
+    // loadInversions() should never reach this point in the code.
+    m1i=m1;
+    m2i=m2;
+    saveInversions();
+  }
+}
+
+
+/**
+ * Test that IK(FK(A))=A
+ */
+void testKinematics() {
+  long A,B,i;
+  float C,D,x=0,y=0;
+
+  for(i=0;i<3000;++i) {
+    x = random(limit_right,limit_right)*0.1;
+    y = random(limit_bottom,limit_top)*0.1;
+
+    IK(x,y,A,B);
+    FK(A,B,C,D);
+    Serial.print(F("\tx="));  Serial.print(x);
+    Serial.print(F("\ty="));  Serial.print(y);
+    Serial.print(F("\tL="));  Serial.print(A);
+    Serial.print(F("\tR="));  Serial.print(B);
+    Serial.print(F("\tx'="));  Serial.print(C);
+    Serial.print(F("\ty'="));  Serial.print(D);
+    Serial.print(F("\tdx="));  Serial.print(C-x);
+    Serial.print(F("\tdy="));  Serial.println(D-y);
+  }
+}
+
+/**
+ * Translate the XYZ through the IK to get the number of motor steps and move the motors.
+ * @input x destination x value
+ * @input y destination y value
+ * @input z destination z value
+ * @input new_feed_rate speed to travel along arc
+ */
+void polargraph_line(float x,float y,float z,float new_feed_rate) {
+  long l1,l2;
+  IK(x,y,l1,l2);
+  posx=x;
+  posy=y;
+  posz=z;
+/*
+  Serial.print('~');
+  Serial.print(x);  Serial.print('\t');
+  Serial.print(y);  Serial.print('\t');
+  Serial.print(z);  Serial.print('\t');
+  Serial.print(l1);  Serial.print('\t');
+  Serial.print(l2);  Serial.print('\n');
+  */
+  feed_rate = new_feed_rate;
+  //Serial.print("l1="); Serial.println(l1);
+  motor_line(l1,l2,z,new_feed_rate);
+}
+
+
+/**
+ * Move the pen holder in a straight line using bresenham's algorithm
+ * @input x destination x value
+ * @input y destination y value
+ * @input z destination z value
+ * @input new_feed_rate speed to travel along arc
+ */
+void line_safe(float x,float y,float z,float new_feed_rate) {
+  x-=tool_offset[current_tool].x;
+  y-=tool_offset[current_tool].y;
+  z-=tool_offset[current_tool].z;
+
+#ifdef TRADITIONALXY
+  motor_line(x,y,z,new_feed_rate);
+  posx=x;
+  posy=y;
+  posz=z; if(VERBOSE>3) {Serial.print(x); Serial.print(","); Serial.println(y);}
+  return;
+#endif
+
+#ifdef POLARGRAPH2
+  // split up long lines to make them straighter?
+  Vector3 destination(x,y,z);
+  Vector3 startPoint(posx,posy,posz);
+  Vector3 dp = destination - startPoint;
+  Vector3 temp;
+
+  float len=dp.Length();
+  int pieces = ceil(dp.Length() * (float)SEGMENT_PER_CM_LINE );
+
+  float a;
+  long j;
+
+  // draw everything up to (but not including) the destination.
+  for(j=1;j<pieces;++j) {
+    a=(float)j/(float)pieces;
+    temp = dp * a + startPoint;
+    polargraph_line(temp.x,temp.y,temp.z,new_feed_rate);
+  }
+#endif  
+  // guarantee we stop exactly at the destination (no rounding errors).
+  polargraph_line(x,y,z,new_feed_rate);
+  /*
+  long l1,l2;
+  IK(x,y,l1,l2);
+  Serial.print(F("H0="));  Serial.print(l1);
+  Serial.print(F("\tH1="));  Serial.println(l2);
+  */
+}
+
+
+/**
+ * This method assumes the limits have already been checked.
+ * This method assumes the start and end radius match.
+ * This method assumes arcs are not >180 degrees (PI radians)
+ * @input cx center of circle x value
+ * @input cy center of circle y value
+ * @input x destination x value
+ * @input y destination y value
+ * @input z destination z value
+ * @input dir - ARC_CW or ARC_CCW to control direction of arc
+ * @input new_feed_rate speed to travel along arc
+ */
+void arc(float cx,float cy,float x,float y,float z,char clockwise,float new_feed_rate) {
+  // get radius
+  float dx = posx - cx;
+  float dy = posy - cy;
+  float sr=sqrt(dx*dx+dy*dy);
+
+  // find angle of arc (sweep)
+  float sa=atan3(dy,dx);
+  float ea=atan3(y-cy,x-cx);
+  float er=sqrt(dx*dx+dy*dy);
+  
+  float da=ea-sa;
+  if(clockwise!=0 && da<0) ea+=2*PI;
+  else if(clockwise==0 && da>0) sa+=2*PI;
+  da=ea-sa;
+  float dr = er-sr;
+
+  // get length of arc
+  // float circ=PI*2.0*radius;
+  // float len=theta*circ/(PI*2.0);
+  // simplifies to
+  float len1 = abs(da) * sr;
+  float len = sqrt( len1 * len1 + dr * dr );
+
+  int i, segments = ceil( len * SEGMENT_PER_CM_ARC );
+
+  float nx, ny, nz, angle3, scale;
+  float a,r;
+  for(i=0;i<=segments;++i) {
+    // interpolate around the arc
+    scale = ((float)i)/((float)segments);
+
+    a = ( da * scale ) + sa;
+    r = ( dr * scale ) + sr;
+    
+    nx = cx + cos(a) * r;
+    ny = cy + sin(a) * r;
+    nz = ( z - posz ) * scale + posz;
+    // send it to the planner
+    line_safe(nx,ny,nz,new_feed_rate);
+  }
+}
+
+
+/**
+ * Instantly move the virtual plotter position.  Does not check if the move is valid.
+ */
+void teleport(float x,float y) {
+  wait_for_empty_segment_buffer();
+  
+  posx=x;
+  posy=y;
+
+  // @TODO: posz?
+  long L1,L2;
+  IK(posx,posy,L1,L2);
+
+  motor_set_step_count(L1,L2,0);
+}
+
+
+/**
+ * Print a helpful message to serial.  The first line must never be changed to play nice with the JAVA software.
+ */
+void help() {
+  Serial.print(F("\n\nHELLO WORLD! I AM DRAWBOT #"));
+  Serial.println(robot_uid);
+  sayVersionNumber();
+  Serial.println(F("== http://www.makelangelo.com/ =="));
+  Serial.println(F("M100 - display this message"));
+  Serial.println(F("M101 [Tx.xx] [Bx.xx] [Rx.xx] [Lx.xx]"));
+  Serial.println(F("       - display/update board dimensions."));
+  Serial.println(F("As well as the following G-codes (http://en.wikipedia.org/wiki/G-code):"));
+  Serial.println(F("G00,G01,G02,G03,G04,G28,G90,G91,G92,M18,M114"));
+}
+
+
+void sayVersionNumber() {
+  char versionNumber = loadVersion();
+  
+  Serial.print(F("Firmware v"));
+  Serial.println(versionNumber,DEC);
+}
+
+/**
+ * Starting from the home position, bump the switches and measure the length of each belt.
+ * Does not save the values, only reports them to serial.
+ */
+void calibrateBelts() {
+#ifdef USE_LIMIT_SWITCH
+  wait_for_empty_segment_buffer();
+  
+  Serial.println(F("Searching for switches..."));
+
+  if(readSwitches()) {
+    Serial.println(F("** ERROR **"));
+    Serial.println(F("Problem: Plotter is already touching switches."));
+    Serial.println(F("Solution: Please unwind the strings a bit and try again."));
+    return;
+  }
+
+  int safeOut=50;
+
+  // reel in the left motor and the right motor out until contact is made.
+  Serial.println(F("Find switches..."));
+  digitalWrite(MOTOR_0_DIR_PIN,motors[0].reel_out);
+  digitalWrite(MOTOR_1_DIR_PIN,motors[1].reel_out);
+  int left=0, right=0;
+  long leftSteps, rightSteps;
+  
+  IK(homeX,homeY,leftSteps,rightSteps);
+  findStepDelay();
+
+  do {
+    if( digitalRead(LIMIT_SWITCH_PIN_LEFT )==LOW ) {
+      left=1;
+    }
+    if(left==0) {
+      digitalWrite(MOTOR_0_STEP_PIN,HIGH);
+      digitalWrite(MOTOR_0_STEP_PIN,LOW);
+      leftSteps++;
+    }
+    if( digitalRead(LIMIT_SWITCH_PIN_RIGHT )==LOW ) {
+      right=1;
+    }
+    if(right==0) {
+      digitalWrite(MOTOR_1_STEP_PIN,HIGH);
+      digitalWrite(MOTOR_1_STEP_PIN,LOW);
+      rightSteps++;
+    }
+    pause(step_delay);
+  } while(left+right<2);
+
+  // make sure there's no momentum to skip the belt on the pulley.
+  delay(500);
+  
+  Serial.println(F("Estimating position..."));
+  calibrateLeft = (float)leftSteps * threadPerStep;
+  calibrateRight = (float)rightSteps * threadPerStep;
+  Serial.print(F("D8 L"));  Serial.println(calibrateLeft);
+  Serial.print(F(" R"));  Serial.println(calibrateRight);
+  
+  // current position is...
+  float x,y;
+  FK(leftSteps,rightSteps,x,y);
+  teleport(x,y);
+  where();
+
+  // go home.
+  Serial.println(F("Homing..."));
+
+  Vector3 offset=get_end_plus_offset();
+  line_safe(homeX,homeY,offset.z,feed_rate);
+  Serial.println(F("Done."));
+#endif // USE_LIMIT_SWITCH
+}
+
+
+/**
+ * If limit switches are installed, move to touch each switch so that the pen holder can move to home position.
+ */
+void findHome() {
+#ifdef USE_LIMIT_SWITCH
+  wait_for_empty_segment_buffer();
+  
+  Serial.println(F("Find Home..."));
+
+  if(readSwitches()) {
+    Serial.println(F("** ERROR **"));
+    Serial.println(F("Problem: Plotter is already touching switches."));
+    Serial.println(F("Solution: Please unwind the strings a bit and try again."));
+    return;
+  }
+  
+  findStepDelay();
+
+  // reel in the left motor and the right motor out until contact is made.
+  digitalWrite(MOTOR_0_DIR_PIN,motors[0].reel_out);
+  digitalWrite(MOTOR_1_DIR_PIN,motors[1].reel_out);
+  int left=0, right=0;
+  do {
+    if(left==0) {
+      if( digitalRead(LIMIT_SWITCH_PIN_LEFT )==LOW ) {
+        left=1;
+        Serial.println(F("Left..."));
+      }
+      digitalWrite(MOTOR_0_STEP_PIN,HIGH);
+      digitalWrite(MOTOR_0_STEP_PIN,LOW);
+    }
+    if(right==0) {
+      if( digitalRead(LIMIT_SWITCH_PIN_RIGHT )==LOW ) {
+        right=1;
+        Serial.println(F("Right..."));
+      }
+      digitalWrite(MOTOR_1_STEP_PIN,HIGH);
+      digitalWrite(MOTOR_1_STEP_PIN,LOW);
+    }
+    pause(step_delay);
+  } while(left+right<2);
+
+  // make sure there's no momentum to skip the belt on the pulley.
+  delay(500);
+  
+  Serial.println(F("Estimating position..."));
+  float leftD = lround( calibrateLeft / threadPerStep );
+  float rightD = lround( calibrateRight / threadPerStep );
+  Serial.print("cl=");   Serial.println(calibrateLeft);
+  Serial.print("cr=");   Serial.println(calibrateRight);
+  Serial.print("t=");    Serial.println(threadPerStep);
+  Serial.print("l=");    Serial.println(leftD);
+  Serial.print("r=");    Serial.println(rightD);
+  
+  // current position is...
+  float x,y;
+  FK(leftD,rightD,x,y);
+  teleport(x,y);
+  where();
+
+  // go home.
+  Serial.println(F("Homing..."));
+
+  Vector3 offset=get_end_plus_offset();
+  line_safe(homeX,homeY,offset.z,feed_rate);
+  Serial.println(F("Done."));
+#endif // USE_LIMIT_SWITCH
+}
+
+
+/**
+ * Print the X,Y,Z, feedrate, and acceleration to serial.
+ * Equivalent to gcode M114
+ */
+void where() {
+  wait_for_empty_segment_buffer();
+
+  Serial.print(F("X"));   Serial.print(posx);
+  Serial.print(F(" Y"));  Serial.print(posy);
+  Serial.print(F(" Z"));  Serial.print(posz);
+  Serial.print(' ');  printFeedRate();
+  Serial.print(F(" A"));  Serial.println(acceleration);
+  
+  Serial.print(F(" HX="));  Serial.print(homeX);
+  Serial.print(F(" HY="));  Serial.println(homeY);
+  
+  //Serial.print(F(" G0="));  Serial.print(global_steps_0);
+  //Serial.print(F(" G1="));  Serial.print(global_steps_1);
+}
+
+
+/**
+ * Print the machine limits to serial.
+ */
+void printConfig() {
+  Serial.print(limit_left);       Serial.print(F(","));
+  Serial.print(limit_top);        Serial.print(F(" - "));
+  Serial.print(limit_right);     Serial.print(F(","));
+  Serial.print(limit_bottom);      Serial.print(F("\n"));
+}
+
+
+/**
+ * Set the relative tool offset
+ * @input axis the active tool id
+ * @input x the x offset
+ * @input y the y offset
+ * @input z the z offset
+ */
+void set_tool_offset(int axis,float x,float y,float z) {
+  tool_offset[axis].x=x;
+  tool_offset[axis].y=y;
+  tool_offset[axis].z=z;
+}
+
+
+/**
+ * @return the position + active tool offset
+ */
+Vector3 get_end_plus_offset() {
+  return Vector3(tool_offset[current_tool].x + posx,
+                 tool_offset[current_tool].y + posy,
+                 tool_offset[current_tool].z + posz);
+}
+
+
+/**
+ * Change the currently active tool
+ */
+void tool_change(int tool_id) {
+  if(tool_id < 0) tool_id=0;
+  if(tool_id >= NUM_TOOLS) tool_id=NUM_TOOLS-1;
+  current_tool=tool_id;
+}
+
+
+/**
+ * Look for character /code/ in the buffer and read the float that immediately follows it.
+ * @return the value found.  If nothing is found, /val/ is returned.
+ * @input code the character to look for.
+ * @input val the return value if /code/ is not found.
+ **/
+float parseNumber(char code,float val) {
+  char *ptr=serialBuffer;  // start at the beginning of buffer
+  while((long)ptr > 1 && (*ptr) && (long)ptr < (long)serialBuffer+sofar) {  // walk to the end
+    if(*ptr==code) {  // if you find code on your walk,
+      return atof(ptr+1);  // convert the digits that follow into a float and return it
+    }
+    ptr++; //=strchr(ptr,' ')+1;  // take a step from here to the letter after the next space
+  }
+  return val;  // end reached, nothing found, return default val.
+}
+
+
+/**
+ * process commands in the serial receive buffer
+ */
+void processCommand() {
+  // blank lines
+  if(serialBuffer[0]==';') return;
+
+  long cmd;
+
+  // is there a line number?
+  cmd=parseNumber('N',-1);
+  if(cmd!=-1 && serialBuffer[0]=='N') {  // line number must appear first on the line
+    if( cmd != line_number ) {
+      // wrong line number error
+      Serial.print(F("BADLINENUM "));
+      Serial.println(line_number);
+      return;
+    }
+
+    // is there a checksum?
+    if(strchr(serialBuffer,'*')!=0) {
+      // yes.  is it valid?
+      char checksum=0;
+      int c=0;
+      while(serialBuffer[c]!='*' && c<MAX_BUF) checksum ^= serialBuffer[c++];
+      c++; // skip *
+      int against = strtod(serialBuffer+c,NULL);
+      if( checksum != against ) {
+        Serial.print(F("BADCHECKSUM "));
+        Serial.println(line_number);
+        return;
+      }
+    } else {
+      Serial.print(F("NOCHECKSUM "));
+      Serial.println(line_number);
+      return;
+    }
+
+    line_number++;
+  }
+
+  if(!strncmp(serialBuffer,"UID",3)) {
+    robot_uid=atoi(strchr(serialBuffer,' ')+1);
+    saveUID();
+  }
+
+
+  cmd=parseNumber('M',-1);
+  switch(cmd) {
+  case 6:  tool_change(parseNumber('T',current_tool));  break;
+  case 17:  motor_engage();  break;
+  case 18:  motor_disengage();  break;
+  case 100:  help();  break;
+  case 101:  processConfig();  break;
+  case 110:  line_number = parseNumber('N',line_number);  break;
+  case 114:  where();  break;
+  default:  break;
+  }
+
+  cmd=parseNumber('G',-1);
+  switch(cmd) {
+  case 0:
+  case 1: {  // line
+      Vector3 offset=get_end_plus_offset();
+      acceleration = min(max(parseNumber('A',acceleration),1),200000);
+      line_safe( parseNumber('X',(absolute_mode?offset.x:0)) + (absolute_mode?0:offset.x),
+                 parseNumber('Y',(absolute_mode?offset.y:0)) + (absolute_mode?0:offset.y),
+                 parseNumber('Z',(absolute_mode?offset.z:0)) + (absolute_mode?0:offset.z),
+                 (feed_rate=parseNumber('F',feed_rate)) ); // update feedrate!!
+      //Serial.print(parseNumber('X',-1));
+      break;
+    }
+  case 2:
+  case 3: {  // arc
+      Vector3 offset=get_end_plus_offset();
+      acceleration = min(max(parseNumber('A',acceleration),1),200000);
+      setFeedRate(parseNumber('F',feed_rate));
+      arc(parseNumber('I',(absolute_mode?offset.x:0)) + (absolute_mode?0:offset.x),
+          parseNumber('J',(absolute_mode?offset.y:0)) + (absolute_mode?0:offset.y),
+          parseNumber('X',(absolute_mode?offset.x:0)) + (absolute_mode?0:offset.x),
+          parseNumber('Y',(absolute_mode?offset.y:0)) + (absolute_mode?0:offset.y),
+          parseNumber('Z',(absolute_mode?offset.z:0)) + (absolute_mode?0:offset.z),
+          (cmd==2) ? 1 : 0,
+          parseNumber('F',feed_rate) );
+      break;
+    }
+  case 4:  {  // dwell
+      wait_for_empty_segment_buffer();
+      float delayTime = parseNumber('S',0) + parseNumber('P',0)*1000.0f;
+      pause(delayTime);
+      break;
+    }
+  case 28:  findHome();  break;
+  case 29:  calibrateBelts();  break;
+  case 54:
+  case 55:
+  case 56:
+  case 57:
+  case 58:
+  case 59: {  // 54-59 tool offsets
+    int tool_id=cmd-54;
+    set_tool_offset(tool_id,parseNumber('X',tool_offset[tool_id].x),
+                            parseNumber('Y',tool_offset[tool_id].y),
+                            parseNumber('Z',tool_offset[tool_id].z));
+    break;
+    }
+  case 90:  absolute_mode=1;  break;  // absolute mode
+  case 91:  absolute_mode=0;  break;  // relative mode
+  case 92: {  // set position (teleport)
+      Vector3 offset = get_end_plus_offset();
+      teleport( parseNumber('X',(absolute_mode?offset.x:0)*10)*0.1 + (absolute_mode?0:offset.x),
+                 parseNumber('Y',(absolute_mode?offset.y:0)*10)*0.1 + (absolute_mode?0:offset.y)
+               //parseNumber('Z',(absolute_mode?offset.z:0)) + (absolute_mode?0:offset.z)
+                 );
+      break;
+    }
+  default:  break;
+  }
+
+  cmd=parseNumber('D',-1);
+  switch(cmd) {
+  case 0: {  // jog one motor
+      int i,amount=parseNumber(m1d,0);
+      digitalWrite(MOTOR_0_DIR_PIN,amount < 0 ? motors[0].reel_in : motors[0].reel_out);
+      amount=abs(amount);
+      findStepDelay();
+      for(i=0;i<amount;++i) {
+        digitalWrite(MOTOR_0_STEP_PIN,HIGH);
+        digitalWrite(MOTOR_0_STEP_PIN,LOW);
+        pause(step_delay);
+      }
+
+      amount=parseNumber(m2d,0);
+      digitalWrite(MOTOR_1_DIR_PIN,amount < 0 ? motors[1].reel_in : motors[1].reel_out);
+      amount = abs(amount);
+      for(i=0;i<amount;++i) {
+        digitalWrite(MOTOR_1_STEP_PIN,HIGH);
+        digitalWrite(MOTOR_1_STEP_PIN,LOW);
+        pause(step_delay);
+      }
+    }
+    break;
+  case 1: {
+      // adjust spool diameters
+      float amountL=parseNumber('L',pulleyDiameter);
+
+      float d1=pulleyDiameter;
+      adjustPulleyDiameter(amountL);
+      if(pulleyDiameter != d1) {
+        // Update EEPROM
+        savePulleyDiameter();
+      }
+    }
+    break;
+  case 2:
+    Serial.print('L');  Serial.print(pulleyDiameter);
+    Serial.print(F(" R"));   Serial.println(pulleyDiameter);
+    break;
+//  case 3:  SD_ListFiles();  break;
+  case 4:  SD_StartPrintingFile(strchr(serialBuffer,' ')+1);  break;  // read file
+  case 5:
+    sayVersionNumber();
+  case 6:  // set home
+    setHome(parseNumber('X',(absolute_mode?homeX:0)*10)*0.1 + (absolute_mode?0:homeX),
+            parseNumber('Y',(absolute_mode?homeY:0)*10)*0.1 + (absolute_mode?0:homeY));
+  case 7:  // set calibration length
+      calibrateLeft = parseNumber('L',calibrateLeft);
+      calibrateRight = parseNumber('R',calibrateRight);
+      reportCalibration();
+    break;
+  case 8:  reportCalibration();  break;
+  case 9:  // save calibration length
+    saveCalibration();
+    break;
+  case 10:  // get hardware version
+    Serial.print("D10 V");
+    Serial.println(MAKELANGELO_HARDWARE_VERSION);
+    break;
+  default:  break;
+  }
+}
+
+
+void reportCalibration() {
+  Serial.print(F("D8 L"));
+  Serial.print(calibrateLeft);
+  Serial.print(F(" R"));
+  Serial.println(calibrateRight);
+}
+
+// equal to three decimal places?
+boolean equalEpsilon(float a,float b) {
+  int aa = a*10;
+  int bb = b*10;
+  //Serial.print("aa=");        Serial.print(aa);
+  //Serial.print("\tbb=");      Serial.print(bb);
+  //Serial.print("\taa==bb ");  Serial.println(aa==bb?"yes":"no");
+  
+  return aa==bb;
+}
+
+
+void setHome(float x,float y) {
+  boolean dx = equalEpsilon(x,homeX);
+  boolean dy = equalEpsilon(y,homeY);
+  if( dx==false || dy==false ) {
+    //Serial.print(F("Was    "));    Serial.print(homeX);    Serial.print(',');    Serial.println(homeY);
+    //Serial.print(F("Is now "));    Serial.print(    x);    Serial.print(',');    Serial.println(    y);
+    //Serial.print(F("DX="));    Serial.println(dx?"true":"false");
+    //Serial.print(F("DY="));    Serial.println(dy?"true":"false");
+    homeX = x;
+    homeY = y;
+    saveHome();
+  }
+}
+
+
+/**
+ * prepares the input buffer to receive a new message and tells the serial connected device it is ready for more.
+ */
+void parser_ready() {
+  sofar=0;  // clear input buffer
+  Serial.print(F("\n> "));  // signal ready to receive input
+  last_cmd_time = millis();
+}
+
+
+//------------------------------------------------------------------------------
+void tools_setup() {
+  for(int i=0;i<NUM_TOOLS;++i) {
+    set_tool_offset(i,0,0,0);
+  }
+}
+
+
+//------------------------------------------------------------------------------
+void setup() {
+  // start communications
+  Serial.begin(BAUD);
+  
+  loadConfig();
+
+
+  motor_setup();
+  motor_engage();
+  tools_setup();
+
+  //easyPWM_init();
+  //SD_init();
+  //LCD_init();
+
+  // initialize the plotter position.
+  teleport(0,0);
+  setPenAngle(PEN_UP_ANGLE);
+  setFeedRate(DEFAULT_FEEDRATE);
+  
+  // display the help at startup.
+  help();
+  
+  parser_ready();
+
+}
+
+
+//------------------------------------------------------------------------------
+// See: http://www.marginallyclever.com/2011/10/controlling-your-arduino-through-the-serial-monitor/
+void Serial_listen() {
+  // listen for serial commands
+  while(Serial.available() > 0) {
+    char c = Serial.read();
+    if(c=='\r') continue;
+    if(sofar<MAX_BUF) serialBuffer[sofar++]=c;
+    if(c=='\n') {
+      serialBuffer[sofar]=0;
+
+      // echo confirmation
+//      Serial.println(F(serialBuffer));
+
+      // do something with the command
+      processCommand();
+      parser_ready();
+    }
+  }
+}
+
+
+//------------------------------------------------------------------------------
+void loop() {
+  Serial_listen();
+  SD_check();
+#ifdef HAS_LCD
+  LCD_update();
+#endif
+
+  // The PC will wait forever for the ready signal.
+  // if Arduino hasn't received a new instruction in a while, send ready() again
+  // just in case USB garbled ready and each half is waiting on the other.
+  if( !segment_buffer_full() && (millis() - last_cmd_time) > TIMEOUT_OK ) {
+    //parser_ready();
+  }
+}
+
+
+/**
+ * This file is part of makelangelo-firmware.
+ *
+ * makelangelo-firmware is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * makelangelo-firmware is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with DrawbotGUI.  If not, see <http://www.gnu.org/licenses/>.
+ */
diff --git a/firmware_rumba_esp/lcd.ino b/firmware_rumba_esp/lcd.ino
new file mode 100644
index 0000000..5626fd9
--- /dev/null
+++ b/firmware_rumba_esp/lcd.ino
@@ -0,0 +1,363 @@
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+#ifdef HAS_LCD
+#include <LiquidCrystal.h>
+#include "sdcard.h"
+
+
+#define LCD_DRAW_DELAY    (150)
+#define LCD_TURN_PER_MENU (5)
+
+
+// Convenience macros that make it easier to generate menus
+
+#define MENU_START \
+  ty=0;
+
+#define MENU_END \
+  num_menu_items=ty; \
+
+#define MENU_ITEM_START(key) \
+  if(ty>=screen_position && ty<screen_end) { \
+    lcd.setCursor(0,ty-screen_position); \
+    lcd.print((menu_position==ty)?'>':' '); \
+    lcd.print(key); \
+
+#define MENU_ITEM_END() \
+  } \
+  ++ty;
+
+#define MENU_GOTO(new_menu) {  lcd_click_now=false;  lcd.clear();  num_menu_items=screen_position=menu_position=menu_position_sum=0;  current_menu=new_menu; return;  }
+
+#define MENU_SUBMENU(menu_label,menu_method) \
+    MENU_ITEM_START(menu_label) \
+    if(menu_position==ty && lcd_click_now) MENU_GOTO(menu_method); \
+    MENU_ITEM_END()
+
+#define MENU_ACTION(menu_label,menu_method) MENU_SUBMENU(menu_label,menu_method)
+
+#define MENU_LONG(key,value) \
+    MENU_ITEM_START(key) \
+    LCD_print_long(value); \
+    if(menu_position==ty && lcd_click_now) LCD_update_long(key,value); \
+    MENU_ITEM_END()
+
+#define MENU_FLOAT(key,value) \
+    MENU_ITEM_START(key) \
+    LCD_print_float(value); \
+    if(menu_position==ty && lcd_click_now) LCD_update_float(key,value); \
+    MENU_ITEM_END()
+
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+LiquidCrystal lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5, LCD_PINS_D6, LCD_PINS_D7);
+long lcd_draw_delay = 0;
+
+int lcd_rot_old  = 0;
+int lcd_turn     = 0;
+char lcd_click_old = HIGH;
+char lcd_click_now = false;
+uint8_t speed_adjust = 100;
+
+int menu_position_sum=0, menu_position=0, screen_position=0, num_menu_items=0, ty, screen_end;
+
+void (*current_menu)();
+
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+
+// initialize the Smart controller LCD panel
+void LCD_init() {
+  lcd.begin(LCD_WIDTH,LCD_HEIGHT);
+  pinMode(BTN_EN1,INPUT);
+  pinMode(BTN_EN2,INPUT);
+  pinMode(BTN_ENC,INPUT);
+  digitalWrite(BTN_EN1,HIGH);
+  digitalWrite(BTN_EN2,HIGH);
+  digitalWrite(BTN_ENC,HIGH);
+  current_menu=LCD_status_menu;
+  menu_position_sum=1;    /* 20160313-NM-Added so the clicking without any movement will display a menu */
+}
+
+
+void LCD_read() {
+  // detect pot turns
+  int rot = ((digitalRead(BTN_EN1)==LOW)<<BLEN_A)
+          | ((digitalRead(BTN_EN2)==LOW)<<BLEN_B);
+  switch(rot) {
+  case encrot0:
+    if( lcd_rot_old == encrot3 ) lcd_turn++;
+    if( lcd_rot_old == encrot1 ) lcd_turn--;
+    break;
+  case encrot1:
+    if( lcd_rot_old == encrot0 ) lcd_turn++;
+    if( lcd_rot_old == encrot2 ) lcd_turn--;
+    break;
+  case encrot2:
+    if( lcd_rot_old == encrot1 ) lcd_turn++;
+    if( lcd_rot_old == encrot3 ) lcd_turn--;
+    break;
+  case encrot3:
+    if( lcd_rot_old == encrot2 ) lcd_turn++;
+    if( lcd_rot_old == encrot0 ) lcd_turn--;
+    break;
+  }
+  lcd_rot_old = rot;
+
+  // find click state
+  int btn = digitalRead(BTN_ENC);
+  if( btn != lcd_click_old && btn == HIGH ) {
+    lcd_click_now=true;
+  }
+  lcd_click_old = btn;
+}
+
+
+void LCD_update() {
+  LCD_read();
+
+  if(millis() >= lcd_draw_delay ) {
+    lcd_draw_delay = millis() + LCD_DRAW_DELAY;
+
+    (*current_menu)();
+
+    if( lcd_turn ) {
+      int op = menu_position_sum / LCD_TURN_PER_MENU;
+      menu_position_sum += lcd_turn;
+      lcd_turn=0;
+
+      if(num_menu_items>0) {
+        if( menu_position_sum > (num_menu_items-1) * LCD_TURN_PER_MENU ) menu_position_sum = (num_menu_items-1) * LCD_TURN_PER_MENU;
+      }
+      if( menu_position_sum < 1)  menu_position_sum=1;  /* 20160313-NM-Added to stop the positon going negative at needing more winds to come back */
+
+      menu_position = menu_position_sum / LCD_TURN_PER_MENU;
+      if(op != menu_position) lcd.clear();
+
+      if(menu_position>num_menu_items-1) menu_position=num_menu_items-1;
+      if(menu_position<0) { menu_position=0;  }   
+      if(screen_position>menu_position) screen_position=menu_position;
+      if(screen_position<menu_position-(LCD_HEIGHT-1)) screen_position=menu_position-(LCD_HEIGHT-1);
+      screen_end=screen_position+LCD_HEIGHT;
+    }
+  }
+}
+
+
+// display the current machine position and feedrate on the LCD.
+void LCD_status_menu() {
+  MENU_START
+    // on click go to the main menu
+    if(lcd_click_now) MENU_GOTO(LCD_main_menu);
+
+    if(lcd_turn) {
+      speed_adjust += lcd_turn;
+    }
+    // update the current status
+    lcd.setCursor( 0, 0);  lcd.print('X');  LCD_print_float(posx);
+    lcd.setCursor(10, 0);  lcd.print('Y');  LCD_print_float(posy);
+    lcd.setCursor( 0, 1);  lcd.print('Z');  LCD_print_float(posz);
+    lcd.setCursor(10, 1);  lcd.print('F');  LCD_print_float(feed_rate);
+    lcd.setCursor( 0, 2);  lcd.print(F("Makelangelo #"));  lcd.print(robot_uid);
+    lcd.setCursor( 0, 3);  LCD_print_long(speed_adjust);  lcd.print(F("% "));
+    if(sd_printing_now==true/* && sd_printing_paused==false*/) {
+      LCD_print_float(sd_percent_complete);
+      lcd.print('%');
+    } else {
+      lcd.print(F("          "));
+    }
+  MENU_END
+}
+
+
+void LCD_main_menu() {
+  MENU_START
+    MENU_SUBMENU("Back",LCD_status_menu);
+    if(!sd_printing_now) {
+      MENU_ACTION("Disable motors",LCD_disable_motors);
+      MENU_ACTION("Enable motors",LCD_enable_motors);
+#if MAKELANGELO_HARDWARE_VERSION  == 5
+      MENU_ACTION("Find home",LCD_find_home);
+#endif
+      MENU_ACTION("This is home",LCD_this_is_home);
+      MENU_ACTION("Go home",LCD_go_home);
+      if(sd_inserted) {
+        MENU_SUBMENU("Start from file...",LCD_start_menu);
+      }
+    } else {
+      if(sd_printing_paused) {
+        MENU_ACTION("Unpause",LCD_pause);
+      } else {
+        MENU_ACTION("Pause",LCD_pause);
+      }
+      MENU_ACTION("Stop",LCD_stop);
+    }
+    //MENU_SUBMENU("Drive",LCD_drive_menu);
+  MENU_END
+}
+
+
+void LCD_pause() {
+  sd_printing_paused = (sd_printing_paused==true?false:true);
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_stop() {
+  sd_printing_now=false;
+  MENU_GOTO(LCD_main_menu);
+}
+
+void LCD_disable_motors() {
+  motor_disengage();
+  MENU_GOTO(LCD_main_menu);
+}
+
+void LCD_enable_motors() {
+  motor_engage();
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_find_home() {
+  findHome();
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_this_is_home() {
+  teleport(0,0);
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_go_home() {
+  polargraph_line( 0, 0, posz, DEFAULT_FEEDRATE );
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_start_menu() {
+  if(!sd_inserted) MENU_GOTO(LCD_main_menu);
+
+  MENU_START
+    MENU_SUBMENU("Back",LCD_main_menu);
+
+    root.rewindDirectory();
+    while(1) {
+      File entry =  root.openNextFile();
+      if (!entry) {
+        // no more files, return to the first file in the directory
+        break;
+      }
+      if (!entry.isDirectory() && entry.name()[0]!='_') {
+        MENU_ITEM_START(entry.name())
+        if(menu_position==ty && lcd_click_now) {
+          lcd_click_now=false;
+          SD_StartPrintingFile(entry.name());
+          MENU_GOTO(LCD_status_menu);
+        }
+        MENU_ITEM_END()
+      }
+      entry.close();
+    }
+  MENU_END
+}
+
+
+void LCD_update_long(char *name,long &value) {
+  lcd.clear();
+  do {
+    LCD_read();
+    if( lcd_turn ) {
+      value+=lcd_turn>0?1:-1;
+      lcd_turn=0;
+    }
+    lcd.setCursor(0,0);
+    lcd.print(name);
+    lcd.setCursor(0,1);
+    LCD_print_long(value);
+  } while( !lcd_click_now );
+}
+
+
+void LCD_update_float(char *name,float &value) {
+  lcd.clear();
+  do {
+    LCD_read();
+    if( lcd_turn ) {
+      value += lcd_turn > 0 ? 0.01 : -0.01;
+      lcd_turn=0;
+    }
+    lcd.setCursor(0,0);
+    lcd.print(name);
+    lcd.setCursor(0,1);
+    LCD_print_float(value);
+  } while( !lcd_click_now );
+}
+
+
+void LCD_print_long(long v) {
+  long av=abs(v);
+  int x=1000;
+  while(x>av && x>1) {
+    lcd.print(' ');
+    x/=10;
+  };
+  if(v>0) lcd.print(' ');
+  lcd.print(v);
+}
+
+
+
+void LCD_print_float(float v) {
+  int left = abs(v);
+  int right = abs((int)(v*100)%100);
+
+  if(left<1000) lcd.print(' ');
+  if(left<100) lcd.print(' ');
+  if(left<10) lcd.print(' ');
+  if(v<0) lcd.print('-');
+  else lcd.print(' ');
+  lcd.print(left);
+  lcd.print('.');
+  if(right<10) lcd.print('0');
+  lcd.print(right);
+}
+
+
+#else
+void LCD_init() {}
+void LCD_menu() {}
+#endif  // HAS_LCD
+
+/**
+ * This file is part of makelangelo-firmware.
+ *
+ * makelangelo-firmware is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * makelangelo-firmware is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with makelangelo-firmware.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
diff --git a/firmware_rumba_esp/motor.ino b/firmware_rumba_esp/motor.ino
new file mode 100644
index 0000000..c31960b
--- /dev/null
+++ b/firmware_rumba_esp/motor.ino
@@ -0,0 +1,711 @@
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+//#include "MServo.h"
+
+#define square(x) x*x
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+Axis a[NUM_AXIES];  // for line()
+Axis atemp;  // for line()
+Motor motors[NUM_AXIES];
+
+Segment line_segments[MAX_SEGMENTS];
+Segment *working_seg = NULL;
+volatile int current_segment=0;
+volatile int last_segment=0;
+int step_multiplier, nominal_step_multiplier;
+unsigned short nominal_OCR1A;
+
+//Servo servos[NUM_SERVOS];
+
+// used by timer1 to optimize interrupt inner loop
+int delta_x,delta_y;
+int over_x,over_y;
+int steps_total;
+int steps_taken;
+int accel_until,decel_after;
+long current_feed_rate;
+long old_feed_rate=0;
+long start_feed_rate,end_feed_rate;
+long time_accelerating,time_decelerating;
+float max_xy_jerk = MAX_JERK;
+long global_steps_0;
+long global_steps_1;
+int global_step_dir_0;
+int global_step_dir_1;
+
+
+/*
+long prescalers[] = {CLOCK_FREQ /   1,
+                     CLOCK_FREQ /   8,
+                     CLOCK_FREQ /  64,
+                     CLOCK_FREQ / 256,
+                     CLOCK_FREQ /1024};
+*/
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+
+
+// for reasons I don't understand... if i put this method in the .ino file i get compile errors.
+// so I put it here, which forces the externs.
+FORCE_INLINE Segment *segment_get_working() {
+  if(current_segment == last_segment ) return NULL;
+  working_seg = &line_segments[current_segment];
+  working_seg->busy=true;
+  return working_seg;
+}
+
+
+FORCE_INLINE int get_next_segment(int i) {
+  return SEGMOD( i + 1 );
+}
+
+
+FORCE_INLINE int get_prev_segment(int i) {
+  return SEGMOD( i - 1 );
+}
+
+
+float max_speed_allowed(float acceleration, float target_velocity, float distance) {
+  return sqrt(target_velocity*target_velocity - 2*acceleration*distance);
+  //return target_velocity - acceleration * distance;
+  //float a=abs(acceleration);
+ // if(distance>target_velocity*target_velocity/a) return target_velocity;
+ // else return sqrt(a*distance); 
+}
+
+
+/**
+ * set up the pins for each motor
+ */
+void motor_setup() {
+  motors[0].step_pin=MOTOR_0_STEP_PIN;
+  motors[0].dir_pin=MOTOR_0_DIR_PIN;
+  motors[0].enable_pin=MOTOR_0_ENABLE_PIN;
+  motors[0].limit_switch_pin=MOTOR_0_LIMIT_SWITCH_PIN;
+
+  motors[1].step_pin=MOTOR_1_STEP_PIN;
+  motors[1].dir_pin=MOTOR_1_DIR_PIN;
+  motors[1].enable_pin=MOTOR_1_ENABLE_PIN;
+  motors[1].limit_switch_pin=MOTOR_1_LIMIT_SWITCH_PIN;
+
+  int i;
+  for(i=0;i<NUM_AXIES;++i) {
+    // set the motor pin & scale
+    pinMode(motors[i].step_pin,OUTPUT);
+    pinMode(motors[i].dir_pin,OUTPUT);
+    pinMode(motors[i].enable_pin,OUTPUT);
+
+    // set the switch pin
+    motors[i].limit_switch_state=HIGH;
+    pinMode(motors[i].limit_switch_pin,INPUT);
+    digitalWrite(motors[i].limit_switch_pin,HIGH);
+  }
+
+  motor_set_step_count(0,0,0);
+
+  // setup servos
+#if NUM_SERVOS>0
+  //servos[0].attach(SERVO0_PIN);
+#endif
+#if NUM_SERVOS>1
+  servos[1].attach(SERVO1_PIN);
+#endif
+#if NUM_SERVOS>2
+  servos[2].attach(SERVO2_PIN);
+#endif
+#if NUM_SERVOS>3
+  servos[3].attach(SERVO3_PIN);
+#endif
+#if NUM_SERVOS>4
+  servos[4].attach(SERVO4_PIN);
+#endif
+
+  current_segment=0;
+  last_segment=0;
+  Segment &old_seg = line_segments[get_prev_segment(last_segment)];
+  old_seg.a[0].step_count=0;
+  old_seg.a[1].step_count=0;
+  old_seg.a[2].step_count=0;
+  working_seg = NULL;
+
+  // disable global interrupts
+  noInterrupts();
+//  // set entire TCCR1A register to 0
+//  TCCR1A = 0;
+//  // set the overflow clock to 0
+//  TCNT1  = 0;
+//  // set compare match register to desired timer count
+//  OCR1A = 2000;  // 1ms
+//  // turn on CTC mode
+//  TCCR1B = (1 << WGM12);
+//  // Set 8x prescaler
+//  TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
+//  // enable timer compare interrupt
+//  TIMSK1 |= (1 << OCIE1A);
+
+  timer0_isr_init();
+  timer0_attachInterrupt(itr);
+  timer0_write(ESP.getCycleCount() +5000L);
+  interrupts();
+}
+
+
+// turn on power to the motors (make them immobile)
+void motor_engage() {
+  int i;
+  for(i=0;i<NUM_AXIES;++i) {
+    digitalWrite(motors[i].enable_pin,LOW);
+  }
+}
+
+
+// turn off power to the motors (make them move freely)
+void motor_disengage() {
+  int i;
+  for(i=0;i<NUM_AXIES;++i) {
+    digitalWrite(motors[i].enable_pin,HIGH);
+  }
+}
+
+
+// Change pen state.
+void setPenAngle(int pen_angle) {
+  if(posz!=pen_angle) {
+    posz=pen_angle;
+
+    if(posz<PEN_DOWN_ANGLE) posz=PEN_DOWN_ANGLE;
+    if(posz>PEN_UP_ANGLE  ) posz=PEN_UP_ANGLE;
+
+    //servos[0].write(posz);
+  }
+}
+
+
+
+
+void recalculate_reverse2(Segment *prev,Segment *current,Segment *next) {
+  if(current==NULL) return;
+  if(next==NULL) return;
+
+  if (current->feed_rate_start != current->feed_rate_start_max) {
+    // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+    // for max allowable speed if block is decelerating and nominal length is false.
+    if ((!current->nominal_length_flag) && (current->feed_rate_start_max > next->feed_rate_start)) {
+      float v = min( current->feed_rate_start_max,
+                     max_speed_allowed(-acceleration,next->feed_rate_start,current->steps_total));
+      current->feed_rate_start = v;
+    } else {
+      current->feed_rate_start = current->feed_rate_start_max;
+    }
+    current->recalculate_flag = true;
+  }
+}
+
+
+void recalculate_reverse() {
+CRITICAL_SECTION_START
+  int s = last_segment;
+CRITICAL_SECTION_END
+
+  Segment *blocks[3] = {NULL,NULL,NULL};
+  int count = SEGMOD( last_segment + current_segment + MAX_SEGMENTS );
+  if(count>3) {
+    while(s != current_segment) {
+      s=get_prev_segment(s);
+      blocks[2]=blocks[1];
+      blocks[1]=blocks[0];
+      blocks[0]=&line_segments[s];
+      recalculate_reverse2(blocks[0],blocks[1],blocks[2]);
+    }
+  }
+}
+
+
+void recalculate_forward2(Segment *prev,Segment *current,Segment *next) {
+  if(prev==NULL) return;
+
+  // If the previous block is an acceleration block, but it is not long enough to complete the
+  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
+  // speeds have already been reset, maximized, and reverse planned by reverse planner.
+  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
+  if (!prev->nominal_length_flag) {
+    if (prev->feed_rate_start < current->feed_rate_start) {
+      double feed_rate_start = min( current->feed_rate_start,
+                                    max_speed_allowed(-acceleration,prev->feed_rate_start,prev->steps_total) );
+
+      // Check for junction speed change
+      if (current->feed_rate_start != feed_rate_start) {
+        current->feed_rate_start = feed_rate_start;
+        current->recalculate_flag = true;
+      }
+    }
+  }
+}
+
+
+void recalculate_forward() {
+  int s = current_segment;
+  Segment *blocks[3] = {NULL,NULL,NULL};
+
+  while(s != last_segment) {
+    s=get_next_segment(s);
+    blocks[0]=blocks[1];
+    blocks[1]=blocks[2];
+    blocks[2]=&line_segments[s];
+    recalculate_forward2(blocks[0],blocks[1],blocks[2]);
+  }
+  recalculate_forward2(blocks[1],blocks[2],NULL);
+}
+
+
+int intersection_distance(float accel,float distance,float start_speed,float end_speed) {
+#if 1
+  return ( ( 2.0*accel*distance - start_speed*start_speed + end_speed*end_speed ) / (4.0*accel) );
+#else
+//  float t2 = ( start_speed - end_speed + accel * distance ) / ( 2.0 * accel );
+//  return distance - t2;
+#endif
+}
+
+// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
+// given acceleration:
+inline unsigned long estimate_acceleration_distance(unsigned long initial_rate, unsigned long target_rate, unsigned long acceleration) {
+  //Serial.println( (target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration));
+  return(
+  (target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration)
+    );
+}
+
+void segment_update_trapezoid(Segment *s,float start_speed,float end_speed) {
+
+  float entry_factor = start_speed / s->feed_rate_max;
+  float exit_factor = end_speed / s->feed_rate_max;
+  long initial_rate = ceil(s->feed_rate_max*entry_factor);
+  long final_rate = ceil(s->feed_rate_max*exit_factor);
+  
+  if(start_speed<MIN_FEEDRATE) start_speed=MIN_FEEDRATE;
+  if(end_speed<MIN_FEEDRATE) end_speed=MIN_FEEDRATE;
+
+    long steps_to_accel = estimate_acceleration_distance(start_speed, s->feed_rate_max, acceleration);
+    long steps_to_decel = estimate_acceleration_distance(end_speed, s->feed_rate_max, acceleration);
+
+  //int steps_to_accel =  ceil( (s->feed_rate_max*s->feed_rate_max - start_speed*start_speed )/ (2.0*acceleration) );
+  //int steps_to_decel = floor( (end_speed*end_speed - s->feed_rate_max*s->feed_rate_max )/ -(2.0*acceleration) );
+  //int steps_to_accel =  ceil( ( s->feed_rate_max - start_speed ) * ( s->feed_rate_max - start_speed ) / acceleration/ 2 );
+  //int steps_to_decel = floor( ( end_speed - s->feed_rate_max ) * ( end_speed - s->feed_rate_max ) / acceleration  /2 );
+
+  long steps_at_top_speed = s->steps_total - steps_to_accel - steps_to_decel;
+  if(steps_at_top_speed<0) {
+    steps_to_accel = ceil( intersection_distance(acceleration,s->steps_total,start_speed,end_speed) );
+    steps_to_accel = max(steps_to_accel,0);
+    steps_to_accel = min(steps_to_accel,s->steps_total);
+    steps_at_top_speed=0;
+  }
+/*
+  Serial.print("M");  Serial.println(s->feed_rate_max);
+  Serial.print("E");  Serial.println(end_speed);
+  Serial.print("S");  Serial.println(start_speed);
+  Serial.print("@");  Serial.println(acceleration);
+  Serial.print("A");  Serial.println(steps_to_accel);
+  Serial.print("D");  Serial.println(steps_to_decel);
+*/
+CRITICAL_SECTION_START
+  if(!s->busy) {
+    s->accel_until = steps_to_accel;
+    s->decel_after = steps_to_accel+steps_at_top_speed;
+    s->feed_rate_start = initial_rate;
+    s->feed_rate_end = final_rate;
+  }
+CRITICAL_SECTION_END
+}
+
+
+void recalculate_trapezoids() {
+  int s = current_segment;
+  Segment *current;
+  Segment *next = NULL;
+
+  while(s != last_segment) {
+    current = next;
+    next = &line_segments[s];
+    if (current) {
+      // Recalculate if current block entry or exit junction speed has changed.
+      if (current->recalculate_flag || next->recalculate_flag)
+      {
+        // NOTE: Entry and exit factors always > 0 by all previous logic operations.
+        segment_update_trapezoid(current,current->feed_rate_start, next->feed_rate_start);
+        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+      }
+    }
+    s=get_next_segment(s);
+  }
+  // Last/newest block in buffer. Make sure the last block always ends motion.
+  if(next != NULL) {
+    segment_update_trapezoid(next, next->feed_rate_start, MIN_FEEDRATE);
+    next->recalculate_flag = false;
+  }
+}
+
+
+void recalculate_acceleration() {
+  recalculate_reverse();
+  recalculate_forward();
+  recalculate_trapezoids();
+
+#if VERBOSE > 1
+  //Serial.println("\nstart max,max,start,end,rate,total,up steps,cruise,down steps,nominal?");
+  Serial.println("---------------");
+  int s = current_segment;
+
+  while(s != last_segment) {
+    Segment *next = &line_segments[s];
+    s=get_next_segment(s);
+//                             Serial.print(next->feed_rate_start_max);
+//    Serial.print(F("\t"));   Serial.print(next->feed_rate_max);
+//    Serial.print(F("\t"));   Serial.print(acceleration);
+    Serial.print(F("\tS"));  Serial.print(next->feed_rate_start);
+    Serial.print(F("\tE"));  Serial.print(next->feed_rate_end);
+    Serial.print(F("\t*"));  Serial.print(next->steps_total);
+    Serial.print(F("\tA"));  Serial.print(next->accel_until);
+    int after = (next->steps_total - next->decel_after);
+    int total = next->steps_total - after - next->accel_until;
+    Serial.print(F("\tT"));  Serial.print(total);
+    Serial.print(F("\tD"));  Serial.print(after);
+    Serial.print(F("\t"));   Serial.println(next->nominal_length_flag==1?'*':' ');
+  }
+#endif
+}
+
+
+void motor_set_step_count(long a0,long a1,long a2) {
+  wait_for_empty_segment_buffer();
+
+  Segment &old_seg = line_segments[get_prev_segment(last_segment)];
+  old_seg.a[0].step_count=a0;
+  old_seg.a[1].step_count=a1;
+  old_seg.a[2].step_count=a2;
+
+  global_steps_0=0;
+  global_steps_1=0;
+}
+
+
+/**
+ * Supports movement with both styles of Motor Shield
+ * @input newx the destination x position
+ * @input newy the destination y position
+ **/
+void motor_onestep(int motor) {  // nobody calls here ???
+#ifdef VERBOSE
+  char *letter="XYZUVW";
+  Serial.print(letter[motor]);
+#endif
+
+  digitalWrite(motors[motor].step_pin,HIGH);
+  digitalWrite(motors[motor].step_pin,LOW);
+}
+
+
+/**
+ * Set the clock 2 timer frequency.
+ * @input desired_freq_hz the desired frequency
+ * Different clock sources can be selected for each timer independently.
+ * To calculate the timer frequency (for example 2Hz using timer1) you will need:
+ */
+FORCE_INLINE unsigned short calc_timer(unsigned short desired_freq_hz) {
+ /* if( desired_freq_hz > MAX_FEEDRATE ) desired_freq_hz = MAX_FEEDRATE;
+  if( desired_freq_hz < MIN_FEEDRATE ) desired_freq_hz = MIN_FEEDRATE;
+  old_feed_rate = desired_freq_hz;
+
+  if( desired_freq_hz > 20000 ) {
+    step_multiplier = 4;
+    desired_freq_hz = (desired_freq_hz>>2)&0x3fff;
+  } else if( desired_freq_hz > 10000 ) {
+    step_multiplier = 2;
+    desired_freq_hz = (desired_freq_hz>>1)&0x7fff;
+  } else {
+    step_multiplier = 1;
+  }
+
+  long counter_value = ( CLOCK_FREQ / 8 ) / desired_freq_hz;
+  if( counter_value >= MAX_COUNTER ) {
+    //Serial.print("this breaks the timer and crashes the arduino");
+    //Serial.flush();
+    counter_value = MAX_COUNTER - 1;
+  } else if( counter_value < 100 ) {
+    counter_value = 100;
+  }*/
+//Serial.print(desired_freq_hz); Serial.print("-"); 
+  step_multiplier = 1;
+  long counter_value = 4000000L / desired_freq_hz ; 
+  return counter_value;
+}
+
+
+/**
+ * Process all line segments in the ring buffer.  Uses bresenham's line algorithm to move all motors.
+ */
+void itr(void) { 
+  // segment buffer empty? do nothing
+  if( working_seg == NULL ) {
+    working_seg = segment_get_working();
+
+    if( working_seg != NULL ) {
+      // New segment!
+      // set the direction pins
+      digitalWrite( MOTOR_0_DIR_PIN, working_seg->a[0].dir );
+      digitalWrite( MOTOR_1_DIR_PIN, working_seg->a[1].dir );
+      global_step_dir_0 = (working_seg->a[0].dir==HIGH)?1:-1;
+      global_step_dir_1 = (working_seg->a[1].dir==HIGH)?1:-1;
+
+      //move the z axis
+      //servos[0].write(working_seg->a[2].step_count);
+
+      // set frequency to segment feed rate
+      nominal_OCR1A = calc_timer(working_seg->feed_rate_max);
+      nominal_step_multiplier = step_multiplier;
+
+      start_feed_rate = working_seg->feed_rate_start;
+      end_feed_rate = working_seg->feed_rate_end;
+      current_feed_rate = start_feed_rate;
+      time_decelerating = 0;
+      time_accelerating = calc_timer(start_feed_rate)/4; //Serial.print(time_accelerating); Serial.print(" ");
+      //OCR1A = time_accelerating;
+      timer0_write(ESP.getCycleCount() +time_accelerating*40);
+      
+
+      // defererencing some data so the loop runs faster.
+      steps_total=working_seg->steps_total;
+      steps_taken=0;
+      delta_x = working_seg->a[0].absdelta;
+      delta_y = working_seg->a[1].absdelta;
+      over_x = -(steps_total>>1);
+      over_y = -(steps_total>>1);
+      accel_until=working_seg->accel_until;
+      decel_after=working_seg->decel_after;
+      return;
+    } else {
+      //OCR1A = 2000; // wait 1ms
+      timer0_write(ESP.getCycleCount() + 80000L);
+      return;
+    }
+  }
+
+  if( working_seg != NULL ) {
+    // move each axis
+    for(uint8_t i=0;i<step_multiplier;++i) {
+      // M0
+      over_x += delta_x;
+      if(over_x > 0) {
+        digitalWrite(MOTOR_0_STEP_PIN,HIGH);
+      }
+      // M1
+      over_y += delta_y;
+      if(over_y > 0) {
+        digitalWrite(MOTOR_1_STEP_PIN,HIGH);
+      }
+      delayMicroseconds(1);
+      // now that the pins have had a moment to settle, do the second half of the steps.
+      // M0
+      if(over_x > 0) {
+        over_x -= steps_total;
+        global_steps_0+=global_step_dir_0;
+        digitalWrite(MOTOR_0_STEP_PIN,LOW);
+      }
+      // M1
+      if(over_y > 0) {
+        over_y -= steps_total;
+        global_steps_1+=global_step_dir_1;
+        digitalWrite(MOTOR_1_STEP_PIN,LOW);
+      }
+      
+      // make a step
+      steps_taken++;
+      if(steps_taken>=steps_total) break;
+    }
+
+    // accel
+    unsigned short t=0;
+    if( steps_taken <= accel_until ) {
+        current_feed_rate = (acceleration * time_accelerating / 1000000);
+        current_feed_rate += start_feed_rate;
+        if(current_feed_rate > working_seg->feed_rate_max) {
+          current_feed_rate = working_seg->feed_rate_max;
+        }
+        t = calc_timer(current_feed_rate);
+        //OCR1A = t;
+        timer0_write(ESP.getCycleCount() +t*40);
+        //nominal_OCR1A=t; // patch to avoid excess speed 
+        time_accelerating+=t/4;
+    } else if( steps_taken > decel_after ) {
+        unsigned short step_time = (acceleration * time_decelerating / 1000000);
+        long end_feed_rate = current_feed_rate - step_time;
+        if( end_feed_rate < working_seg->feed_rate_end ) {
+          end_feed_rate = working_seg->feed_rate_end;
+        }
+        t = calc_timer(end_feed_rate);
+        //OCR1A = t;
+        timer0_write(ESP.getCycleCount() +t*40);
+        time_decelerating+=t/4;
+    } else {
+      //OCR1A = nominal_OCR1A;
+      timer0_write(ESP.getCycleCount() +nominal_OCR1A*40);
+      step_multiplier = nominal_step_multiplier;
+    }
+                                                                //Serial.print(t>0?t:nominal_OCR1A);    Serial.print(".");
+    //OCR1A = (OCR1A < (TCNT1 + 16)) ? (TCNT1 + 16) : OCR1A;
+
+    // Is this segment done?
+    if( steps_taken >= steps_total ) {
+      // Move on to next segment without wasting an interrupt tick.
+      working_seg = NULL;
+      current_segment = get_next_segment(current_segment);
+    }
+  }
+}
+
+
+/**
+ * @return 1 if buffer is full, 0 if it is not.
+ */
+char segment_buffer_full() {
+  int next_segment = get_next_segment(last_segment);
+  return (next_segment == current_segment);
+}
+
+
+/**
+ * Uses bresenham's line algorithm to move both motors
+ **/
+void motor_line(long n0,long n1,long n2,float new_feed_rate) {
+  // get the next available spot in the segment buffer
+  int next_segment = get_next_segment(last_segment);
+  while( next_segment == current_segment ) {
+    // the buffer is full, we are way ahead of the motion system
+    if(VERBOSE>1) Serial.println("Buffer full"); 
+    delay(1);
+  }
+//Serial.print(n0); Serial.println(" new line");
+  int prev_segment = get_prev_segment(last_segment);
+  Segment &new_seg = line_segments[last_segment];
+  Segment &old_seg = line_segments[prev_segment];
+
+
+  // use LCD to adjust speed while drawing
+#ifdef HAS_LCD
+  new_feed_rate *= (float)speed_adjust * 0.01f;
+#endif
+/*
+  Serial.print('^');
+  Serial.print(n0);  Serial.print('\t');
+  Serial.print(n1);  Serial.print('\t');
+  Serial.print(n2);  Serial.print('\n');
+*/
+  new_seg.a[0].step_count = n0;
+  new_seg.a[1].step_count = n1;
+  new_seg.a[2].step_count = n2;
+  new_seg.a[0].delta = n0 - old_seg.a[0].step_count;
+  new_seg.a[1].delta = n1 - old_seg.a[1].step_count;
+  new_seg.a[2].delta = n2 - old_seg.a[2].step_count;
+  new_seg.feed_rate_max = new_feed_rate;
+  new_seg.busy=false;
+
+  // the axis that has the most steps will control the overall acceleration
+  new_seg.steps_total = 0;
+  float len=0;
+  int i;
+  for(i=0;i<NUM_AXIES;++i) {
+    new_seg.a[i].dir = (new_seg.a[i].delta < 0 ? motors[i].reel_in:motors[i].reel_out);
+    new_seg.a[i].absdelta = abs(new_seg.a[i].delta);
+    len += square(new_seg.a[i].delta);
+    if( new_seg.steps_total < new_seg.a[i].absdelta ) {
+      new_seg.steps_total = new_seg.a[i].absdelta;
+    }
+  }
+
+  // No steps?  No work!  Stop now.
+  if( new_seg.steps_total == 0 ) return;
+
+  len = sqrt( len );
+  float ilen = 1.0f / len;
+  float iSecond = new_feed_rate * ilen;
+  
+  for(i=0;i<NUM_AXIES;++i) {
+    new_seg.a[i].delta_normalized = new_seg.a[i].delta * ilen;
+  }
+  new_seg.steps_taken = 0;
+
+  // what is the maximum starting speed for this segment?
+  float feed_rate_start_max = MIN_FEEDRATE;
+  // is the robot changing direction sharply?
+  // aka is there a previous segment with a wildly different delta_normalized?
+  if(last_segment != current_segment) {
+    float dsx = new_seg.a[0].delta_normalized - old_seg.a[0].delta_normalized;
+    float dsy = new_seg.a[1].delta_normalized - old_seg.a[1].delta_normalized;
+    float dsz = new_seg.a[2].delta_normalized - old_seg.a[2].delta_normalized;
+    float jerk = sqrt(dsx*dsx + dsy*dsy + dsz*dsz);
+    float vmax_junction_factor = 1.0;
+    if(jerk> max_xy_jerk) {
+      vmax_junction_factor = max_xy_jerk / jerk;
+    }
+    feed_rate_start_max = min( new_seg.feed_rate_max * vmax_junction_factor, old_seg.feed_rate_max );
+  }
+
+  float allowable_speed = max_speed_allowed(-acceleration, MIN_FEEDRATE, new_seg.steps_total);
+
+  // come to a stop for entering or exiting a Z move
+  //if( new_seg.a[2].delta != 0 || old_seg.a[2].delta != 0 ) allowable_speed = MIN_FEEDRATE;
+
+  //Serial.print("max = ");  Serial.println(feed_rate_start_max);
+//  Serial.print("allowed = ");  Serial.println(allowable_speed);
+  new_seg.feed_rate_start_max = feed_rate_start_max;
+  new_seg.feed_rate_start = min(feed_rate_start_max, allowable_speed);
+
+  new_seg.nominal_length_flag = ( allowable_speed >= new_seg.feed_rate_max );
+  new_seg.recalculate_flag = true;
+
+  // when should we accelerate and decelerate in this segment?
+  segment_update_trapezoid(&new_seg,new_seg.feed_rate_start,MIN_FEEDRATE);
+
+  last_segment = next_segment;
+
+  recalculate_acceleration();
+}
+
+
+void wait_for_empty_segment_buffer() {
+  while( current_segment != last_segment ) delay(0);
+}
+
+
+/**
+ * This file is part of makelangelo-firmware.
+ *
+ * makelangelo-firmware is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * makelangelo-firmware is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with makelangelo-firmware.  If not, see <http://www.gnu.org/licenses/>.
+ */
diff --git a/firmware_rumba_esp/sdcard.h b/firmware_rumba_esp/sdcard.h
new file mode 100644
index 0000000..bca43f0
--- /dev/null
+++ b/firmware_rumba_esp/sdcard.h
@@ -0,0 +1,18 @@
+#ifndef SDCARD_H
+#define SDCARD_H
+
+
+#include <SPI.h>
+#include <SD.h>
+
+#ifdef HAS_SD
+
+extern File root;
+extern char sd_inserted;
+extern char sd_printing_now;
+extern char sd_printing_paused;
+extern float sd_percent_complete;
+#endif
+
+
+#endif // SDCARD_H
diff --git a/firmware_rumba_esp/sdcard.ino b/firmware_rumba_esp/sdcard.ino
new file mode 100644
index 0000000..29173c0
--- /dev/null
+++ b/firmware_rumba_esp/sdcard.ino
@@ -0,0 +1,158 @@
+//------------------------------------------------------------------------------
+// Makelangelo - supports raprapdiscount RUMBA controller
+// dan@marginallycelver.com 2013-12-26
+// RUMBA should be treated like a MEGA 2560 Arduino.
+//------------------------------------------------------------------------------
+// Copyright at end of file.  Please see
+// http://www.github.com/MarginallyClever/Makelangelo for more information.
+
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+#include "sdcard.h"
+#include <SD.h>
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+#ifdef HAS_SD
+
+File root;
+char sd_inserted;
+char sd_printing_now;
+char sd_printing_paused;
+File sd_print_file;
+float sd_percent_complete;
+long sd_file_size;
+long sd_bytes_read;
+
+#endif
+
+
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+
+// initialize the SD card and print some info.
+void SD_init() {
+#ifdef HAS_SD
+  pinMode(SDSS, OUTPUT);
+  pinMode(SDCARDDETECT,INPUT);
+  digitalWrite(SDCARDDETECT,HIGH);
+
+  sd_inserted = false;
+  sd_printing_now=false;
+  sd_percent_complete=0;
+  SD_check();
+#endif  // HAS_SD
+}
+
+
+// Load the SD card and read some info about it
+void SD_load_card() {
+#ifdef HAS_SD
+  SD.begin(SDSS);
+  root = SD.open("/");
+#endif
+}
+
+
+// Check if the SD card has been added or removed
+void SD_check() {
+#ifdef HAS_SD
+  int state = (digitalRead(SDCARDDETECT) == LOW);
+  if(sd_inserted != state) {
+    Serial.print("SD is ");
+    if(!state) {
+      Serial.println(F("removed"));
+      sd_printing_now=false;
+    } else {
+      Serial.println(F("added"));
+      SD_load_card();
+    }
+    sd_inserted = state;
+  }
+
+  // read one line from the file.  don't read too fast or the LCD will appear to hang.
+  if(sd_printing_now==true && sd_printing_paused==false && segment_buffer_full()==false ) {
+    int c;
+    while(sd_print_file.peek() != -1) {
+      c=sd_print_file.read();
+      serialBuffer[sofar++]=c;
+      sd_bytes_read++;
+      if(c==';') {
+        // eat to the end of the line
+        while(sd_print_file.peek() != -1) {
+          c=sd_print_file.read();
+          sd_bytes_read++;
+          if(c=='\n' || c=='\r') break;
+        }
+      }
+      if(c=='\n' || c=='\r') {
+        // update the % visible on the LCD.
+        sd_percent_complete = (float)sd_bytes_read * 100.0 / (float)sd_file_size;
+
+        // end string
+        serialBuffer[sofar]=0;
+        // print for our benefit
+        Serial.println(serialBuffer);
+        // process command
+        processCommand();
+        // reset buffer for next line
+        parser_ready();
+        // quit this loop so we can update the LCD and listen for commands from the laptop (if any)
+        break;
+      }
+    }
+
+    if(sd_print_file.peek() == -1) {
+      sd_print_file.close();
+      sd_printing_now=false;
+    }
+  }
+#endif // HAS_SD
+}
+
+
+void SD_StartPrintingFile(char *filename) {
+#ifdef HAS_SD
+  sd_print_file=SD.open(filename);
+  if(!sd_print_file) {
+    Serial.print(F("File "));
+    Serial.print(filename);
+    Serial.println(F(" not found."));
+    return;
+  }
+
+  // count the number of lines (\n characters) for displaying % complete.
+  sd_file_size=sd_print_file.size();
+  sd_bytes_read=0;
+  sd_percent_complete=0;
+
+  // return to start
+  sd_print_file.seek(0);
+
+  sd_printing_now=true;
+  sd_printing_paused=false;
+#endif
+}
+
+
+/**
+ * This file is part of makelangelo-firmware.
+ *
+ * makelangelo-firmware is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * makelangelo-firmware is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with makelangelo-firmware.  If not, see <http://www.gnu.org/licenses/>.
+ */