diff --git a/MSerial.ino b/MSerial.cpp
similarity index 58%
rename from MSerial.ino
rename to MSerial.cpp
index bc2a489..3e16bf1 100644
--- a/MSerial.ino
+++ b/MSerial.cpp
@@ -1,8 +1,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 /*
@@ -29,22 +28,4 @@ void serial_setup(long baud) {
 // 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/>.
- */
+*/
\ No newline at end of file
diff --git a/MServo.cpp b/MServo.cpp
index 23e24dd..c35952a 100644
--- a/MServo.cpp
+++ b/MServo.cpp
@@ -45,6 +45,8 @@
 
 #include "MServo.h"
 
+#ifndef ESP8266
+
 #define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
 #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
 
@@ -333,3 +335,6 @@ bool Servo::attached()
 {
   return servos[this->servoIndex].Pin.isActive ;
 }
+
+
+#endif // ESP8266
diff --git a/MServo.h b/MServo.h
index a24499d..2f233a7 100644
--- a/MServo.h
+++ b/MServo.h
@@ -45,6 +45,8 @@
 #ifndef Servo_h
 #define Servo_h
 
+#ifndef ESP8266
+
 #include <avr/interrupt.h>
 #include <Arduino.h>
 #include <inttypes.h>
@@ -86,10 +88,10 @@ typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
 typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
 
 #else  // everything else
-//#define _useTimer1
-#define _useTimer3
-//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
-typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
+#define _useTimer1
+//#define _useTimer3
+typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+//typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
 #endif
 
 #define Servo_VERSION           2      // software version of this library
@@ -132,4 +134,6 @@ class Servo
    int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
 };
 
-#endif
+#endif // ESP8266
+
+#endif // Servo_h
diff --git a/Makelangelo-firmware.ino b/Makelangelo-firmware.ino
index 8c3ff2c..9b5fd66 100644
--- a/Makelangelo-firmware.ino
+++ b/Makelangelo-firmware.ino
@@ -1,8 +1,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
@@ -10,7 +9,10 @@
 // INCLUDES
 //------------------------------------------------------------------------------
 #include "configure.h"
-#include "lcd.h"
+#include "motor.h"
+#include "SD.h"
+#include "LCD.h"
+#include "eeprom.h"
 
 #include <SPI.h>  // pkm fix for Arduino 1.5
 
@@ -48,6 +50,17 @@ long line_number = 0;           // make sure commands arrive in order
 float tool_offset[NUM_TOOLS][NUM_AXIES];
 int current_tool = 0;
 
+#ifdef HAS_WIFI
+
+#include <ESP8266WiFi.h>
+#include <WiFiUdp.h>
+const char* SSID_NAME = WIFI_SSID_NAME;
+const char* SSID_PASS = WIFI_SSID_PASS;
+WiFiUDP port;
+unsigned int localPort = 9999;
+
+#endif  // HAS_WIFI
+
 
 
 
@@ -56,6 +69,11 @@ int current_tool = 0;
 //------------------------------------------------------------------------------
 
 
+void findStepDelay() {
+  step_delay = 1000000.0f / (DEFAULT_FEEDRATE / THREAD_PER_STEP);
+}
+
+
 // returns angle of dy/dx as a value from 0...2PI
 float atan3(float dy, float dx) {
   float a = atan2(dy, dx);
@@ -65,8 +83,8 @@ float atan3(float dy, float dx) {
 
 
 /**
- * @return switch state
- */
+   @return switch state
+*/
 char readSwitches() {
 #ifdef USE_LIMIT_SWITCH
   // get the current switch state
@@ -77,13 +95,12 @@ char readSwitches() {
 }
 
 
-/** 
- * feed rate is given in units/min and converted to cm/s
- */
+/**
+   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="));
@@ -93,70 +110,55 @@ void setFeedRate(float v1) {
 }
 
 
-void findStepDelay() {
-  step_delay = 1000000.0f / DEFAULT_FEEDRATE;
-}
-
-
 /**
- * @param delay in microseconds
- */
-void pause(long us) {
+   @param delay in microseconds
+*/
+void pause(const long us) {
   delay(us / 1000);
   delayMicroseconds(us % 1000);
 }
 
 
 /**
- * print the current feed rate
- */
-void printFeedRate() {
-  Serial.print(F("F"));
-  Serial.print(feed_rate);
-  Serial.print(F("steps/s"));
-}
-
-
-/**
- * M101 Annn Tnnn Bnnn
- * Change axis A limits to max T and min B.
- * look for change to dimensions in command, apply and save changes.
- */
+   M101 Annn Tnnn Bnnn
+   Change axis A limits to max T and min B.
+   look for change to dimensions in command, apply and save changes.
+*/
 void parseLimits() {
-  int axisNumber = parseNumber('A',-1);
-  if(axisNumber==-1) return;
-  if(axisNumber>=NUM_AXIES) return;
-  
+  int axisNumber = parseNumber('A', -1);
+  if (axisNumber == -1) return;
+  if (axisNumber >= NUM_AXIES) return;
+
   float newT = parseNumber('T', axies[axisNumber].limitMax);
   float newB = parseNumber('B', axies[axisNumber].limitMin);
-  boolean changed=false;
-  
-  if(!equalEpsilon(axies[axisNumber].limitMax,newT)) {
-    axies[axisNumber].limitMax=newT;
-    changed=true;
+  boolean changed = false;
+
+  if (!equalEpsilon(axies[axisNumber].limitMax, newT)) {
+    axies[axisNumber].limitMax = newT;
+    changed = true;
   }
-  if(!equalEpsilon(axies[axisNumber].limitMin,newB)) {
-    axies[axisNumber].limitMin=newB;
-    changed=true;
+  if (!equalEpsilon(axies[axisNumber].limitMin, newB)) {
+    axies[axisNumber].limitMin = newB;
+    changed = true;
   }
-  if(changed==true) {
+  if (changed == true) {
     saveLimits();
   }
 
   printConfig();
   /*
-  float pos[NUM_AXIES];
-  int i;
-  for(i=0;i<NUM_AXIES;++i) {
+    float pos[NUM_AXIES];
+    int i;
+    for(i=0;i<NUM_AXIES;++i) {
     pos[i]=axies[i].pos;
-  }
-  teleport(pos);*/
+    }
+    teleport(pos);*/
 }
 
 
 /**
- * Test that IK(FK(A))=A
- */
+   Test that IK(FK(A))=A
+*/
 void testKinematics() {
   long A[NUM_MOTORS], i, j;
   float axies1[NUM_AXIES];
@@ -169,7 +171,7 @@ void testKinematics() {
 
     IK(axies1, A);
     FK(A, axies2);
-    
+
     for (j = 0; j < NUM_AXIES; ++j) {
       Serial.print('\t');
       Serial.print(AxisNames[j]);
@@ -190,7 +192,7 @@ void testKinematics() {
       Serial.print(F("\td"));
       Serial.print(AxisNames[j]);
       Serial.print('=');
-      Serial.print(axies2[j]-axies1[j]);
+      Serial.print(axies2[j] - axies1[j]);
     }
     Serial.println();
   }
@@ -198,33 +200,14 @@ void testKinematics() {
 
 
 /**
- * Translate the XYZ through the IK to get the number of motor steps and move the motors.
- * @input pos NUM_AXIES floats describing destination coordinates
- * @input new_feed_rate speed to travel along arc
- */
-void lineSafeInternal(float *pos, float new_feed_rate) {
-  long steps[NUM_MOTORS + NUM_SERVOS];
-  IK(pos, steps);
-
-  int i;
-  for(i=0;i<NUM_AXIES;++i) {
-    axies[i].pos = pos[i];
-  }
-  
-  feed_rate = new_feed_rate;
-  motor_line(steps, new_feed_rate);
-}
-
-
-/**
- * Move the pen holder in a straight line using bresenham's algorithm
- * @input pos NUM_AXIES floats describing destination coordinates
- * @input new_feed_rate speed to travel along arc
- */
+   Move the pen holder in a straight line using bresenham's algorithm
+   @input pos NUM_AXIES floats describing destination coordinates
+   @input new_feed_rate speed to travel along arc
+*/
 void lineSafe(float *pos, float new_feed_rate) {
   float destination[NUM_AXIES];
   int i;
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     destination[i] = pos[i] - tool_offset[current_tool][i];
     // @TODO confirm destination is within max/min limits.
   }
@@ -234,44 +217,51 @@ void lineSafe(float *pos, float new_feed_rate) {
   float delta[NUM_AXIES];
   float startPos[NUM_AXIES];
   float temp[NUM_AXIES];
-  float len=0;  
-  for(i=0;i<NUM_AXIES;++i) {
+  float len = 0;
+  for (i = 0; i < NUM_AXIES; ++i) {
     startPos[i] = axies[i].pos;
     delta[i] = destination[i] - startPos[i];
-    len += delta[i] * delta[i];
+    len += sq(delta[i]);
   }
+  
+#if MACHINE_STYLE == POLARGRAPH
+  // often SEGMENT_PER_CM_LINE is 10mm or 20mm.  but a servo movement can be 90-160=70, or 7 segments.  This is pretty nuts.
+  // discount the z movement from the subdivision to use less segments and (I hope) move the servo faster.
+  len -= sq(delta[2]);
+  delta[2] = 0;
+#endif
 
-  len = sqrt(len);
+  len = sqrt(len);  //mm
   // @TODO what if some axies don't need subdividing?  like z axis on polargraph.
-  int pieces = ceil(len * (float)SEGMENT_PER_CM_LINE );
+  int pieces = ceil(len * (float)SEGMENT_PER_CM_LINE / 10.0 );
   float a;
   long j;
 
   // draw everything up to (but not including) the destination.
   for (j = 1; j < pieces; ++j) {
     a = (float)j / (float)pieces;
-    for(i=0;i<NUM_AXIES;++i) {
+    for (i = 0; i < NUM_AXIES; ++i) {
       temp[i] = delta[i] * a + startPos[i];
     }
-    lineSafeInternal(temp, new_feed_rate);
+    motor_line(temp, new_feed_rate);
   }
 #endif
 
   // guarantee we stop exactly at the destination (no rounding errors).
-  lineSafeInternal(destination, new_feed_rate);
+  motor_line(destination, new_feed_rate);
 }
 
 
 /**
- * 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 destination point where movement ends
- * @input dir - ARC_CW or ARC_CCW to control direction of arc
- * @input new_feed_rate speed to travel along arc
- */
+   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 destination point where movement ends
+   @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 *destination, char clockwise, float new_feed_rate) {
   // get radius
   float dx = axies[0].pos - cx;
@@ -284,7 +274,7 @@ void arc(float cx, float cy, float *destination, char clockwise, float new_feed_
   float er = sqrt(dx * dx + dy * dy);
 
   float da = ea - sa;
-       if (clockwise != 0 && da < 0) ea += 2 * PI;
+  if (clockwise != 0 && da < 0) ea += 2 * PI;
   else if (clockwise == 0 && da > 0) sa += 2 * PI;
   da = ea - sa;
   float dr = er - sr;
@@ -294,17 +284,17 @@ void arc(float cx, float cy, float *destination, char clockwise, float new_feed_
   // float len=theta*circ/(PI*2.0);
   // simplifies to
   float len1 = abs(da) * sr;
-  float len = sqrt( len1 * len1 + dr * dr );
+  float len = sqrt( len1 * len1 + dr * dr ); // mm
 
-  int i, segments = ceil( len * SEGMENT_PER_CM_ARC );
+  int i, segments = ceil( len * SEGMENT_PER_CM_ARC / 10 );
 
-  float n[NUM_AXIES], angle3, scale;
+  float n[NUM_AXIES], scale;
   float a, r;
-  #if NUM_AXIES>2
+#if NUM_AXIES>2
   float sz = axies[2].pos;
   float z = destination[2];
-  #endif
-  
+#endif
+
   for (i = 0; i <= segments; ++i) {
     // interpolate around the arc
     scale = ((float)i) / ((float)segments);
@@ -314,9 +304,9 @@ void arc(float cx, float cy, float *destination, char clockwise, float new_feed_
 
     n[0] = cx + cos(a) * r;
     n[1] = cy + sin(a) * r;
-    #if NUM_AXIES>2
+#if NUM_AXIES>2
     n[2] = ( z - sz ) * scale + sz;
-    #endif
+#endif
     // send it to the planner
     lineSafe(n, new_feed_rate);
   }
@@ -324,26 +314,26 @@ void arc(float cx, float cy, float *destination, char clockwise, float new_feed_
 
 
 /**
- * Instantly move the virtual plotter position.  Does not check if the move is valid.
- */
+   Instantly move the virtual plotter position.  Does not check if the move is valid.
+*/
 void teleport(float *pos) {
   wait_for_empty_segment_buffer();
 
   int i;
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     axies[i].pos = pos[i];
   }
-  
-  long steps[NUM_MOTORS+NUM_SERVOS];
+
+  long steps[NUM_MOTORS + NUM_SERVOS];
   IK(pos, steps);
   motor_set_step_count(steps);
 }
 
 
 /**
- * M100
- * Print a helpful message to serial.  The first line must never be changed to play nice with the JAVA software.
- */
+   M100
+   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! "));
   sayModelAndUID();
@@ -354,6 +344,14 @@ void help() {
   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"));
+#ifdef HAS_WIFI
+  // Print the IP address
+  Serial.print("Use this URL to connect: http://");
+  Serial.print(WiFi.softAPIP());
+  Serial.print(':');
+  Serial.print(localPort);
+  Serial.println('/');
+#endif  // HAS_WIFI
 }
 
 
@@ -366,9 +364,9 @@ void sayModelAndUID() {
 
 
 /**
- * D5
- * report current firmware version
- */
+   D5
+   report current firmware version
+*/
 void sayFirmwareVersionNumber() {
   char versionNumber = loadVersion();
 
@@ -378,25 +376,27 @@ void sayFirmwareVersionNumber() {
 
 
 /**
- * M114
- * Print the X,Y,Z, feedrate, acceleration, and home position
- */
+   M114
+   Print the X,Y,Z, feedrate, acceleration, and home position
+*/
 void where() {
   wait_for_empty_segment_buffer();
 
   int i;
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     Serial.print(AxisNames[i]);
     Serial.print(axies[i].pos);
     Serial.print(' ');
   }
 
-  printFeedRate();
-  
+  Serial.print(F("F"));
+  Serial.print(feed_rate);
+  Serial.print(F("mm/s"));
+
   Serial.print(F(" A"  ));
   Serial.println(acceleration);
-  
-  for(i=0;i<NUM_AXIES;++i) {
+
+  for (i = 0; i < NUM_AXIES; ++i) {
     Serial.print('H');
     Serial.print(AxisNames[i]);
     Serial.print(axies[i].homePos);
@@ -407,24 +407,24 @@ void where() {
 
 
 /**
- * M102
- * Print the machine limits to serial.
- */
+   M102
+   Print the machine limits to serial.
+*/
 void printConfig() {
   int i;
 
   Serial.print(F("("));
-  
-  for(i=0;i<NUM_AXIES;++i) {
+
+  for (i = 0; i < NUM_AXIES; ++i) {
     Serial.print(axies[i].limitMin);
-    if(i<NUM_AXIES-1)  Serial.print(',');
+    if (i < NUM_AXIES - 1)  Serial.print(',');
   }
 
   Serial.print(F(") - ("));
-  
-  for(i=0;i<NUM_AXIES;++i) {
+
+  for (i = 0; i < NUM_AXIES; ++i) {
     Serial.print(axies[i].limitMax);
-    if(i<NUM_AXIES-1)  Serial.print(',');
+    if (i < NUM_AXIES - 1)  Serial.print(',');
   }
 
   Serial.print(F(")\n"));
@@ -432,53 +432,48 @@ void printConfig() {
 
 
 /**
- * Set the relative tool offset
- * @input toolID the active tool id
- * @input pos the offsets
- */
+   Set the relative tool offset
+   @input toolID the active tool id
+   @input pos the offsets
+*/
 void set_tool_offset(int toolID, float *pos) {
   int i;
 
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     tool_offset[toolID][i] = pos[i];
   }
 }
 
 
 /**
- * @param results array of NUM_AXIES floats
- * @return the position + active tool offset
- */
+   @param results array of NUM_AXIES floats
+   @return the position + active tool offset
+*/
 void get_end_plus_offset(float *results) {
   int i;
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     results[i] = tool_offset[current_tool][i] + axies[i].pos;
   }
 }
 
 
 /**
- * M6 [Tnnn]
- * Change the currently active tool
- */
+   M6 [Tnnn]
+   Change the currently active tool
+*/
 void toolChange(int tool_id) {
   if (tool_id < 0) tool_id = 0;
   if (tool_id >= NUM_TOOLS) tool_id = NUM_TOOLS - 1;
   current_tool = tool_id;
-#ifdef HAS_SD
-  if (sd_printing_now) {
-    sd_printing_paused = true;
-  }
-#endif
 }
 
 
 /**
- * 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.
- */
+   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
@@ -492,8 +487,8 @@ float parseNumber(char code, float val) {
 
 
 /**
- * @return 1 if the character is found in the serial buffer, 0 if it is not found.
- */
+   @return 1 if the character is found in the serial buffer, 0 if it is not found.
+*/
 char hasGCode(char code) {
   char *ptr = serialBuffer; // start at the beginning of buffer
   while ((long)ptr > 1 && (*ptr) && (long)ptr < (long)serialBuffer + sofar) { // walk to the end
@@ -506,9 +501,9 @@ char hasGCode(char code) {
 }
 
 /**
- * G4 [Snn] [Pnn]
- * Wait S milliseconds and P seconds.
- */
+   G4 [Snn] [Pnn]
+   Wait S milliseconds and P seconds.
+*/
 void parseDwell() {
   wait_for_empty_segment_buffer();
   float delayTime = parseNumber('S', 0) + parseNumber('P', 0) * 1000.0f;
@@ -516,43 +511,47 @@ void parseDwell() {
 }
 
 
-/** 
- * G0-G1 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn] [Ann] [Fnn]
- * straight lines
- */
+/**
+   G0-G1 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn] [Ann] [Fnn]
+   straight lines.  distance in mm.
+*/
 void parseLine() {
   float offset[NUM_AXIES];
   get_end_plus_offset(offset);
-  acceleration = min(max(parseNumber('A', acceleration), MIN_ACCELERATION), MAX_ACCELERATION);
+  acceleration = parseNumber('A', acceleration);
+  acceleration = min(max(acceleration, (float)MIN_ACCELERATION), (float)MAX_ACCELERATION);
   float f = parseNumber('F', feed_rate);
-  
+  f = max(f, (float)MIN_FEEDRATE);
+
   int i;
   float pos[NUM_AXIES];
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     pos[i] = parseNumber(AxisNames[i], (absolute_mode ? offset[i] : 0)) + (absolute_mode ? 0 : offset[i]);
   }
-  
+
   lineSafe( pos, f );
 }
 
 
-/** 
- * G2-G3 [Xnnn] [Ynnn] [Ann] [Fnn] [Inn] [Jnn]
- * arcs in the XY plane
- * @param clockwise (G2) 1 for cw, (G3) 0 for ccw
- */
+/**
+   G2-G3 [Xnnn] [Ynnn] [Ann] [Fnn] [Inn] [Jnn]
+   arcs in the XY plane
+   @param clockwise (G2) 1 for cw, (G3) 0 for ccw
+*/
 void parseArc(int clockwise) {
   float offset[NUM_AXIES];
   get_end_plus_offset(offset);
-  acceleration = min(max(parseNumber('A', acceleration), MIN_ACCELERATION), MAX_ACCELERATION);
+  acceleration = parseNumber('A', acceleration);
+  acceleration = min(max(acceleration, (float)MIN_ACCELERATION), (float)MAX_ACCELERATION);
   float f = parseNumber('F', feed_rate);
-  
+  f = max(f, (float)MIN_FEEDRATE);
+
   int i;
   float pos[NUM_AXIES];
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     pos[i] = parseNumber(AxisNames[i], (absolute_mode ? offset[i] : 0)) + (absolute_mode ? 0 : offset[i]);
   }
-  
+
   arc(parseNumber('I', (absolute_mode ? offset[0] : 0)) + (absolute_mode ? 0 : offset[0]),
       parseNumber('J', (absolute_mode ? offset[1] : 0)) + (absolute_mode ? 0 : offset[1]),
       pos,
@@ -562,16 +561,16 @@ void parseArc(int clockwise) {
 
 
 /**
- * G92 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn]
- * Teleport mental position
- */
+   G92 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn]
+   Teleport mental position
+*/
 void parseTeleport() {
   float offset[NUM_AXIES];
   get_end_plus_offset(offset);
-  
+
   int i;
   float pos[NUM_AXIES];
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     pos[i] = parseNumber(AxisNames[i], (absolute_mode ? offset[i] : 0)) + (absolute_mode ? 0 : offset[i]);
   }
   teleport(pos);
@@ -579,22 +578,22 @@ void parseTeleport() {
 
 
 /**
- * G54-G59 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn]
- * Adjust tool offset
- */
+   G54-G59 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn]
+   Adjust tool offset
+*/
 void parseToolOffset(int toolID) {
   int i;
   float offset[NUM_AXIES];
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     offset[i] = parseNumber(AxisNames[i], tool_offset[toolID][i]);
   }
-  set_tool_offset(toolID,offset);
+  set_tool_offset(toolID, offset);
 }
 
 
 /**
- * @return 1 if CRC ok or not present, 0 if CRC check fails.
- */
+   @return 1 if CRC ok or not present, 0 if CRC check fails.
+*/
 char checkLineNumberAndCRCisOK() {
   // is there a line number?
   long cmd = parseNumber('N', -1);
@@ -608,17 +607,17 @@ char checkLineNumberAndCRCisOK() {
 
     // is there a checksum?
     int i;
-    for(i=strlen(serialBuffer)-1;i>=0;--i) {
-      if(serialBuffer[i]=='*') {
+    for (i = strlen(serialBuffer) - 1; i >= 0; --i) {
+      if (serialBuffer[i] == '*') {
         break;
       }
     }
-    
-    if(i>=0) {
+
+    if (i >= 0) {
       // yes.  is it valid?
       char checksum = 0;
       int c;
-      for(c=0;c<i;++c) {
+      for (c = 0; c < i; ++c) {
         checksum ^= serialBuffer[c];
       }
       c++; // skip *
@@ -635,142 +634,158 @@ char checkLineNumberAndCRCisOK() {
     }
 
     // remove checksum
-    serialBuffer[i]=0;
+    serialBuffer[i] = 0;
 
     line_number++;
   }
-  
+
   return 1;  // ok!
 }
 
 
 /**
- * M117 [string] 
- * Display string on the LCD panel.  Command is ignored if there is no LCD panel.
- */
+   M117 [string]
+   Display string on the LCD panel.  Command is ignored if there is no LCD panel.
+*/
 void parseMessage() {
 #ifdef HAS_LCD
-  int i,j=0;
-  for(i=0;i<strlen(serialBuffer);++i) {
-    if((serialBuffer[i]=='M'||serialBuffer[i]=='m')&&
-       serialBuffer[i+1]=='1'&&
-       serialBuffer[i+2]=='1'&&
-       serialBuffer[i+3]=='7') {
-      i+=4;
+  uint16_t i;
+  for (i = 0; i < strlen(serialBuffer); ++i) {
+    if ((serialBuffer[i] == 'M' || serialBuffer[i] == 'm') &&
+        serialBuffer[i + 1] == '1' &&
+        serialBuffer[i + 2] == '1' &&
+        serialBuffer[i + 3] == '7') {
       //Serial.print("Found M117:");
       //Serial.println(serialBuffer+i);
       break;
     }
   }
-  
 
-  if(i>=strlen(serialBuffer)) {
-    //Serial.println("No message.");
+  // wipe previous message
+  for (uint16_t j = LCD_MESSAGE_LENGTH / 2; j < LCD_MESSAGE_LENGTH; ++j) {
+    lcd_message[j] = ' ';
+  }
+
+  i += 4;
+  if (i >= strlen(serialBuffer)) {
     // no message
-    lcd_message[0]=0;
-    lcd_message[LCD_WIDTH + 1]=0;
+    Serial.println("No message.");
     return;
   }
 
   // preserve message for display
-  int top = min(LCD_MESSAGE_LENGTH,MAX_BUF);
+  uint16_t top = min(LCD_MESSAGE_LENGTH, MAX_BUF);
 
-  char *buf = serialBuffer+i;
-  while(*buf==' ') ++buf;  // eat whitespace
-  
-  //Serial.print("message found:");
-  i=j=0;
-  while(isPrintable(*buf) && *buf!='\r' && *buf!='\n' && i<LCD_MESSAGE_LENGTH-1) {
-    lcd_message[i]=*buf;
-    //Serial.print(*buf);
+  char *buf = serialBuffer + i;
+  while (*buf == ' ') ++buf; // eat whitespace
+
+  i = LCD_MESSAGE_LENGTH / 2;
+  while (isPrintable(*buf) && (*buf) != '\r' && (*buf) != '\n' && i < top) {
+    lcd_message[i] = *buf;
     ++i;
-    ++j;
-    if((j%LCD_WIDTH)==0) {
-      //Serial.println();
-      lcd_message[i]=0;
-      ++i;
-    }
     buf++;
   }
-  while(i<LCD_MESSAGE_LENGTH) {
-    lcd_message[i++]=0;
-  }
-  //Serial.println();
-#endif
+
+  //Serial.print("message found: ");
+  //Serial.println(lcd_message);
+#endif  // HAS_LCD
 }
 
 /**
- * M203 X2000 Y5000 Z200 etc...
- * adjust the max feedrate of each axis
- */
-void adjustMaxFeedRates() { 
+   M203 X2000 Y5000 Z200 etc...
+   adjust the max feedrate of each axis
+*/
+void adjustMaxFeedRates() {
   int i;
-  for(i=0;i<NUM_MOTORS;++i) {
+  for (i = 0; i < NUM_MOTORS; ++i) {
     maxFeedRate[i] = parseNumber(MotorNames[i], maxFeedRate[i]);
   }
 }
 
-
+/**
+   M205 X4.0
+   adjust max jerk
+*/
+void parseAdvancedSettings() {
+  max_xy_jerk = parseNumber('X', max_xy_jerk);
+  max_xy_jerk = max(min(max_xy_jerk, (float)MAX_JERK), (float)0);
+}
 
 /**
- * M226 P[a] S[b] 
- * Wait for pin a to be in state b (1 or 0).  
- * If there is an LCD and P or S are missing, wait for user to press click wheel on LCD.
- * If there is no LCD, P must be specified.
- */
+   M226 P[a] S[b]
+   Wait for pin a to be in state b (1 or 0).
+   If there is an LCD and P or S are missing, wait for user to press click wheel on LCD.
+   If there is no LCD, P must be specified.
+*/
 void waitForPinState() {
 #ifdef HAS_LCD
   int pin = parseNumber('P', BTN_ENC);
 #else
-  int pin = parseNumber('P',-1);
-  if(pin==-1) return;  // no pin specified.
+  int pin = parseNumber('P', -1);
+  if (pin == -1) return; // no pin specified.
 #endif
-  int newState = parseNumber('S', 0);
-  newState = (newState==1)?HIGH:LOW;
-  //Serial.print("pausing");
-  while(digitalRead(pin)!=newState) {
+  int oldState = parseNumber('S', -1);
+  if (oldState == -1) {
+    // default: assume the pin is not in the requested state
+    oldState = digitalRead(pin);
+  } else {
+    // 0 for HIGH, anything else for LOW
+    oldState = (oldState == 0) ? HIGH : LOW;
+  }
+  Serial.print("pausing");
+#ifdef HAS_SD
+  sd_printing_paused = true;
+#endif
+
+  // while pin is in oldState (opposite of state for which we are waiting)
+  while (digitalRead(pin) == oldState) {
     SD_check();
-    //LCD_update();
+    LCD_update();
     //Serial.print(".");
   }
-  //Serial.println(" ended.");
+
+#ifdef HAS_SD
+  sd_printing_paused = false;
+#endif
+
+  Serial.println(" ended.");
 }
 
 
 /**
- * M42 P[a] S[b]
- * Set digital pin a to state b (1 or 0).  
- * default pin is LED_BUILTIN.  default state is LOW
- */
+   M42 P[a] S[b]
+   Set digital pin a to state b (1 or 0).
+   default pin is LED_BUILTIN.  default state is LOW
+*/
 void adjustPinState() {
   int pin = parseNumber('P', LED_BUILTIN);
   int newState = parseNumber('S', 0);
-  digitalWrite(pin,newState?HIGH:LOW);
+  digitalWrite(pin, newState ? HIGH : LOW);
 }
 
 
 /**
- * M300 S[a] P[b]
- * play frequency a for b milliseconds
- */
+   M300 S[a] P[b]
+   play frequency a for b milliseconds
+*/
 void parseBeep() {
-  int ms = parseNumber('P', 250);
-  int freq = parseNumber('S',60);
-  
 #ifdef HAS_LCD
-  digitalWrite(BEEPER,HIGH);
+  int ms = parseNumber('P', 250);
+  //int freq = parseNumber('S', 60);
+
+  digitalWrite(BEEPER, HIGH);
   delay(ms);
-  digitalWrite(BEEPER,LOW);
+  digitalWrite(BEEPER, LOW);
 #endif
 }
 
 
 /**
- * process commands in the serial receive buffer
- */
+   process commands in the serial receive buffer
+*/
 void processCommand() {
   if (serialBuffer[0] == ';') return;  // blank lines
-  if(!checkLineNumberAndCRCisOK()) return;  // message garbled
+  if (!checkLineNumberAndCRCisOK()) return; // message garbled
 
   if (!strncmp(serialBuffer, "UID", 3)) {
     robot_uid = atoi(strchr(serialBuffer, ' ') + 1);
@@ -793,6 +808,7 @@ void processCommand() {
     case 110:  line_number = parseNumber('N', line_number);  break;
     case 114:  where();  break;
     case 117:  parseMessage();  break;
+    case 205:  parseAdvancedSettings();  break;
     case 226:  waitForPinState();  break;
     case 300:  parseBeep();  break;
     default:   break;
@@ -815,18 +831,18 @@ void processCommand() {
     case 56:
     case 57:
     case 58:
-    case 59:  parseToolOffset(cmd-54);  break;
+    case 59:  parseToolOffset(cmd - 54);  break;
     case 90:  absolute_mode = 1;  break; // absolute mode
     case 91:  absolute_mode = 0;  break; // relative mode
     case 92:  parseTeleport();  break;
     default:  break;
   }
-  
+
   // machine style-specific codes
   cmd = parseNumber('D', -1);
   switch (cmd) {
     case  0:  jogMotors();  break;
-//    case  3:  SD_ListFiles();  break;
+    //    case  3:  SD_ListFiles();  break;
     case  4:  SD_StartPrintingFile(strchr(serialBuffer, ' ') + 1);  break; // read file
     case  5:  sayFirmwareVersionNumber();  break;
     case  6:  parseSetHome();  break;
@@ -834,20 +850,23 @@ void processCommand() {
     case  8:  reportCalibration();  break;
     case  9:  saveCalibration();  break;
     case 10:  // get hardware version
-              Serial.print(F("D10 V"));
-              Serial.println(MACHINE_HARDWARE_VERSION);
-              break;
+      Serial.print(F("D10 V"));
+      Serial.println(MACHINE_HARDWARE_VERSION);
+      break;
 #if MACHINE_STYLE == POLARGRAPH
     case 11:  makelangelo5Setup();  break;
     case 12:  recordHome();  break;
 #endif
 #ifdef MACHINE_HAS_LIFTABLE_PEN
-    case 13:  setPenAngle(parseNumber('Z',axies[2].pos));  break;
+    case 13:  setPenAngle(parseNumber('Z', axies[2].pos));  break;
 #endif
     case 14:  // get machine style
-              Serial.print(F("D14 "));
-              Serial.println(MACHINE_STYLE_NAME);
-              break;
+      Serial.print(F("D14 "));
+      Serial.println(MACHINE_STYLE_NAME);
+      break;
+#if MACHINE_STYLE == STEWART
+    case 15:  stewartDemo();  break;
+#endif
     default:  break;
   }
 }
@@ -855,11 +874,11 @@ void processCommand() {
 
 #if MACHINE_STYLE == POLARGRAPH
 /**
- * D11 makelangelo 5 specific setup call
- */
+   D11 makelangelo 5 specific setup call
+*/
 void makelangelo5Setup() {
   // if you accidentally upload m3 firmware to an m5 then upload it ONCE with this line uncommented.
-  float limits[NUM_AXIES*2];
+  float limits[NUM_AXIES * 2];
   limits[0] = 325.0;
   limits[1] = -325.0;
   limits[2] = 500;
@@ -867,14 +886,14 @@ void makelangelo5Setup() {
   limits[4] = PEN_UP_ANGLE;
   limits[5] = PEN_DOWN_ANGLE;
   adjustDimensions(limits);
-  
-  calibrateLeft=1011;
-  calibrateRight=1011;
+
+  calibrateLeft = 1011;
+  calibrateRight = 1011;
   saveCalibration();
 
   float homePos[NUM_AXIES];
   homePos[0] = 0;
-  homePos[1] = limits[2]-217.0;
+  homePos[1] = limits[2] - 217.0;
   homePos[2] = 50;
   setHome(homePos);
 
@@ -883,13 +902,13 @@ void makelangelo5Setup() {
 
 
 /**
- * D6 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn]
- * Set home position for each axis.
- */
+   D6 [Xnnn] [Ynnn] [Znnn] [Unnn] [Vnnn] [Wnnn]
+   Set home position for each axis.
+*/
 void parseSetHome() {
   int i;
   float homePos[NUM_AXIES];
-  for(i=0;i<NUM_AXIES;++i) {
+  for (i = 0; i < NUM_AXIES; ++i) {
     homePos[i] = parseNumber(AxisNames[i], axies[i].homePos);
   }
   setHome(homePos);
@@ -897,15 +916,16 @@ void parseSetHome() {
 
 
 /**
- * D0 [Lnn] [Rnn] [Unn] [Vnn] [Wnn] [Tnn]
- * Jog each motor nn steps.
- * I don't know why the latter motor names are UVWT.
- */
+   D0 [Lnn] [Rnn] [Unn] [Vnn] [Wnn] [Tnn]
+   Jog each motor nn steps.
+   I don't know why the latter motor names are UVWT.
+*/
 void jogMotors() {
   int i, j, amount;
 
   motor_engage();
-  
+  findStepDelay();
+
   for (i = 0; i < NUM_MOTORS; ++i) {
     if (MotorNames[i] == 0) continue;
     amount = parseNumber(MotorNames[i], 0);
@@ -937,9 +957,9 @@ void jogMotors() {
 
 
 /**
- * D7 [Lnnn] [Rnnn]
- * Set calibration length of each belt
- */
+   D7 [Lnnn] [Rnnn]
+   Set calibration length of each belt
+*/
 void setCalibration() {
   calibrateLeft = parseNumber('L', calibrateLeft);
   calibrateRight = parseNumber('R', calibrateRight);
@@ -948,9 +968,9 @@ void setCalibration() {
 
 
 /**
- * D8
- * Report calibration values for left and right belts
- */
+   D8
+   Report calibration values for left and right belts
+*/
 void reportCalibration() {
   Serial.print(F("D8 L"));
   Serial.print(calibrateLeft);
@@ -960,9 +980,9 @@ void reportCalibration() {
 
 
 /**
- * Compare two floats to the first decimal place.
- * return true when abs(a-b)<0.1
- */
+   Compare two floats to the first decimal place.
+   return true when abs(a-b)<0.1
+*/
 boolean equalEpsilon(float a, float b) {
   int aa = floor(a * 10);
   int bb = floor(b * 10);
@@ -975,14 +995,14 @@ boolean equalEpsilon(float a, float b) {
 
 
 void setHome(float *pos) {
-  boolean changed=false;
-  
+  boolean changed = false;
+
   int i;
-  for(i=0;i<NUM_AXIES;++i) {
-    if(!equalEpsilon(axies[i].homePos,pos[i])) changed=true;
+  for (i = 0; i < NUM_AXIES; ++i) {
+    if (!equalEpsilon(axies[i].homePos, pos[i])) changed = true;
   }
-  if(changed==true) {
-    for(i=0;i<NUM_AXIES;++i) {
+  if (changed == true) {
+    for (i = 0; i < NUM_AXIES; ++i) {
       axies[i].homePos = pos[i];
     }
     saveHome();
@@ -991,8 +1011,8 @@ void setHome(float *pos) {
 
 
 /**
- * prepares the input buffer to receive a new message and tells the serial connected device it is ready for more.
- */
+   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
@@ -1001,43 +1021,61 @@ void parser_ready() {
 
 
 /**
- * reset all tool offsets
- */
+   reset all tool offsets
+*/
 void tools_setup() {
   for (int i = 0; i < NUM_TOOLS; ++i) {
     for (int j = 0; j < NUM_AXIES; ++j) {
-      tool_offset[i][j]=0;
+      tool_offset[i][j] = 0;
     }
   }
 }
 
 
 /**
- * runs once on machine start
- */
+   runs once on machine start
+*/
 void setup() {
   // start communications
   Serial.begin(BAUD);
   
-  SD_init();
-  LCD_init();
+#ifdef HAS_WIFI
+  // Start WIFI
+  WiFi.mode(WIFI_AP);
+  Serial.println( WiFi.softAP(SSID_NAME, SSID_PASS) ? "WIFI OK":"WIFI FAILED" );
+  Serial.println( port.begin(localPort) ? "UDP OK" : "UDP FAILED" );
+  // Print the IP address
+  Serial.print("Use this URL to connect: http://");
+  Serial.print(WiFi.softAPIP());
+  Serial.print(':');
+  Serial.print(localPort);
+  Serial.println('/');
+#endif  // HAS_WIFI
+
+  SD_setup();
+  LCD_setup();
 
   loadConfig();
 
+  Serial.println("A1");
   motor_setup();
+  Serial.println("A2");
   motor_engage();
+  Serial.println("A3");
   tools_setup();
+  Serial.println("A4");
   findStepDelay();
 
+  Serial.println('B');
   //easyPWM_init();
 
   // initialize the plotter position.
   float pos[NUM_AXIES];
-  for(int i=0;i<NUM_AXIES;++i) {
-    pos[i]=0;
+  for (int i = 0; i < NUM_AXIES; ++i) {
+    pos[i] = 0;
   }
 #ifdef MACHINE_HAS_LIFTABLE_PEN
-  if(NUM_AXIES>=3) pos[2]=PEN_UP_ANGLE;
+  if (NUM_AXIES >= 3) pos[2] = PEN_UP_ANGLE;
 #endif
   teleport(pos);
 #ifdef MACHINE_HAS_LIFTABLE_PEN
@@ -1045,8 +1083,9 @@ void setup() {
 #endif
   setFeedRate(DEFAULT_FEEDRATE);
 
+  Serial.println('C');
   robot_setup();
-  
+
   // display the help at startup.
   help();
 
@@ -1055,8 +1094,8 @@ void setup() {
 
 
 /**
- * See: http://www.marginallyclever.com/2011/10/controlling-your-arduino-through-the-serial-monitor/
- */
+   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) {
@@ -1064,22 +1103,36 @@ void Serial_listen() {
     if (c == '\r') continue;
     if (sofar < MAX_BUF) serialBuffer[sofar++] = c;
     if (c == '\n') {
-      serialBuffer[sofar-1] = 0;
+      serialBuffer[sofar - 1] = 0;
 
       // echo confirmation
-      //      Serial.println(F(serialBuffer));
+      //Serial.println(serialBuffer);
 
       // do something with the command
       processCommand();
       parser_ready();
     }
   }
+
+#ifdef HAS_WIFI
+  int packetSize = port.parsePacket();
+  if (packetSize) {
+    int len = port.read(serialBuffer, MAX_BUF);
+    sofar = len;
+    if (len > 0) serialBuffer[len - 1] = 0;
+    Serial.println(serialBuffer);
+    processCommand();
+    port.beginPacket(port.remoteIP(), port.remotePort());
+    port.write("ok\r\n");
+    port.endPacket();
+  }
+#endif  // HAS_WIFI
 }
 
 
 /**
- * main loop
- */
+   main loop
+*/
 void loop() {
   Serial_listen();
   SD_check();
@@ -1093,39 +1146,21 @@ void loop() {
   }
 
 #if MACHINE_STYLE == ARM6
-/*
-  static int switchState=LOW;
-  if(digitalRead(MOTOR_5_LIMIT_SWITCH_PIN)!=switchState) {
-    switchState = !switchState;
-    Serial.println(switchState?"ON":"OFF");
-  }
+  /*
+    static int switchState=LOW;
+    if(digitalRead(MOTOR_5_LIMIT_SWITCH_PIN)!=switchState) {
+      switchState = !switchState;
+      Serial.println(switchState?"ON":"OFF");
+    }
   */
   /*
-  Serial.print( digitalRead(MOTOR_0_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
-  Serial.print( digitalRead(MOTOR_1_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
-  Serial.print( digitalRead(MOTOR_2_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
-  Serial.print( digitalRead(MOTOR_3_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
-  Serial.print( digitalRead(MOTOR_4_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
-  Serial.print( digitalRead(MOTOR_5_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
-  Serial.println();*/
-  
+    Serial.print( digitalRead(MOTOR_0_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
+    Serial.print( digitalRead(MOTOR_1_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
+    Serial.print( digitalRead(MOTOR_2_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
+    Serial.print( digitalRead(MOTOR_3_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
+    Serial.print( digitalRead(MOTOR_4_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
+    Serial.print( digitalRead(MOTOR_5_LIMIT_SWITCH_PIN)==HIGH ? "1 " : "0 ");
+    Serial.println();*/
+
 #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 DrawbotGUI.  If not, see <http://www.gnu.org/licenses/>.
-*/
diff --git a/README.txt b/README.txt
index 7d275c0..9a758f7 100644
--- a/README.txt
+++ b/README.txt
@@ -14,6 +14,9 @@ Makelangelo-firmware can also support traditional XY gantries, CoreXY gantries,
 
 Newer versions might be available at https://www.marginallyclever.com/product/makelangelo-firmware/
 
+## Note ##
+
+Unless otherwise stated in the code, the default units of measurement are millimeters (mm), kilograms (kg), and seconds (s).
 ## Instructions ##
 
 - Make sure the parent folder is called Makelangelo-firmware.
@@ -24,7 +27,7 @@ Newer versions might be available at https://www.marginallyclever.com/product/ma
   for Makelangelo 3 #define MACHINE_HARDWARE_VERSION 3 
   for Makelangelo 5 #define MACHINE_HARDWARE_VERSION 5
 - Tools > board > set type for your flavor of arduino 
-- Tools > port > set the connection for your arudion
+- Tools > port > set the connection for your arudio
 - upload
 
 For developers, please see
diff --git a/Vector3.h b/Vector3.h
index ce6a715..605c774 100644
--- a/Vector3.h
+++ b/Vector3.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
@@ -293,23 +292,4 @@ class Vector3 {
   }
 };
 
-
-
-/**
-* 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
-
+#endif
\ No newline at end of file
diff --git a/board_mks.h b/board_mks.h
index 8f85c49..7a20ef8 100644
--- a/board_mks.h
+++ b/board_mks.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 // NOTE: MKS boards are identical to RAMPS boards, except that they have a few extra features.
diff --git a/board_ramps.h b/board_ramps.h
index c16ba95..96b7434 100644
--- a/board_ramps.h
+++ b/board_ramps.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
@@ -84,10 +83,15 @@
 #define LCD_PINS_D6        27
 #define LCD_PINS_D7        29
 
+// Encoder rotation values
+#define BTN_EN1            31
+#define BTN_EN2            33
+#define BTN_ENC            35
+
 // SD card settings
-#define SDPOWER            31
-#define SDSS               33
-#define SDCARDDETECT       35
+#define SDPOWER            -1
+#define SDSS               53
+#define SDCARDDETECT       49
 
 #define KILL_PIN           41
 #endif
diff --git a/board_rumba.h b/board_rumba.h
index 2804f8e..91965e6 100644
--- a/board_rumba.h
+++ b/board_rumba.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
diff --git a/board_sanguinolulu.h b/board_sanguinolulu.h
index 95388e0..6d963ea 100644
--- a/board_sanguinolulu.h
+++ b/board_sanguinolulu.h
@@ -1,32 +1,44 @@
-#ifndef BOARD_RAMPS_H
-#define BOARD_RAMPS_H
+#ifndef BOARD_SANGUINOLULU_H
+#define BOARD_SANGUINOLULU_H
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
 #if MOTHERBOARD == BOARD_SANGUINOLULU 
-#define MAX_MOTORS                 (2)
-
+#define MAX_MOTORS                 (4)
+//x
 #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)
-
+//y
 #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.
+//z
+#define MOTOR_2_DIR_PIN           (2)
+#define MOTOR_2_STEP_PIN          (3)
+#define MOTOR_2_ENABLE_PIN        (14)
+#define MOTOR_2_LIMIT_SWITCH_PIN  (20)
+//e
+#define MOTOR_3_DIR_PIN           (0)
+#define MOTOR_3_STEP_PIN          (1)
+#define MOTOR_3_ENABLE_PIN        (14)
+// the sanguinolulu doesn't have a limit switch pin for E.
+// I've assigned it to pin 24, the e-thermistor pin.  I have no idea if this will work.
+#define MOTOR_3_LIMIT_SWITCH_PIN  (24)
 
 #define MAX_BOARD_SERVOS           (1)
 #define SERVO0_PIN                (12)
-#endif
 
+#undef HAS_LCD
+#undef HAS_SD
+
+#endif
 
-#endif  // BOARD_RAMPS_H
 
+#endif  // BOARD_SANGUINOLULU_H
diff --git a/board_teensylu.h b/board_teensylu.h
index 082eaeb..0e6fe92 100644
--- a/board_teensylu.h
+++ b/board_teensylu.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
diff --git a/board_wemos.h b/board_wemos.h
new file mode 100644
index 0000000..a35c5a5
--- /dev/null
+++ b/board_wemos.h
@@ -0,0 +1,53 @@
+#ifndef BOARD_WEMOS_H
+#define BOARD_WEMOS_H
+//------------------------------------------------------------------------------
+// Makelangelo - firmware for various robot kinematic models
+// dan@marginallycelver.com 2013-12-26
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+//------------------------------------------------------------------------------
+
+// https://www.instructables.com/id/Programming-the-WeMos-Using-Arduino-SoftwareIDE/
+
+// cnc shield pins: https://blog.protoneer.co.nz/arduino-cnc-shield/arduino-cnc-shield-scematics-v3-xx/
+// WEMOS D1 R2 pins: https://protosupplies.com/wp-content/uploads/2018/07/Wemos-D1-Pin-Differences.pdf.jpg
+// WEMOS D1 R2 board type should be set to "LOLIN(WEMOS) R1 D2 & mini"
+
+#if MOTHERBOARD == BOARD_WEMOS 
+
+// wrong board type set
+#ifndef ESP8266
+  #error "Oops!  Make sure you have 'Wemos D1 R1' selected from the 'Tools -> Boards' menu."
+#endif
+
+// actual limit is 4 but I only have the pins for the first two motors.
+// TODO add more pin definitions
+#define MAX_MOTORS                (2)
+
+#define MOTOR_0_DIR_PIN           (3)
+#define MOTOR_0_STEP_PIN          (0)
+#define MOTOR_0_ENABLE_PIN        (6)
+#define MOTOR_0_LIMIT_SWITCH_PIN  (7)   /* X min */
+
+#define MOTOR_1_DIR_PIN           (4)
+#define MOTOR_1_STEP_PIN          (1)
+#define MOTOR_1_ENABLE_PIN        (6)
+#define MOTOR_1_LIMIT_SWITCH_PIN  (8)  /* Y min */
+
+#define MAX_BOARD_SERVOS          (1)
+#define SERVO0_PIN                (14)   /* Servo 1 */
+
+#define LIMIT_SWITCH_PIN_LEFT     (MOTOR_0_LIMIT_SWITCH_PIN)
+#define LIMIT_SWITCH_PIN_RIGHT    (MOTOR_1_LIMIT_SWITCH_PIN)
+
+#define HAS_WIFI
+#define WIFI_SSID_NAME "CSIS VAN #1"    // WiFi AP SSID Name
+#define WIFI_SSID_PASS "TDPJYGZEDH123"  // WiFi AP SSID Password
+
+#define CLOCK_FREQ            (80000000L)
+
+#undef HAS_SD  
+#undef HAS_LCD
+
+#endif // MOTHERBOARD == BOARD_WEMOS 
+
+#endif  // BOARD_WEMOS_H
diff --git a/configure.h b/configure.h
index 5a1e376..d7040be 100644
--- a/configure.h
+++ b/configure.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 //------------------------------------------------------------------------------
@@ -48,6 +47,17 @@
 //#include "robot_arm5.h"
 #include "robot_arm6.h"
 
+//------------------------------------------------------------------------------
+// LCD panels supported
+//------------------------------------------------------------------------------
+
+#define HAS_LCD  // if you have an LCD panel
+#define HAS_SD   // if you have SD card support on your LCD panel (must be on panel?)
+
+// only uncomment one of these options
+//#define LCD_IS_128X64  // reprapdiscount Full Graphic Smart LCD Controller
+#define LCD_IS_SMART  // reprapdiscount Smart LCD Controller (including XXL model)
+
 //------------------------------------------------------------------------------
 // Microcontrollers supported
 //------------------------------------------------------------------------------
@@ -56,6 +66,7 @@
 #define BOARD_RAMPS        2
 #define BOARD_SANGUINOLULU 3
 #define BOARD_TEENSYLU     4
+#define BOARD_WEMOS        5
 
 #define MOTHERBOARD BOARD_RUMBA  // change this
 
@@ -63,14 +74,7 @@
 #include "board_ramps.h"
 #include "board_sanguinolulu.h"
 #include "board_teensylu.h"
-
-//------------------------------------------------------------------------------
-// LCD panels supported
-//------------------------------------------------------------------------------
-
-// only uncomment one of these options
-#define LCD_IS_128X64  // reprapdiscount Full Graphic Smart LCD Controller
-//#define LCD_IS_SMART  // reprapdiscount Smart LCD Controller (including XXL model)
+#include "board_wemos.h"
 
 //------------------------------------------------------------------------------
 // MOTOR DETAILS
@@ -109,7 +113,10 @@
 #define MAX_BUF              (64)  // What is the longest message Arduino can store?
 
 // buffering commands
+#ifndef MAX_SEGMENTS
 #define MAX_SEGMENTS         (32)  // number of line segments to buffer ahead. must be a power of two.
+#endif
+
 #define SEGMOD(x)            ((x)&(MAX_SEGMENTS-1))
 
 //------------------------------------------------------------------------------
@@ -136,11 +143,10 @@
 //#error "NUM_SERVOS + NUM_MOTORS != NUM_AXIES"
 #endif
 
-
 //------------------------------------------------------------------------------
 // EEPROM MEMORY MAP
 //------------------------------------------------------------------------------
-#define EEPROM_VERSION          8    // Increment when adding new variables
+#define FIRMWARE_VERSION        9    // Increment when adding new variables
 #define ADDR_VERSION            0                          // 0..255 (1 byte)
 #define ADDR_UUID               (ADDR_VERSION+1)           // long - 4 bytes
 #define ADDR_LIMITS             (ADDR_UUID+4)              // float - 4 bytes
@@ -154,21 +160,38 @@
 //------------------------------------------------------------------------------
 // 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)
+
+#ifdef ESP8266
+
+  #define CLOCK_FREQ            (80000000L)
+  #define MAX_COUNTER           (4294967295L)  // 32 bits
+  #define CRITICAL_SECTION_START  noInterrupts();
+  #define CRITICAL_SECTION_END    interrupts();
+
+#else  // ESP8266
+
+  // for timer interrupt control
+  #define CLOCK_FREQ            (16000000L)
+  #define MAX_COUNTER           (65536L)  // 16 bits
+  #ifndef CRITICAL_SECTION_START
+    #define CRITICAL_SECTION_START  unsigned char _sreg = SREG;  cli();
+    #define CRITICAL_SECTION_END    SREG = _sreg;
+  #endif //CRITICAL_SECTION_START
+
+#endif  // ESP8266
+
+#define TIMER_RATE            ((CLOCK_FREQ)/8)
 
 // optimize code, please
 #define FORCE_INLINE         __attribute__((always_inline)) inline
 
+// TODO a guess.  use real math here!
+// https://reprap.org/wiki/Step_rates
+// 0.9deg stepper, 20-tooth GT2 pulley, 1/16 microstepping = 160 steps/mm, 1500mm/s = 240000 steps/s
+#define CLOCK_MAX_STEP_FREQUENCY (240000)
+#define CLOCK_MIN_STEP_FREQUENCY (CLOCK_FREQ/500000U)
 
-#ifndef CRITICAL_SECTION_START
-  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG;  cli();
-  #define CRITICAL_SECTION_END    SREG = _sreg;
-#endif //CRITICAL_SECTION_START
-
+#define TIMEOUT_OK (1000)
 
 //------------------------------------------------------------------------------
 // STRUCTURES
@@ -182,53 +205,35 @@ typedef struct {
 } Axis;
 
 
-// for line()
-typedef struct {
-  long step_count;
-  long delta;  // number of steps to move
-  long absdelta;
-  int dir;
-  float delta_normalized;
-} SegmentAxis;
-
-
-typedef struct {
-  int step_pin;
-  int dir_pin;
-  int enable_pin;
-  int limit_switch_pin;
-} Motor;
-
-
-typedef struct {
-  SegmentAxis a[NUM_MOTORS+NUM_SERVOS];
-  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;
-
+#include "motor.h"
 
 //------------------------------------------------------------------------------
 // GLOBALS
 //------------------------------------------------------------------------------
 
-extern Segment line_segments[MAX_SEGMENTS];
-extern Segment *working_seg;
-extern volatile int current_segment;
-extern volatile int last_segment;
+extern float feed_rate;
 extern float acceleration;
-extern Motor motors[NUM_MOTORS+NUM_SERVOS];
-extern const char *AxisNames;
-extern const char *MotorNames;
-extern float maxFeedRate[NUM_MOTORS];
-
+extern float step_delay;
+extern int robot_uid;
+extern Axis axies[NUM_AXIES];
+extern float calibrateRight;
+extern float calibrateLeft;
+extern char serialBuffer[MAX_BUF + 1]; // Serial buffer
+extern int sofar;                      // Serial buffer progress
+
+extern void pause(const long us);
+extern void findStepDelay();
+extern void IK(const float *const axies, long *motorStepArray);
+extern int FK(long *motorStepArray,float *axies);
+extern void robot_findHome();
+extern void robot_setup();
+extern void processCommand();
+extern void parser_ready();
+extern void teleport(float *pos);
+extern void lineSafe(float *pos, float new_feed_rate);
+extern void get_end_plus_offset(float *results);
+extern void set_tool_offset(int toolID, float *pos);
+extern void reportCalibration();
+extern void where();
 
 #endif // CONFIGURE_H
diff --git a/eeprom.ino b/eeprom.cpp
similarity index 74%
rename from eeprom.ino
rename to eeprom.cpp
index 6fde590..3fb9282 100644
--- a/eeprom.ino
+++ b/eeprom.cpp
@@ -1,23 +1,23 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 //------------------------------------------------------------------------------
 // INCLUDES
 //------------------------------------------------------------------------------
+#include "configure.h"
+#include "motor.h"
 #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++)
+  for (uint16_t i = 0; i < sizeof(value); i++)
   EEPROM.write(ee++, *p++);
 }
 
@@ -28,7 +28,7 @@ void EEPROM_writeLong(int ee, long value) {
 float EEPROM_readLong(int ee) {
   long value = 0;
   byte* p = (byte*)(void*)&value;
-  for (int i = 0; i < sizeof(value); i++)
+  for (uint16_t i = 0; i < sizeof(value); i++)
   *p++ = EEPROM.read(ee++);
   return value;
 }
@@ -84,7 +84,7 @@ void loadLimits() {
  * @param limits NUM_AXIES pairs of floats.  each pair is one float for max limit and one for min limit.
  */
 void adjustDimensions(float *limits) {
-  int i,j;
+  int i,j=0;
   int changed=0;
   float v;
   for(i=0;i<NUM_AXIES;++i) {
@@ -159,10 +159,10 @@ void loadCalibration() {
  */
 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)
+  if( versionNumber != FIRMWARE_VERSION ) {
+    // If not the current FIRMWARE_VERSION or the FIRMWARE_VERSION is sullied (i.e. unknown data)
     // Update the version number
-    EEPROM.write(ADDR_VERSION,EEPROM_VERSION);
+    EEPROM.write(ADDR_VERSION,FIRMWARE_VERSION);
 #if MAKELANGELO_HARDWARE_VERSION == 5 || MAKELANGELO_HARDWARE_VERSION == 6
     adjustDimensions(50,-50,-32.5,32.5);
     saveCalibration();
@@ -175,20 +175,3 @@ void loadConfig() {
   loadHome();
   loadCalibration();
 }
-
-/**
- * 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/eeprom.h b/eeprom.h
new file mode 100644
index 0000000..332f99e
--- /dev/null
+++ b/eeprom.h
@@ -0,0 +1,59 @@
+#ifndef EEPROM_H
+#define EEPROM_H
+//------------------------------------------------------------------------------
+// Makelangelo - firmware for various robot kinematic models
+// dan@marginallycelver.com 2013-12-26
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+//------------------------------------------------------------------------------
+
+/**
+ * 
+ */
+char loadVersion();
+
+/**
+ * 
+ */
+void saveUID();
+
+/**
+ * 
+ */
+void saveLimits();
+
+/**
+ * 
+ */
+void loadLimits();
+
+/**
+ * @param limits NUM_AXIES pairs of floats.  each pair is one float for max limit and one for min limit.
+ */
+void adjustDimensions(float *limits);
+
+/**
+ * 
+ */
+void saveHome();
+
+/**
+ * 
+ */
+void loadHome();
+
+/**
+ *
+ */
+void saveCalibration();
+
+/**
+ * 
+ */
+void loadCalibration();
+
+/**
+ * 
+ */
+void loadConfig();
+
+#endif // EEPROM_H
\ No newline at end of file
diff --git a/lcd.cpp b/lcd.cpp
new file mode 100644
index 0000000..7f9bf4e
--- /dev/null
+++ b/lcd.cpp
@@ -0,0 +1,881 @@
+//------------------------------------------------------------------------------
+// Makelangelo - firmware for various robot kinematic models
+// dan@marginallycelver.com 2013-12-26
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+//------------------------------------------------------------------------------
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+#include "configure.h"
+#include "lcd.h"
+#include "sdcard.h"
+
+
+#ifdef HAS_LCD
+
+#ifdef LCD_IS_SMART
+#include <LiquidCrystal.h>
+#endif
+#ifdef LCD_IS_128X64
+#include <Arduino.h>
+#include <U8glib.h>
+#include <SPI.h>
+#include <Wire.h>
+#include "dogm_font_data_6x9.h"
+#endif
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+#ifdef LCD_IS_SMART
+LiquidCrystal lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5, LCD_PINS_D6, LCD_PINS_D7);
+#endif
+#ifdef LCD_IS_128X64
+// This is not ideal - will not work when board models change.
+U8GLIB_ST7920_128X64_1X u8g(LCD_PINS_D4, LCD_PINS_ENABLE, LCD_PINS_RS);
+#endif
+
+uint32_t lcd_draw_delay  = 0;
+
+int lcd_rot_old  = 0;
+int lcd_turn     = 0;
+int lcd_posx = 0, lcd_posy = 0;
+char lcd_click_old = HIGH;
+char lcd_click_now = false;
+uint8_t speed_adjust = 100;
+char lcd_message[LCD_MESSAGE_LENGTH + 1];
+char lcd_dirty=0;
+
+int menu_position_sum = 0, menu_position = 0, screen_position = 0, num_menu_items = 0, ty, screen_end;
+
+
+void (*current_menu)();
+
+
+#ifdef LCD_IS_128X64
+/**
+   Made with Marlin Bitmap Converter
+   http://marlinfw.org/tools/u8glib/converter.html
+
+   This bitmap from the file 'icon_mono.png'
+   Filesize: 906 Bytes
+   Size bitmap: 108 bytes
+*/
+#define logoImageWidth 32
+#define logoImageHeight 32
+
+const unsigned char logoImage [] PROGMEM = {
+  0x00, 0x60, 0x00, 0x00, // .........##.....................
+  0x00, 0xF0, 0x00, 0x00, // ........####....................
+  0x03, 0x10, 0x00, 0x00, // ......##...#....................
+  0x01, 0x60, 0x00, 0x00, // .......#.##.....................
+  0x01, 0xC0, 0x00, 0x00, // .......###......................
+  0x00, 0x9F, 0xF0, 0x00, // ........#..#########............
+  0x00, 0xF0, 0x3C, 0x00, // ........####......####..........
+  0x01, 0xC0, 0x3F, 0x00, // .......###........######........
+  0x03, 0x00, 0x1F, 0x80, // ......##...........######.......
+  0x06, 0x00, 0x1F, 0xC0, // .....##............#######......
+  0x0C, 0x02, 0x0F, 0xE0, // ....##........#.....#######.....
+  0x08, 0x06, 0x0F, 0xE0, // ....#........##.....#######.....
+  0x18, 0x02, 0x0F, 0xF0, // ...##.........#.....########....
+  0x10, 0x02, 0x0F, 0xF0, // ...#..........#.....########....
+  0x31, 0x80, 0x07, 0xF0, // ..##...##............#######....
+  0x23, 0x80, 0x07, 0xF8, // ..#...###............########...
+  0x23, 0x00, 0x07, 0xF8, // ..#...##.............########...
+  0x23, 0x00, 0x07, 0xF8, // ..#...##.............########...
+  0x23, 0x00, 0x07, 0xF8, // ..#...##.............########...
+  0x21, 0x80, 0x07, 0xF8, // ..#....##............########...
+  0x20, 0x00, 0x07, 0xF8, // ..#..................########...
+  0x30, 0x00, 0x37, 0xF8, // ..##..............##.########...
+  0x30, 0x00, 0x63, 0xF0, // ..##.............##...######....
+  0x10, 0x00, 0x63, 0xF0, // ...#.............##...######....
+  0x18, 0x00, 0x63, 0xF0, // ...##............##...######....
+  0x08, 0x60, 0x37, 0xE0, // ....#....##.......##.######.....
+  0x04, 0x20, 0x0F, 0xC0, // .....#....#.........######......
+  0x06, 0x00, 0x1F, 0xC0, // .....##............#######......
+  0x03, 0x00, 0x1F, 0x80, // ......##...........######.......
+  0x00, 0xC0, 0x3E, 0x00, // ........##........#####.........
+  0x00, 0x70, 0x3C, 0x00, // .........###......####..........
+  0x00, 0x1F, 0xE0, 0x00 // ...........########.............
+};
+#endif
+
+//------------------------------------------------------------------------------
+// MACROS
+//------------------------------------------------------------------------------
+
+/**
+   Clear the screen
+*/
+#ifdef LCD_IS_SMART
+inline void LCD_clear() {
+  for (int i = 0; i < LCD_MESSAGE_LENGTH; ++i) lcd_message[i] = ' ';
+  lcd_message[LCD_MESSAGE_LENGTH - 1] = 0;
+}
+#endif
+#ifdef LCD_IS_128X64
+inline void LCD_clear() {
+  //u8g.firstPage();
+  //while( u8g.nextPage() );
+  u8g.setColorIndex(0);
+  u8g.drawBox(0, 0, LCD_PIXEL_WIDTH, LCD_PIXEL_HEIGHT);
+  u8g.setColorIndex(1);
+}
+#endif
+
+/**
+   print text to the LCD
+*/
+#ifdef LCD_IS_SMART
+inline void LCD_advance() {
+  lcd_posx++;
+  if (lcd_posx >= LCD_WIDTH) {
+    lcd_posx = 0;
+    lcd_posy++;
+    if (lcd_posy >= LCD_HEIGHT) {
+      lcd_posy = 0;
+    }
+  }
+}
+
+inline void LCD_print(const char *x) {
+  while ((*x) != 0) {
+    lcd_message[lcd_posy * LCD_WIDTH + lcd_posx] = *x;
+    x++;
+    LCD_advance();
+  }
+}
+
+//inline void LCD_print(const __FlashStringHelper *x) {
+//  LCD_print((const PROGMEM char*)x);
+//}
+
+// see https://en.wikibooks.org/wiki/C_Programming/stdlib.h/itoa
+inline void LCD_print(long x) {
+  char b[12];
+  itoa(x, b, 10);
+  LCD_print(b);
+}
+
+inline void LCD_print(int x) {
+  char b[12];
+  itoa(x, b, 10);
+  LCD_print(b);
+}
+
+inline void LCD_print(const char x) {
+  lcd_message[lcd_posy * LCD_WIDTH + lcd_posx] = x;
+  LCD_advance();
+}
+#endif
+#ifdef LCD_IS_128X64
+#define LCD_print      u8g.print
+#endif
+
+
+/**
+   Set the row/column of text at which to begin printing
+*/
+#ifdef LCD_IS_SMART
+#define LCD_setCursor(x,y)   {lcd_posx=x; lcd_posy=y;}
+#endif
+#ifdef LCD_IS_128X64
+#define LCD_setCursor(x,y)   u8g.setPrintPos(((x)+1)*FONT_WIDTH,((y)+1)*FONT_HEIGHT)
+#endif
+
+
+
+// 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;  \
+    screen_end = screen_position + LCD_HEIGHT;  \
+    current_menu=new_menu;  \
+    return;  \
+  }
+
+#define MENU_LABEL(menu_label) \
+  MENU_ITEM_START(menu_label) \
+  MENU_ITEM_END()
+
+#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()
+
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+
+void LCD_status_menu();
+void LCD_main_menu();
+void LCD_drawSplash();
+void LCD_driveX();
+void LCD_driveY();
+void LCD_driveZ();
+void LCD_driveF();
+void LCD_print_float(float);
+void LCD_print_long(long);
+void LCD_refresh_display();
+void LCD_draw_border();
+
+
+void LCD_read() {
+  // detect potentiometer changes
+  int rot = ((digitalRead(BTN_EN1) == LOW) << BLEN_A)
+          | ((digitalRead(BTN_EN2) == LOW) << BLEN_B);
+  // potentiometer uses grey code.  Pattern is 0 3
+  if (lcd_rot_old != rot) {
+    switch (rot) {
+      case ENCROT0:
+        if ( lcd_rot_old == ENCROT3 ) lcd_turn++;
+        else if ( lcd_rot_old == ENCROT1 ) lcd_turn--;
+        break;
+      case ENCROT1:
+        if ( lcd_rot_old == ENCROT0 ) lcd_turn++;
+        else if ( lcd_rot_old == ENCROT2 ) lcd_turn--;
+        break;
+      case ENCROT2:
+        if ( lcd_rot_old == ENCROT1 ) lcd_turn++;
+        else if ( lcd_rot_old == ENCROT3 ) lcd_turn--;
+        break;
+      case ENCROT3:
+        if ( lcd_rot_old == ENCROT2 ) lcd_turn++;
+        else if ( lcd_rot_old == ENCROT0 ) lcd_turn--;
+        break;
+    }
+    //if(lcd_turn !=0) Serial.print(lcd_turn>0?'+':'-');  // for debugging potentiometer
+    //else Serial.print(' ');
+    //Serial.println(lcd_turn);  // for debugging potentiometer
+  }
+  //Serial.println(rot);  // for debugging potentiometer
+
+  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_pause() {
+  // TODO: if pen down before pause, lift pen on pause, lower pen on unpause.
+  // problem: machine does not know what is pen up or down.
+#ifdef HAS_SD
+  sd_printing_paused = (sd_printing_paused == true ? false : true);
+#endif
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_stop() {
+#ifdef HAS_SD
+  sd_printing_now = false;
+#endif
+  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() {
+  robot_findHome();
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_this_is_home() {
+  float offset[NUM_AXIES];
+  for (int i = 0; i < NUM_AXIES; ++i) offset[i] = axies[i].homePos;
+  teleport(offset);
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_go_home() {
+  float homes[NUM_AXIES];
+  for (int i = 0; i < NUM_AXIES; ++i) homes[i] = axies[i].homePos;
+  lineSafe( homes, DEFAULT_FEEDRATE );
+  MENU_GOTO(LCD_main_menu);
+}
+
+
+void LCD_drive_menu() {
+  MENU_START
+  MENU_SUBMENU("Back", LCD_main_menu);
+  MENU_ACTION("Disable motors", LCD_disable_motors);
+  MENU_ACTION("Enable motors", LCD_enable_motors);
+  MENU_SUBMENU("X", LCD_driveX);
+  MENU_SUBMENU("Y", LCD_driveY);
+  MENU_SUBMENU("Z", LCD_driveZ);
+  MENU_SUBMENU("Feedrate", LCD_driveF);
+  MENU_END
+}
+
+
+void LCD_driveX() {
+  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
+
+  float offset[NUM_AXIES];
+  get_end_plus_offset(offset);
+
+  if (lcd_turn) {
+    offset[0] += lcd_turn > 0 ? 1 : -1;
+    lineSafe(offset, feed_rate);
+  }
+
+  LCD_setCursor( 0, 0);
+  LCD_print('X');
+  LCD_print_float(offset[0]);
+}
+
+
+void LCD_driveY() {
+  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
+
+  float offset[NUM_AXIES];
+  get_end_plus_offset(offset);
+
+  if (lcd_turn) {
+    offset[1] += lcd_turn > 0 ? 1 : -1;
+    lineSafe(offset, feed_rate);
+  }
+
+  LCD_setCursor( 0, 0);
+  LCD_print('Y');
+  LCD_print_float(offset[1]);
+}
+
+
+void LCD_driveZ() {
+  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
+
+  float offset[NUM_AXIES];
+  get_end_plus_offset(offset);
+
+  if (lcd_turn) {
+    // protect servo, don't drive beyond physical limits
+    offset[2] += lcd_turn > 0 ? 1 : -1;
+    lineSafe(offset, feed_rate);
+  }
+
+  LCD_setCursor( 0, 0);
+  LCD_print('Z');
+  LCD_print_float(offset[2]);
+}
+
+
+void LCD_driveF() {
+  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
+
+  if (lcd_turn) {
+    // protect servo, don't drive beyond physical limits
+    float newF = feed_rate + lcd_turn > 0 ? 1 : -1;
+    if (newF < MIN_FEEDRATE) newF = MIN_FEEDRATE;
+    if (newF > MAX_FEEDRATE) newF = MAX_FEEDRATE;
+    // move
+    feed_rate = newF;
+  }
+
+  LCD_setCursor( 0, 0);
+  LCD_print('F');
+  LCD_print_float(feed_rate);
+}
+
+
+void LCD_start_menu() {
+#ifdef HAS_SD
+  if (!sd_inserted) MENU_GOTO(LCD_main_menu);
+
+  /*
+    Serial.print(menu_position    );  Serial.print("\t");  // 0
+    Serial.print(menu_position_sum);  Serial.print("\t");  // 1
+    Serial.print(screen_position  );  Serial.print("\t");  // 0
+    Serial.print(num_menu_items   );  Serial.print("\n");  // 8
+  */
+  if(lcd_turn!=0 || lcd_click_now==1) lcd_dirty=1;
+
+  if(lcd_dirty==1) {
+    //long t0=micros();
+    
+    MENU_START
+    MENU_SUBMENU("Back", LCD_main_menu);
+    
+    root.rewindDirectory();
+    while ( true ) {
+      //long tStart = millis();
+      File entry = root.openNextFile();
+      //long tEnd = millis();
+      //Serial.print(tEnd-tStart);
+      //Serial.print('\t');
+      if (!entry) {
+        // no more files, return to the first file in the directory
+        break;
+      }
+      const char *filename = entry.name();
+      //Serial.print( entry.isDirectory()?">":" " );
+      //Serial.println(filename);
+      if (!entry.isDirectory() && filename[0] != '_') {
+        MENU_ITEM_START(filename)
+        if (menu_position == ty && lcd_click_now) {
+          lcd_click_now = false;
+          SD_StartPrintingFile(filename);
+          MENU_GOTO(LCD_status_menu);
+        }
+        MENU_ITEM_END()
+      }
+      entry.close();
+    }
+    MENU_END
+    
+    //long t1=micros();
+    //Serial.print(menu_position,DEC);
+    //Serial.print(' ');
+    //Serial.print(num_menu_items,DEC);
+    //Serial.print(' ');
+    //Serial.print(t1-t0);
+    //Serial.println();
+    lcd_dirty=0;
+  }
+  
+#else
+  // i don't know how you got here surfing the LCD panel.
+  // someone messed up in the logic.  Go back to the main menu.
+  MENU_GOTO(LCD_main_menu);
+#endif
+}
+
+
+void draw_border(int width, int height, int landscape) {
+#if NUM_AXIES == 3
+  LCD_clear();
+  LCD_setCursor(0, 0);
+  LCD_print("Drawing border...");
+
+  width /= 2;
+  height /= 2;
+
+  if (landscape == 1) {
+    // swap the two values.
+    int temp = width;
+    width = height;
+    height = temp;
+  }
+
+  // get start position
+  float start[NUM_AXIES];
+  get_end_plus_offset(start);
+
+  float pos[NUM_AXIES];
+  // lift pen at current position
+  pos[0] = start[0];
+  pos[1] = start[1];
+  pos[2] = PEN_UP_ANGLE;
+  lineSafe( pos, feed_rate );
+  // move to first corner
+  pos[0] = -width;  pos[1] =  height;  lineSafe( pos, feed_rate );
+  // lower pen
+  pos[2] = PEN_DOWN_ANGLE;
+  lineSafe( pos, feed_rate );
+  // move around border
+  pos[0] =  width;  pos[1] =  height;  lineSafe( pos, feed_rate );
+  pos[0] =  width;  pos[1] = -height;  lineSafe( pos, feed_rate );
+  pos[0] = -width;  pos[1] = -height;  lineSafe( pos, feed_rate );
+  pos[0] = -width;  pos[1] =  height;  lineSafe( pos, feed_rate );
+  // lift pen
+  pos[2] = PEN_UP_ANGLE;  lineSafe( pos, feed_rate );
+
+  // return to start position
+  lineSafe( start, feed_rate );
+#endif // NUM_AXIES
+  MENU_GOTO(LCD_draw_border);
+}
+
+void draw_A2_portrait() {
+  draw_border(420, 594, 0);
+}
+void draw_A3_portrait() {
+  draw_border(297, 420, 0);
+}
+void draw_A4_portrait() {
+  draw_border(210, 297, 0);
+}
+void draw_A5_portrait() {
+  draw_border(148, 210, 0);
+}
+void draw_USletter_portrait() {
+  draw_border(216, 279, 0);
+}
+void draw_USlegal_portrait() {
+  draw_border(216, 356, 0);
+}
+
+void draw_A2_landscape() {
+  draw_border(420, 594, 1);
+}
+void draw_A3_landscape() {
+  draw_border(297, 420, 1);
+}
+void draw_A4_landscape() {
+  draw_border(210, 297, 1);
+}
+void draw_A5_landscape() {
+  draw_border(148, 210, 1);
+}
+void draw_USletter_landscape() {
+  draw_border(216, 279, 1);
+}
+void draw_USlegal_landscape() {
+  draw_border(216, 356, 1);
+}
+
+void LCD_draw_border() {
+  MENU_START
+  MENU_SUBMENU("Back", LCD_main_menu);
+  MENU_ACTION("A2 portrait", draw_A2_portrait);
+  MENU_ACTION("A3 portrait", draw_A3_portrait);
+  MENU_ACTION("A4 portrait", draw_A4_portrait);
+  MENU_ACTION("A5 portrait", draw_A5_portrait);
+  MENU_ACTION("US legal portrait", draw_USlegal_portrait);
+  MENU_ACTION("US letter portrait", draw_USletter_portrait);
+
+  MENU_ACTION("A2 landscape", draw_A2_landscape);
+  MENU_ACTION("A3 landscape", draw_A3_landscape);
+  MENU_ACTION("A4 landscape", draw_A4_landscape);
+  MENU_ACTION("A5 landscape", draw_A5_landscape);
+  MENU_ACTION("US legal landscape", draw_USlegal_landscape);
+  MENU_ACTION("US letter landscape", draw_USletter_landscape);
+  MENU_END
+}
+
+
+void LCD_update_long(char *label, 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(label);
+    LCD_setCursor(0, 1);
+    LCD_print_long(value);
+  } while ( !lcd_click_now );
+}
+
+
+void LCD_update_float(char *label, float &value, float stepSize = 0.01) {
+  LCD_clear();
+  do {
+    LCD_read();
+    if ( lcd_turn ) {
+      value += lcd_turn > 0 ? stepSize : -stepSize;
+      lcd_turn = 0;
+    }
+    LCD_setCursor(0, 0);
+    LCD_print(label);
+    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(' ');
+
+  LCD_print(left);
+  LCD_print('.');
+  LCD_print(right);
+}
+#endif  // HAS_LCD
+
+
+
+
+void LCD_update() {
+#ifdef HAS_LCD
+  LCD_read();
+
+  if (millis() >= lcd_draw_delay ) {
+    lcd_draw_delay = millis() + LCD_DRAW_DELAY;
+
+    //Serial.print(lcd_turn,DEC);
+    //Serial.print('\t');  Serial.print(menu_position,DEC);
+    //Serial.print('\t');  Serial.print(menu_position_sum,DEC);
+    //Serial.print('\t');  Serial.print(screen_position,DEC);
+    //Serial.print('\t');  Serial.print(screen_end,DEC);
+    //Serial.print('\t');  Serial.print(num_menu_items,DEC);
+    //Serial.print('\n');
+
+    // update the menu position
+    if ( lcd_turn && num_menu_items > 1 ) {
+      int originalPosition = menu_position_sum / LCD_TURN_PER_MENU;
+      menu_position_sum += lcd_turn;
+
+      menu_position = menu_position_sum / LCD_TURN_PER_MENU;
+      if (menu_position > num_menu_items - 1) menu_position = num_menu_items - 1;
+      if (menu_position < 0) menu_position = 0;
+
+      if (originalPosition != menu_position) {
+        LCD_clear();
+      }
+
+      //Serial.println(menu_position);
+
+      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;
+    }
+    
+    // draw the new screen contents
+    #ifdef LCD_IS_128X64
+    u8g.firstPage();
+    do {
+    #endif
+      (*current_menu)();
+    #ifdef LCD_IS_128X64
+    } while(u8g.nextPage());
+    #endif
+
+    lcd_turn = 0;  // reset.  must come after (*current_menu)() because it might be used there.  (damn globals...)
+  }
+  LCD_refresh_display();
+#endif  // HAS_LCD
+}
+
+
+void LCD_refresh_display() {
+#ifdef HAS_LCD
+#ifdef LCD_IS_SMART
+  char temp[LCD_MESSAGE_LENGTH];
+  memcpy(temp + (LCD_WIDTH * 0), lcd_message + (LCD_WIDTH * 0), LCD_WIDTH);
+  memcpy(temp + (LCD_WIDTH * 1), lcd_message + (LCD_WIDTH * 2), LCD_WIDTH);
+  memcpy(temp + (LCD_WIDTH * 2), lcd_message + (LCD_WIDTH * 1), LCD_WIDTH);
+  memcpy(temp + (LCD_WIDTH * 3), lcd_message + (LCD_WIDTH * 3), LCD_WIDTH);
+  for (int i = 0; i < LCD_MESSAGE_LENGTH-1; ++i) {
+    lcd.write(temp[i]);
+  }
+#endif
+#endif  // HAS_LCD
+}
+
+
+void LCD_main_menu() {
+#ifdef HAS_LCD
+  lcd_dirty=1;
+  
+  MENU_START
+
+  MENU_SUBMENU("Back", LCD_status_menu);
+#ifdef HAS_SD
+  if (!sd_printing_now) {
+#endif
+#if MACHINE_HARDWARE_VERSION  == 5
+    MENU_ACTION("Find home", LCD_find_home);
+#else
+    MENU_ACTION("This is home", LCD_this_is_home);
+    MENU_ACTION("Go home", LCD_go_home);
+#endif
+#ifdef HAS_SD
+    if (sd_inserted) {
+      MENU_SUBMENU("Print from SD card", LCD_start_menu);
+    } else {
+      MENU_LABEL("No SD card");
+    }
+#endif
+#if NUM_AXIES == 3
+    MENU_SUBMENU("Draw border", LCD_draw_border);
+#endif
+    MENU_SUBMENU("Drive", LCD_drive_menu);
+#ifdef HAS_SD
+  } else {
+    if (sd_printing_paused) {
+      MENU_ACTION("Unpause", LCD_pause);
+    } else {
+      MENU_ACTION("Pause", LCD_pause);
+    }
+    MENU_ACTION("Stop", LCD_stop);
+  }
+#endif
+  MENU_END
+#endif  // HAS_LCD
+}
+
+
+// display the current machine position and feed rate on the LCD.
+void LCD_status_menu() {
+#ifdef HAS_LCD
+  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;
+  }
+  LCD_setCursor( 0, 0);
+
+  // update the current status
+  float offset[NUM_AXIES];
+  get_end_plus_offset(offset);
+
+  LCD_setCursor(0, 0);  LCD_print('X');  LCD_print_float(offset[0]);
+  LCD_setCursor(9, 0);  LCD_print('Z');  LCD_print_float(offset[2]);
+#if MACHINE_STYLE == POLARGRAPH && defined(USE_LIMIT_SWITCH)
+  LCD_setCursor(8, 0);  LCD_print(( digitalRead(LIMIT_SWITCH_PIN_LEFT) == LOW ) ? '*' : ' ');
+#endif
+
+  LCD_setCursor(0, 1);  LCD_print('Y');  LCD_print_float(offset[1]);
+  LCD_setCursor(9, 1);  LCD_print('F');  LCD_print_long(speed_adjust);  LCD_print('%');
+#if MACHINE_STYLE == POLARGRAPH && defined(USE_LIMIT_SWITCH)
+  LCD_setCursor(8, 1);  LCD_print(( digitalRead(LIMIT_SWITCH_PIN_RIGHT) == LOW ) ? '*' : ' ');
+#endif
+
+  //LCD_setCursor( 1, 15);
+  //if (sd_printing_now == true && sd_printing_paused==false) {
+  //if (sd_printing_now == true) {
+    //LCD_print_float(sd_percent_complete);
+    //LCD_print('%');
+  //} else {
+    //LCD_print("          ");
+  //}
+  MENU_END
+#endif  // HAS_LCD
+}
+
+
+// initialize the Smart controller LCD panel
+void LCD_setup() {
+#ifdef HAS_LCD
+
+#ifdef LCD_IS_SMART
+  lcd.begin(LCD_WIDTH, LCD_HEIGHT);
+#endif
+#ifdef LCD_IS_128X64
+  u8g.begin();
+  u8g.disableCursor();
+  u8g.setFont(u8g_font_6x9);
+  //  u8g.setFont(u8g_font_helvR08);
+#endif
+
+  pinMode(BEEPER, OUTPUT);
+  digitalWrite(BEEPER, LOW);
+
+  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 */
+
+  LCD_drawSplash();
+  
+  LCD_read();
+  lcd_turn=0;
+  LCD_clear();
+#endif  // HAS_LCD
+}
+
+
+#ifdef HAS_LCD
+void LCD_drawSplash() {
+  // splash screen
+  char message[LCD_WIDTH];
+  char mhv[12];
+  itoa(MACHINE_HARDWARE_VERSION, mhv, 10);
+  char *ptr = message;
+  strcpy(ptr, MACHINE_STYLE_NAME );  ptr += strlen(MACHINE_STYLE_NAME);
+  strcpy(ptr, " v" );                ptr += strlen(" v");
+  strcpy(ptr, mhv );                 ptr += strlen(mhv);
+
+  int x = (LCD_WIDTH - strlen(message)) / 2;
+  int y = LCD_HEIGHT - 2;
+  int x2 = (LCD_WIDTH - strlen("marginallyclever.com")) / 2;
+  int y2 = y + 1;
+
+#ifdef LCD_IS_128X64
+  u8g.firstPage();
+  do {
+#endif
+    LCD_clear();
+#ifdef LCD_IS_128X64
+    u8g.drawBitmapP((LCD_PIXEL_WIDTH - logoImageWidth) / 2, 0, logoImageWidth / 8, logoImageHeight, logoImage);
+#endif
+    LCD_setCursor(x, y);
+    LCD_print(message);
+    LCD_setCursor(x2, y2);
+    LCD_print("marginallyclever.com");
+#ifdef LCD_IS_128X64
+  } while (u8g.nextPage());
+#endif
+  LCD_refresh_display();
+  delay(2500);
+}
+#endif  // HAS_LCD
diff --git a/lcd.h b/lcd.h
index 818facd..4e4f038 100644
--- a/lcd.h
+++ b/lcd.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 //------------------------------------------------------------------------------
@@ -12,10 +11,7 @@
 //------------------------------------------------------------------------------
 #ifdef HAS_LCD
 
-// only uncomment one of these options
-#define LCD_IS_128X64  // reprapdiscount Full Graphic Smart LCD Controller
-//#define LCD_IS_SMART  // reprapdiscount Smart LCD Controller (including XXL model)
-
+#include <Arduino.h>
 
 //----------------------------------------------------
 
@@ -29,8 +25,8 @@
 #define FONT_WIDTH         6
 
 // # of characters
-#define LCD_HEIGHT         (LCD_PIXEL_HEIGHT/FONT_HEIGHT)
-#define LCD_WIDTH          (LCD_PIXEL_WIDTH/FONT_WIDTH)
+#define LCD_HEIGHT         (LCD_PIXEL_HEIGHT/FONT_HEIGHT)  // 64/9=7
+#define LCD_WIDTH          (LCD_PIXEL_WIDTH/FONT_WIDTH)    // 128/6=21
 
 #include "dogm_font_data_6x9.h"
 
@@ -68,12 +64,16 @@
 #define ENCROT2            3
 #define ENCROT3            1
 
-#define LCD_MESSAGE_LENGTH (LCD_HEIGHT * (LCD_WIDTH + 1))  // we have two lines of 20 characters avialable in 7.16
+#define LCD_MESSAGE_LENGTH (LCD_HEIGHT * LCD_WIDTH + 1)  // we have two lines of 20 characters avialable in 7.16
 #define LCD_DRAW_DELAY     (100)
 #define LCD_TURN_PER_MENU  (5)
 
 extern char lcd_message[LCD_MESSAGE_LENGTH+1];
+extern uint8_t speed_adjust;
 
 #endif // HAS_LCD
 
+extern void LCD_update();
+extern void LCD_setup();
+
 #endif // LCD_H
diff --git a/lcd.ino b/lcd.ino
deleted file mode 100644
index 407ed42..0000000
--- a/lcd.ino
+++ /dev/null
@@ -1,688 +0,0 @@
-//------------------------------------------------------------------------------
-// Makelangelo - firmware for various robot kinematic models
-// dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
-//------------------------------------------------------------------------------
-
-//------------------------------------------------------------------------------
-// INCLUDES
-//------------------------------------------------------------------------------
-#ifdef HAS_LCD
-#include "lcd.h"
-
-#include "sdcard.h"
-
-#ifdef LCD_IS_SMART
-#include <LiquidCrystal.h>
-#endif
-#ifdef LCD_IS_128X64
-#include <Arduino.h>
-#include <U8glib.h>
-#include <SPI.h>
-#include <Wire.h>
-#include "dogm_font_data_6x9.h"
-#endif
-
-//------------------------------------------------------------------------------
-// GLOBALS
-//------------------------------------------------------------------------------
-#ifdef LCD_IS_SMART
-LiquidCrystal lcd(LCD_PINS_RS, LCD_PINS_ENABLE, LCD_PINS_D4, LCD_PINS_D5, LCD_PINS_D6, LCD_PINS_D7);
-#endif
-#ifdef LCD_IS_128X64
-// This is not ideal - will not work when board models change.
-U8GLIB_ST7920_128X64_1X u8g(LCD_PINS_D4,LCD_PINS_ENABLE,LCD_PINS_RS);
-
-
-/**
- * Made with Marlin Bitmap Converter
- * http://marlinfw.org/tools/u8glib/converter.html
- *
- * This bitmap from the file 'icon_mono.png'
- * Filesize: 906 Bytes
- * Size bitmap: 108 bytes
- */
-#define logoImageWidth 32
-#define logoImageHeight 32
-
-const unsigned char logoImage [] PROGMEM= {
-  0x00,0x60,0x00,0x00, // .........##.....................
-  0x00,0xF0,0x00,0x00, // ........####....................
-  0x03,0x10,0x00,0x00, // ......##...#....................
-  0x01,0x60,0x00,0x00, // .......#.##.....................
-  0x01,0xC0,0x00,0x00, // .......###......................
-  0x00,0x9F,0xF0,0x00, // ........#..#########............
-  0x00,0xF0,0x3C,0x00, // ........####......####..........
-  0x01,0xC0,0x3F,0x00, // .......###........######........
-  0x03,0x00,0x1F,0x80, // ......##...........######.......
-  0x06,0x00,0x1F,0xC0, // .....##............#######......
-  0x0C,0x02,0x0F,0xE0, // ....##........#.....#######.....
-  0x08,0x06,0x0F,0xE0, // ....#........##.....#######.....
-  0x18,0x02,0x0F,0xF0, // ...##.........#.....########....
-  0x10,0x02,0x0F,0xF0, // ...#..........#.....########....
-  0x31,0x80,0x07,0xF0, // ..##...##............#######....
-  0x23,0x80,0x07,0xF8, // ..#...###............########...
-  0x23,0x00,0x07,0xF8, // ..#...##.............########...
-  0x23,0x00,0x07,0xF8, // ..#...##.............########...
-  0x23,0x00,0x07,0xF8, // ..#...##.............########...
-  0x21,0x80,0x07,0xF8, // ..#....##............########...
-  0x20,0x00,0x07,0xF8, // ..#..................########...
-  0x30,0x00,0x37,0xF8, // ..##..............##.########...
-  0x30,0x00,0x63,0xF0, // ..##.............##...######....
-  0x10,0x00,0x63,0xF0, // ...#.............##...######....
-  0x18,0x00,0x63,0xF0, // ...##............##...######....
-  0x08,0x60,0x37,0xE0, // ....#....##.......##.######.....
-  0x04,0x20,0x0F,0xC0, // .....#....#.........######......
-  0x06,0x00,0x1F,0xC0, // .....##............#######......
-  0x03,0x00,0x1F,0x80, // ......##...........######.......
-  0x00,0xC0,0x3E,0x00, // ........##........#####.........
-  0x00,0x70,0x3C,0x00, // .........###......####..........
-  0x00,0x1F,0xE0,0x00  // ...........########.............
-};
-#endif
-
-//------------------------------------------------------------------------------
-// MACROS
-//------------------------------------------------------------------------------
-
-/**
- * Clear the screen
- */
-#ifdef LCD_IS_SMART
-#define LCD_clear()      lcd.clear()
-#endif
-#ifdef LCD_IS_128X64
-void LCD_clear() {
-  //u8g.firstPage(); 
-  //while( u8g.nextPage() );
-  u8g.setColorIndex(0);
-  u8g.drawBox(0,0,LCD_PIXEL_WIDTH,LCD_PIXEL_HEIGHT);
-  u8g.setColorIndex(1);
-}
-#endif
-
-/**
- * print text to the LCD
- */
-#ifdef LCD_IS_SMART
-#define LCD_print      lcd.print
-#endif
-#ifdef LCD_IS_128X64
-#define LCD_print      u8g.print
-#endif
-
-
-/**
- * Set the row/column of text at which to begin printing
- */
-#ifdef LCD_IS_SMART
-#define LCD_setCursor(x,y)   lcd.setCursor(x,y)
-#endif
-#ifdef LCD_IS_128X64
-#define LCD_setCursor(x,y)   u8g.setPrintPos(((x)+1)*FONT_WIDTH,((y)+1)*FONT_HEIGHT)
-#endif
-
-
-
-// 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;  \
-    screen_end = screen_position + LCD_HEIGHT;  \
-    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
-//------------------------------------------------------------------------------
-
-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;
-char lcd_message[LCD_MESSAGE_LENGTH+1];
-
-int menu_position_sum = 0, menu_position = 0, screen_position = 0, num_menu_items = 0, ty, screen_end;
-
-
-void (*current_menu)();
-
-
-//------------------------------------------------------------------------------
-// METHODS
-//------------------------------------------------------------------------------
-
-
-void LCD_read() {
-  // detect potentiometer changes
-  int rot = ((digitalRead(BTN_EN1) == LOW) << BLEN_A)
-          | ((digitalRead(BTN_EN2) == LOW) << BLEN_B);
-  // potentiometer uses grey code.  Pattern is 0 3 
-  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;
-  }
-  if(lcd_rot_old != rot) {
-    //if(lcd_turn !=0) Serial.print(lcd_turn>0?'+':'-');  // for debugging potentiometer
-    //Serial.println(rot);  // for debugging potentiometer
-  }
-  
-  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;
-}
-
-
-// display the current machine position and feed rate 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
-  float offset[NUM_AXIES];
-  get_end_plus_offset(offset);
-  LCD_setCursor( 0, 0);  LCD_print('X');  LCD_print_float(offset[0]);
-  LCD_setCursor(10, 0);  LCD_print('Z');  LCD_print_float(offset[2]);
-#if MACHINE_STYLE == POLARGRAPH && defined(USE_LIMIT_SWITCH)
-  LCD_setCursor(19, 0);  LCD_print(( digitalRead(LIMIT_SWITCH_PIN_LEFT) == LOW ) ? '*':' ');
-#endif
-
-  LCD_setCursor( 0, 1);  LCD_print('Y');  LCD_print_float(offset[1]);
-  LCD_setCursor(10, 1);  LCD_print('F');  LCD_print_long(speed_adjust);  LCD_print(F("% "));
-#if MACHINE_STYLE == POLARGRAPH && defined(USE_LIMIT_SWITCH)
-  LCD_setCursor(19, 1);  LCD_print(( digitalRead(LIMIT_SWITCH_PIN_RIGHT) == LOW ) ? '*':' ');
-#endif
-  
-  
-  //LCD_setCursor(10, 1);  LCD_print('F');  LCD_print_float(feed_rate);
-  //LCD_setCursor( 0, 1);  LCD_print(F("Makelangelo #"));  LCD_print(robot_uid);
-  LCD_setCursor( 0, 2);
-  if (sd_printing_now == true/* && sd_printing_paused==false*/) {
-    LCD_print_float(sd_percent_complete);
-    LCD_print('%');
-  } else {
-    LCD_print(F("          "));
-  }
-  LCD_print_message();
-  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 MACHINE_HARDWARE_VERSION  == 5
-    MENU_ACTION("Find home", LCD_find_home);
-#else
-    MENU_ACTION("This is home", LCD_this_is_home);
-    MENU_ACTION("Go home", LCD_go_home);
-#endif
-    if (sd_inserted) {
-      MENU_SUBMENU("Draw *.NGC file...", LCD_start_menu);
-    }
-    MENU_SUBMENU("Drive", LCD_drive_menu);
-  } else {
-    if (sd_printing_paused) {
-      MENU_ACTION("Unpause", LCD_pause);
-    } else {
-      MENU_ACTION("Pause", LCD_pause);
-    }
-    MENU_ACTION("Stop", LCD_stop);
-  }
-  MENU_END
-}
-
-
-void LCD_print_message() {
-  lcd_message[LCD_MESSAGE_LENGTH - 1] = 0;
-  LCD_setCursor( 0, 2);  LCD_print(lcd_message);
-  LCD_setCursor( 0, 3);  LCD_print(lcd_message + LCD_WIDTH + 1);
-}
-
-
-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() {
-  robot_findHome();
-  MENU_GOTO(LCD_main_menu);
-}
-
-
-void LCD_this_is_home() {
-  float offset[NUM_AXIES];
-  for (int i = 0; i < NUM_AXIES; ++i) offset[i] = axies[i].homePos;
-  teleport(offset);
-  MENU_GOTO(LCD_main_menu);
-}
-
-
-void LCD_go_home() {
-  float homes[NUM_AXIES];
-  for (int i = 0; i < NUM_AXIES; ++i) homes[i] = axies[i].homePos;
-  lineSafe( homes, DEFAULT_FEEDRATE );
-  MENU_GOTO(LCD_main_menu);
-}
-
-
-void LCD_drive_menu() {
-  MENU_START
-  MENU_SUBMENU("Back", LCD_main_menu);
-  MENU_SUBMENU("X", LCD_driveX);
-  MENU_SUBMENU("Y", LCD_driveY);
-  MENU_SUBMENU("Z", LCD_driveZ);
-  MENU_SUBMENU("Feedrate", LCD_driveF);
-  MENU_END
-}
-
-
-void LCD_driveX() {
-  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
-
-  float offset[NUM_AXIES];
-  get_end_plus_offset(offset);
-  
-  if (lcd_turn) {
-    offset[0] += lcd_turn > 0 ? 1 : -1;
-    lineSafe(offset, feed_rate);
-  }
-
-  LCD_setCursor( 0, 0);
-  LCD_print('X');
-  LCD_print_float(offset[0]);
-}
-
-
-void LCD_driveY() {
-  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
-
-  float offset[NUM_AXIES];
-  get_end_plus_offset(offset);
-
-  if (lcd_turn) {
-    offset[1] += lcd_turn > 0 ? 1 : -1;
-    lineSafe(offset, feed_rate);
-  }
-
-  LCD_setCursor( 0, 0);
-  LCD_print('Y');
-  LCD_print_float(offset[1]);
-}
-
-
-void LCD_driveZ() {
-  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
-
-  float offset[NUM_AXIES];
-  get_end_plus_offset(offset);
-
-  if (lcd_turn) {
-    // protect servo, don't drive beyond physical limits
-    offset[2] += lcd_turn > 0 ? 1 : -1;
-    lineSafe(offset, feed_rate);
-  }
-
-  LCD_setCursor( 0, 0);
-  LCD_print('Z');
-  LCD_print_float(offset[2]);
-}
-
-
-void LCD_driveF() {
-  if (lcd_click_now) MENU_GOTO(LCD_drive_menu);
-
-  if (lcd_turn) {
-    // protect servo, don't drive beyond physical limits
-    float newF = feed_rate + (lcd_turn > 0 ? 1 : -1);
-    if (newF < MIN_FEEDRATE) newF = MIN_FEEDRATE;
-    if (newF > MAX_FEEDRATE) newF = MAX_FEEDRATE;
-    // move
-    feed_rate = newF;
-  }
-
-  LCD_setCursor( 0, 0);
-  LCD_print('F');
-  LCD_print_float(feed_rate);
-}
-
-
-void LCD_start_menu() {
-  if (!sd_inserted) MENU_GOTO(LCD_main_menu);
-
-  MENU_START
-  MENU_SUBMENU("Back", LCD_main_menu);
-  /*
-    Serial.print(menu_position    );  Serial.print("\t");  // 0
-    Serial.print(menu_position_sum);  Serial.print("\t");  // 1
-    Serial.print(screen_position  );  Serial.print("\t");  // 0
-    Serial.print(num_menu_items   );  Serial.print("\n");  // 8
-  */
-
-  root.rewindDirectory();
-  while ( true ) {
-    //long tStart = millis();
-    File entry = root.openNextFile();
-    //long tEnd = millis();
-    //Serial.print(tEnd-tStart);
-    //Serial.print('\t');
-    if (!entry) {
-      // no more files, return to the first file in the directory
-      break;
-    }
-    char *filename = entry.name();
-    //Serial.print( entry.isDirectory()?">":" " );
-    //Serial.println(filename);/*
-    if (!entry.isDirectory() && filename[0] != '_') {
-      MENU_ITEM_START(filename)
-      if (menu_position == ty && lcd_click_now) {
-        lcd_click_now = false;
-        SD_StartPrintingFile(filename);
-        MENU_GOTO(LCD_status_menu);
-      }
-      MENU_ITEM_END()
-    }
-    entry.close();
-  }
-
-  //Serial.println();
-
-
-  MENU_END
-}
-
-void LCD_update_long(char *label, 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(label);
-    LCD_setCursor(0, 1);
-    LCD_print_long(value);
-  } while ( !lcd_click_now );
-}
-
-
-void LCD_update_float(char *label, float &value,float stepSize=0.01) {
-  LCD_clear();
-  do {
-    LCD_read();
-    if ( lcd_turn ) {
-      value += lcd_turn > 0 ? stepSize : -stepSize;
-      lcd_turn = 0;
-    }
-    LCD_setCursor(0, 0);
-    LCD_print(label);
-    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);
-}
-#endif  // HAS_LCD
-
-
-
-
-void LCD_update() {
-#ifdef HAS_LCD
-  LCD_read();
-
-  if (millis() >= lcd_draw_delay ) {
-    lcd_draw_delay = millis() + LCD_DRAW_DELAY;
-
-    //Serial.print(lcd_turn,DEC);
-    //Serial.print('\t');  Serial.print(menu_position,DEC);
-    //Serial.print('\t');  Serial.print(menu_position_sum,DEC);
-    //Serial.print('\t');  Serial.print(screen_position,DEC);
-    //Serial.print('\t');  Serial.print(screen_end,DEC);
-    //Serial.print('\t');  Serial.print(num_menu_items,DEC);
-    //Serial.print('\n');
-
-    // update the menu position
-    if ( lcd_turn && num_menu_items>1 ) {
-      int originalPosition = menu_position_sum / LCD_TURN_PER_MENU;
-      menu_position_sum += lcd_turn;
-  
-      menu_position = menu_position_sum / LCD_TURN_PER_MENU;
-      if (menu_position > num_menu_items - 1) menu_position = num_menu_items - 1;
-      if (menu_position < 0) menu_position = 0;
-  
-      if (originalPosition != menu_position) {
-        LCD_clear();
-      }
-
-      Serial.println(menu_position);
-  
-      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;
-    }
-
-    // draw the new screen contents
-#ifdef LCD_IS_128X64
-  u8g.firstPage();
-  do {
-#endif
-    (*current_menu)();
-#ifdef LCD_IS_128X64
-  } while(u8g.nextPage());
-#endif
-
-    lcd_turn = 0;  // reset.  must come after (*current_menu)() because it might be used there.  (damn globals...)
-  }
-#endif  // HAS_LCD
-}
-
-// initialize the Smart controller LCD panel
-void LCD_init() {
-#ifdef HAS_LCD
-
-#ifdef LCD_IS_SMART
-  lcd.begin(LCD_WIDTH, LCD_HEIGHT);
-#endif
-#ifdef LCD_IS_128X64
-  u8g.begin();
-  u8g.disableCursor();
-  u8g.setFont(u8g_font_6x9);
-//  u8g.setFont(u8g_font_helvR08);
-#endif
-
-  pinMode(BEEPER,OUTPUT);
-  digitalWrite(BEEPER,LOW);
-
-  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 */
-
-  lcd_message[0]=0;
-
-  LCD_drawSplash();
-  //LCD_drawSplash();
-
-#endif  // HAS_LCD
-}
-
-
-#ifdef HAS_LCD
-void LCD_drawSplash() {
-  // splash screen
-  char message[LCD_WIDTH];
-  char mhv[10];
-  itoa(MACHINE_HARDWARE_VERSION,mhv,10);
-  char *ptr = message;
-  strcpy(ptr, MACHINE_STYLE_NAME );  ptr += strlen(MACHINE_STYLE_NAME);
-  strcpy(ptr, " v" );                ptr += strlen(" v");
-  strcpy(ptr, mhv );                 ptr += strlen(mhv);
-
-  int x = (LCD_WIDTH - strlen(message)) / 2;
-  int y = LCD_HEIGHT-2;
-  int x2 = (LCD_WIDTH - strlen("marginallyclever.com")) / 2;
-  int y2 = y+1;
-
-#ifdef LCD_IS_128X64
-  u8g.firstPage();
-  do {
-#endif
-    LCD_clear();
-#ifdef LCD_IS_128X64
-    u8g.drawBitmapP((LCD_PIXEL_WIDTH-logoImageWidth)/2,0,logoImageWidth/8,logoImageHeight,logoImage);
-#endif
-    LCD_setCursor(x,y);
-    LCD_print(message);
-    LCD_setCursor(x2,y2);
-    LCD_print("marginallyclever.com");
-#ifdef LCD_IS_128X64
-  } while(u8g.nextPage());
-#endif
-
-  delay(2500);
-  LCD_clear();
-}
-#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/>.
-*/
-
diff --git a/motor.cpp b/motor.cpp
new file mode 100644
index 0000000..6fa507d
--- /dev/null
+++ b/motor.cpp
@@ -0,0 +1,1200 @@
+//------------------------------------------------------------------------------
+// Makelangelo - firmware for various robot kinematic models
+// dan@marginallycelver.com 2013-12-26
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+//------------------------------------------------------------------------------
+
+
+//------------------------------------------------------------------------------
+// INCLUDES
+//------------------------------------------------------------------------------
+#include "configure.h"
+#include "motor.h"
+#include "MServo.h"
+#include "LCD.h"
+
+//------------------------------------------------------------------------------
+// MACROS
+//------------------------------------------------------------------------------
+
+#ifdef ESP8266
+#define CLOCK_ADJUST(x) {  timer0_write(ESP.getCycleCount() + (long) (80000L*(x)) );  }  // microseconds
+#else
+#define CLOCK_ADJUST(x) {  OCR1A = (x);  }  // microseconds
+#endif
+
+//------------------------------------------------------------------------------
+// DEFINES
+//------------------------------------------------------------------------------
+
+#define BLOCK_DELAY_FOR_1ST_MOVE 100
+#define MIN_STEP_RATE 120
+
+//------------------------------------------------------------------------------
+// GLOBALS
+//------------------------------------------------------------------------------
+
+Motor motors[NUM_MOTORS + NUM_SERVOS];
+#ifndef ESP8266
+Servo servos[NUM_SERVOS];
+#endif
+
+Segment line_segments[MAX_SEGMENTS];
+Segment *working_seg = NULL;
+volatile int current_segment = 0;
+volatile int last_segment = 0;
+int first_segment_delay;
+
+// used by timer1 to optimize interrupt inner loop
+int steps_total;
+int steps_taken;
+int accel_until, decel_after;
+uint32_t current_feed_rate;
+uint32_t current_acceleration;
+uint32_t start_feed_rate, end_feed_rate, isr_nominal_rate;
+uint32_t time_accelerating, time_decelerating;
+float max_xy_jerk = MAX_JERK;
+float max_feedrate_mm_s[NUM_MOTORS + NUM_SERVOS];
+uint8_t isr_step_multiplier = 1;
+
+int delta0;
+int over0;
+long global_steps_0;
+int global_step_dir_0;
+#if NUM_MOTORS>1
+int delta1;
+int over1;
+long global_steps_1;
+int global_step_dir_1;
+#endif
+#if NUM_MOTORS>2
+int delta2;
+int over2;
+long global_steps_2;
+int global_step_dir_2;
+#endif
+#if NUM_MOTORS>3
+int delta3;
+int over3;
+long global_steps_3;
+int global_step_dir_3;
+#endif
+#if NUM_MOTORS>4
+int delta4;
+int over4;
+long global_steps_4;
+int global_step_dir_4;
+#endif
+#if NUM_MOTORS>5
+int delta5;
+int over5;
+long global_steps_5;
+int global_step_dir_5;
+#endif
+
+float previous_nominal_speed = 0;
+float previous_safe_speed = 0;
+float previous_speed[NUM_MOTORS + NUM_SERVOS];
+
+const char *MotorNames = "LRUVWT";
+const char *AxisNames = "XYZUVWT";
+float maxFeedRate[NUM_MOTORS];
+
+
+//------------------------------------------------------------------------------
+// METHODS
+//------------------------------------------------------------------------------
+#ifdef ESP8266
+void itr();
+#endif
+
+
+const int movesPlanned() {
+  return SEGMOD( last_segment - current_segment );
+}
+
+// for reasons I don't understand... if i put this method in the .h file i get compile errors.
+// so I put it here, which forces the externs.
+FORCE_INLINE Segment *get_current_segment() {
+  if (current_segment == last_segment ) return NULL;
+  if (first_segment_delay > 0) {
+    --first_segment_delay;
+    if (movesPlanned() > 3) first_segment_delay = 0;
+    return NULL;
+  }
+  return &line_segments[current_segment];
+}
+
+
+FORCE_INLINE int get_next_segment(int i) {
+  return SEGMOD( i + 1 );
+}
+
+
+FORCE_INLINE int get_prev_segment(int i) {
+  return SEGMOD( i - 1 );
+}
+
+/**
+   Calculate the maximum allowable speed at this point, in order
+   to reach 'target_velocity' using 'acceleration' within a given
+   'distance'.
+   @param acc acceleration
+   @param target_velocity
+   @param distance
+*/
+float max_speed_allowed(const float &acc, const float &target_velocity, const float &distance) {
+  return sqrt( sq(target_velocity) - 2 * acc * 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;
+#if NUM_MOTORS>1
+  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;
+#endif
+#if NUM_MOTORS>2
+  motors[2].step_pin        = MOTOR_2_STEP_PIN;
+  motors[2].dir_pin         = MOTOR_2_DIR_PIN;
+  motors[2].enable_pin      = MOTOR_2_ENABLE_PIN;
+  motors[2].limit_switch_pin = MOTOR_2_LIMIT_SWITCH_PIN;
+#endif
+#if NUM_MOTORS>3
+  motors[3].step_pin        = MOTOR_3_STEP_PIN;
+  motors[3].dir_pin         = MOTOR_3_DIR_PIN;
+  motors[3].enable_pin      = MOTOR_3_ENABLE_PIN;
+  motors[3].limit_switch_pin = MOTOR_3_LIMIT_SWITCH_PIN;
+#endif
+#if NUM_MOTORS>4
+  motors[4].step_pin        = MOTOR_4_STEP_PIN;
+  motors[4].dir_pin         = MOTOR_4_DIR_PIN;
+  motors[4].enable_pin      = MOTOR_4_ENABLE_PIN;
+  motors[4].limit_switch_pin = MOTOR_4_LIMIT_SWITCH_PIN;
+#endif
+#if NUM_MOTORS>5
+  motors[5].step_pin        = MOTOR_5_STEP_PIN;
+  motors[5].dir_pin         = MOTOR_5_DIR_PIN;
+  motors[5].enable_pin      = MOTOR_5_ENABLE_PIN;
+  motors[5].limit_switch_pin = MOTOR_5_LIMIT_SWITCH_PIN;
+#endif
+
+  Serial.println("  set pins");
+
+  int i;
+  for (i = 0; i < NUM_MOTORS; ++i) {
+    // set the motor pin & scale
+    //pinMode(motors[i].step_pin, OUTPUT);
+    //pinMode(motors[i].dir_pin, OUTPUT);
+    //pinMode(motors[i].enable_pin, OUTPUT);
+    delay(100);
+
+    // set the switch pin
+    //pinMode(motors[i].limit_switch_pin, INPUT);
+    //digitalWrite(motors[i].limit_switch_pin, HIGH);
+  }
+
+  long steps[NUM_MOTORS + NUM_SERVOS];
+  memset(steps, 0, (NUM_MOTORS + NUM_SERVOS)*sizeof(long));
+
+  for (i = 0; i < NUM_MOTORS; ++i) {
+    max_feedrate_mm_s[i] = MAX_FEEDRATE;
+  }
+  for (i = NUM_MOTORS; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    max_feedrate_mm_s[i] = MAX_FEEDRATE;
+  }
+
+  Serial.println("  motor_set_step_count()");
+  motor_set_step_count(steps);
+
+  // setup servos
+#if NUM_SERVOS>0
+#ifdef ESP8266
+  pinMode(SERVO0_PIN, OUTPUT);
+#else
+  servos[0].attach(SERVO0_PIN);
+#endif  // ESP8266
+#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;
+#if NUM_MOTORS>1
+  old_seg.a[1].step_count = 0;
+#endif
+#if NUM_MOTORS>2
+  old_seg.a[2].step_count = 0;
+#endif
+#if NUM_MOTORS>3
+  old_seg.a[3].step_count = 0;
+#endif
+#if NUM_MOTORS>4
+  old_seg.a[4].step_count = 0;
+#endif
+#if NUM_MOTORS>5
+  old_seg.a[5].step_count = 0;
+#endif
+
+#if NUM_SERVOS>0
+  old_seg.a[NUM_MOTORS].step_count = 0;
+#endif
+
+  working_seg = NULL;
+  first_segment_delay = 0;
+
+  Serial.println("  interrupt setup");
+
+  // disable global interrupts
+  noInterrupts();
+#ifdef ESP8266
+  timer0_isr_init();
+  timer0_attachInterrupt(itr);
+  CLOCK_ADJUST(2000);
+#else
+  // 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);
+#endif  // ESP8266
+
+  interrupts();  // enable global interrupts
+
+  Serial.println("  done");
+}
+
+
+// turn on power to the motors (make them immobile)
+void motor_engage() {
+  int i;
+  for (i = 0; i < NUM_MOTORS; ++i) {
+    digitalWrite(motors[i].enable_pin, LOW);
+  }
+  /*
+    #if MACHINE_STYLE == ARM6
+    // DM320T drivers want high for enabled
+    digitalWrite(motors[4].enable_pin,HIGH);
+    digitalWrite(motors[5].enable_pin,HIGH);
+    #endif*/
+}
+
+
+// turn off power to the motors (make them move freely)
+void motor_disengage() {
+  int i;
+  for (i = 0; i < NUM_MOTORS; ++i) {
+    digitalWrite(motors[i].enable_pin, HIGH);
+  }/*
+  #if MACHINE_STYLE == ARM6
+  // DM320T drivers want low for disabled
+  digitalWrite(motors[4].enable_pin,LOW);
+  digitalWrite(motors[5].enable_pin,LOW);
+  #endif*/
+}
+
+
+// Change pen state.
+void setPenAngle(int arg0) {
+#if NUM_AXIES>=3
+  if (arg0 < axies[2].limitMin) arg0 = axies[2].limitMin;
+  if (arg0 > axies[2].limitMax) arg0 = axies[2].limitMax;
+
+  axies[2].pos = arg0;
+#endif  // NUM_AXIES>=3
+
+#if NUM_SERVOS>0
+// this is commented out because compiler segfault for unknown reasons.
+//#ifndef ESP8266
+//  servos[0].write(arg0);
+//#else
+  analogWrite(SERVO0_PIN, arg0);
+//#endif  // ESP8266
+#endif // NUM_SERVOS>0
+}
+
+
+
+
+void recalculate_reverse_kernel(Segment *const current, const Segment *next) {
+  if (current == NULL) return;
+
+  const float entry_speed_max2 = current->entry_speed_max;
+  if (current->entry_speed != entry_speed_max2 || (next && next->recalculate_flag) ) {
+    // 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.
+    const float new_entry_speed = current->nominal_length_flag
+                                  ? entry_speed_max2
+                                  : min( entry_speed_max2, max_speed_allowed(-current->acceleration, (next ? next->entry_speed : MIN_FEEDRATE), current->distance) );
+
+    if (current->entry_speed != new_entry_speed ) {
+      current->entry_speed = new_entry_speed;
+      current->recalculate_flag = true;
+    }
+  }
+}
+
+
+void recalculate_reverse() {
+  if (last_segment == current_segment) return;
+
+  int s = get_prev_segment(last_segment);
+
+  Segment *current, *next = NULL;
+  while (s != current_segment) {
+    current = &line_segments[s];
+
+    recalculate_reverse_kernel(current, next);
+    next = current;
+    s = get_prev_segment(s);
+  }
+}
+
+
+void recalculate_forward_kernel(const Segment *prev, Segment *const current) {
+  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 && prev->entry_speed < current->entry_speed) {
+    const float new_entry_speed2 = max_speed_allowed(-prev->acceleration, prev->entry_speed, prev->distance);
+    // Check for junction speed change
+    if (new_entry_speed2 < current->entry_speed) {
+      current->recalculate_flag = true;
+    } else {
+      current->entry_speed = new_entry_speed2;
+    }
+  }
+}
+
+
+void recalculate_forward() {
+  int s = current_segment;
+
+  Segment *previous = NULL, *current;
+  while (s != last_segment) {
+    current = &line_segments[s];
+    recalculate_forward_kernel(previous, current);
+    previous = current;
+    s = get_next_segment(s);
+  }
+}
+
+
+float estimate_acceleration_distance(const float &initial_rate, const float &target_rate, const float &accel) {
+  if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
+  return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
+}
+
+
+int intersection_distance(const float &start_rate, const float &end_rate, const float &accel, const float &distance) {
+  return ( 2.0 * accel * distance - sq(start_rate) + sq(end_rate) ) / (4.0 * accel);
+}
+
+
+void segment_update_trapezoid(Segment *s, const float &entry_factor, const float &exit_factor) {
+  uint32_t intial_rate = ceil(s->nominal_rate * entry_factor);
+  uint32_t final_rate  = ceil(s->nominal_rate * exit_factor );
+
+  if (intial_rate < MIN_STEP_RATE) intial_rate = MIN_STEP_RATE;
+  if (final_rate  < MIN_STEP_RATE) final_rate  = MIN_STEP_RATE;
+
+  const int32_t accel = s->acceleration_steps_per_s2;
+  int32_t accelerate_steps =  ceil( estimate_acceleration_distance(intial_rate    , s->nominal_rate,  accel) );
+  int32_t decelerate_steps = floor( estimate_acceleration_distance(s->nominal_rate, final_rate     , -accel) );
+  int32_t plateau_steps = s->steps_total - accelerate_steps - decelerate_steps;
+  if (plateau_steps < 0) {
+    accelerate_steps = ceil( intersection_distance( intial_rate, final_rate, accel, s->steps_total ) );
+    accelerate_steps = min( (uint32_t)max( accelerate_steps, 0 ), s->steps_total );
+    plateau_steps = 0;
+  }
+  CRITICAL_SECTION_START
+  /*
+    decelerate_steps = s->steps_total - plateau_steps - accelerate_steps;
+    Serial.print("t entry_factor=");  Serial.print(entry_factor);
+    Serial.print(" exit_factor=");  Serial.print(exit_factor);
+    Serial.print(" accel=");  Serial.print(accel);
+    Serial.print(" intial_rate=");  Serial.print(intial_rate);
+    Serial.print(" nominal_rate=");  Serial.print(s->nominal_rate);
+    Serial.print(" final_rate=");  Serial.print(final_rate);
+    Serial.print(" steps_total=");  Serial.print(s->steps_total);
+    Serial.print(" accelerate_steps=");  Serial.print(accelerate_steps);
+    Serial.print(" plateau_steps=");  Serial.print(plateau_steps);
+    Serial.print(" decelerate_steps=");  Serial.print(decelerate_steps);
+    Serial.println();
+  */
+  if (!s->busy) {
+    s->accel_until = accelerate_steps;
+    s->decel_after = accelerate_steps + plateau_steps;
+    s->initial_rate = intial_rate;
+    s->final_rate  = final_rate;
+  }
+  CRITICAL_SECTION_END
+}
+
+
+void recalculate_trapezoids() {
+  int s = current_segment;
+  Segment *current = NULL;
+  Segment *next = NULL;
+
+  float current_entry_speed = 0, next_entry_speed = 0;
+
+  while (s != last_segment) {
+    next = &line_segments[s];
+    next_entry_speed = next->entry_speed;
+    if (current) {
+      // Recalculate if current block entry or exit junction speed has changed.
+      if ( current->recalculate_flag || next->recalculate_flag ) {
+        if (!current->busy) {
+          // NOTE: Entry and exit factors always > 0 by all previous logic operations.
+          const float inom = 1.0 / current->nominal_speed;
+          segment_update_trapezoid(current, current_entry_speed * inom, next_entry_speed * inom);
+        }
+      }
+      current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+    }
+    s = get_next_segment(s);
+    current_entry_speed = next_entry_speed;
+    current = next;
+  }
+
+  // Last/newest block in buffer. Make sure the last block always ends motion.
+  if (next != NULL) {
+    const float inom = 1.0 / next->nominal_speed;
+    segment_update_trapezoid(next, next_entry_speed * inom, MIN_FEEDRATE * inom);
+    next->recalculate_flag = false;
+  }
+}
+
+
+void describe_segments() {
+  CRITICAL_SECTION_START
+  /**/
+  Serial.println("A = index");
+  Serial.println("B = distance");
+  Serial.println("C = acceleration");
+
+  Serial.println("D = entry_speed");
+  Serial.println("E = nominal_speed");
+  Serial.println("F = entry_speed_max");
+
+  Serial.println("G = entry rate");
+  Serial.println("H = nominal rate");
+  Serial.println("I = exit rate");
+
+  Serial.println("J = accel_until");
+  Serial.println("K = coast steps");
+  Serial.println("L = decel steps");
+
+  Serial.println("O = nominal?");
+  Serial.println("P = recalculate?");
+  Serial.println("Q = busy?");/**/
+  Serial.println("\nA\tB\tC\tD\tE\tF\t[G\tH\tI]\t[J\tK\tL]\tO\tP\tQ");
+  Serial.println("---------------------------------------------------------------------------------------------------------------------------");
+
+  int s = current_segment;
+  while (s != last_segment) {
+    Segment *next = &line_segments[s];
+    int coast = next->decel_after - next->accel_until;
+    int decel = next->steps_total - next->decel_after;
+    Serial.print(s);
+    Serial.print(F("\t"));   Serial.print(next->distance);
+    Serial.print(F("\t"));   Serial.print(next->acceleration);
+
+    Serial.print(F("\t"));   Serial.print(next->entry_speed);
+    Serial.print(F("\t"));   Serial.print(next->nominal_speed);
+    Serial.print(F("\t"));   Serial.print(next->entry_speed_max);
+
+    Serial.print(F("\t"));   Serial.print(next->initial_rate);
+    Serial.print(F("\t"));   Serial.print(next->nominal_rate);
+    Serial.print(F("\t"));   Serial.print(next->final_rate);
+
+    Serial.print(F("\t"));   Serial.print(next->accel_until);
+    Serial.print(F("\t"));   Serial.print(coast);
+    Serial.print(F("\t"));   Serial.print(decel);
+    //Serial.print(F("\t"));   Serial.print(next->steps_total);
+    //Serial.print(F("\t"));   Serial.print(next->steps_taken);
+
+    Serial.print(F("\t"));   Serial.print(next->nominal_length_flag != 0 ? 'Y' : 'N');
+    Serial.print(F("\t"));   Serial.print(next->recalculate_flag != 0 ? 'Y' : 'N');
+    Serial.print(F("\t"));   Serial.print(next->busy != 0 ? 'Y' : 'N');
+    Serial.println();
+    s = get_next_segment(s);
+  }
+  CRITICAL_SECTION_END
+}
+
+
+void recalculate_acceleration() {
+  recalculate_reverse();
+  recalculate_forward();
+  recalculate_trapezoids();
+
+  //Serial.println("**FINISHED**");
+  //describe_segments();
+}
+
+
+void motor_set_step_count(long *a) {
+  wait_for_empty_segment_buffer();
+
+  for (int i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    previous_speed[i] = 0;
+  }
+
+  Segment &old_seg = line_segments[get_prev_segment(last_segment)];
+  old_seg.a[0].step_count = a[0];
+#if NUM_MOTORS>1
+  old_seg.a[1].step_count = a[1];
+#endif
+#if NUM_MOTORS>2
+  old_seg.a[2].step_count = a[2];
+#endif
+#if NUM_MOTORS>3
+  old_seg.a[3].step_count = a[3];
+#endif
+#if NUM_MOTORS>4
+  old_seg.a[4].step_count = a[4];
+#endif
+#if NUM_MOTORS>5
+  old_seg.a[5].step_count = a[5];
+#endif
+#if NUM_SERVOS>0
+  old_seg.a[NUM_MOTORS].step_count = a[NUM_MOTORS];
+#endif
+
+  global_steps_0 = 0;
+#if NUM_MOTORS>1
+  global_steps_1 = 0;
+#endif
+#if NUM_MOTORS>2
+  global_steps_2 = 0;
+#endif
+#if NUM_MOTORS>3
+  global_steps_3 = 0;
+#endif
+#if NUM_MOTORS>4
+  global_steps_4 = 0;
+#endif
+#if NUM_MOTORS>5
+  global_steps_5 = 0;
+#endif
+}
+
+
+/**
+   Step one motor one time in the currently set direction.
+   @input newx the destination x position
+   @input newy the destination y position
+ **/
+void motor_onestep(int motor) {
+#ifdef VERBOSE
+  Serial.print(motorNames[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(uint32_t desired_freq_hz, uint8_t*loops) {
+  if ( desired_freq_hz > CLOCK_MAX_STEP_FREQUENCY ) desired_freq_hz = CLOCK_MAX_STEP_FREQUENCY;
+  if ( desired_freq_hz < CLOCK_MIN_STEP_FREQUENCY ) desired_freq_hz = CLOCK_MIN_STEP_FREQUENCY;
+  //desired_freq_hz-=CLOCK_MIN_STEP_FREQUENCY;
+
+  uint8_t step_multiplier = 1;
+  if ( desired_freq_hz > 20000 ) {
+    step_multiplier = 4;
+    desired_freq_hz >>= 2;
+  } else if ( desired_freq_hz > 10000 ) {
+    step_multiplier = 2;
+    desired_freq_hz >>= 1;
+  }
+  *loops = step_multiplier;
+
+  long counter_value = ( CLOCK_FREQ >> 3 ) / desired_freq_hz;
+  if ( counter_value >= MAX_COUNTER ) {
+    counter_value = MAX_COUNTER - 1;
+  } else if ( counter_value < 100 ) {
+    counter_value = 100;
+  }
+
+  return counter_value;
+}
+
+
+#define A(CODE) " " CODE "\n\t"
+
+// intRes = longIn1 * longIn2 >> 24
+// uses:
+// A[tmp] to store 0
+// B[tmp] to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result.
+// note that the lower two bytes and the upper byte of the 48bit result are not calculated.
+// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones.
+// B A are bits 24-39 and are the returned value
+// C B A is longIn1
+// D C B A is longIn2
+//
+static FORCE_INLINE uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) {
+#ifdef ESP8266
+  uint16_t intRes = longIn1 * longIn2 >> 24;
+#else // ESP8266
+  register uint8_t tmp1;
+  register uint8_t tmp2;
+  register uint16_t intRes;
+  __asm__ __volatile__(
+    A("clr %[tmp1]")
+    A("mul %A[longIn1], %B[longIn2]")
+    A("mov %[tmp2], r1")
+    A("mul %B[longIn1], %C[longIn2]")
+    A("movw %A[intRes], r0")
+    A("mul %C[longIn1], %C[longIn2]")
+    A("add %B[intRes], r0")
+    A("mul %C[longIn1], %B[longIn2]")
+    A("add %A[intRes], r0")
+    A("adc %B[intRes], r1")
+    A("mul %A[longIn1], %C[longIn2]")
+    A("add %[tmp2], r0")
+    A("adc %A[intRes], r1")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %B[longIn1], %B[longIn2]")
+    A("add %[tmp2], r0")
+    A("adc %A[intRes], r1")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %C[longIn1], %A[longIn2]")
+    A("add %[tmp2], r0")
+    A("adc %A[intRes], r1")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %B[longIn1], %A[longIn2]")
+    A("add %[tmp2], r1")
+    A("adc %A[intRes], %[tmp1]")
+    A("adc %B[intRes], %[tmp1]")
+    A("lsr %[tmp2]")
+    A("adc %A[intRes], %[tmp1]")
+    A("adc %B[intRes], %[tmp1]")
+    A("mul %D[longIn2], %A[longIn1]")
+    A("add %A[intRes], r0")
+    A("adc %B[intRes], r1")
+    A("mul %D[longIn2], %B[longIn1]")
+    A("add %B[intRes], r0")
+    A("clr r1")
+    : [intRes] "=&r" (intRes),
+    [tmp1] "=&r" (tmp1),
+    [tmp2] "=&r" (tmp2)
+    : [longIn1] "d" (longIn1),
+    [longIn2] "d" (longIn2)
+    : "cc"
+  );
+#endif // ESP8266
+  return intRes;
+}
+
+/**
+   Process all line segments in the ring buffer.  Uses bresenham's line algorithm to move all motors.
+*/
+
+#ifdef ESP8266
+void itr() {
+#else
+ISR(TIMER1_COMPA_vect) {
+#endif
+  // segment buffer empty? do nothing
+  if ( working_seg == NULL ) {
+    working_seg = get_current_segment();
+
+    if ( working_seg != NULL ) {
+      // New segment!
+      working_seg->busy = true;
+
+      // set the direction pins
+      digitalWrite( MOTOR_0_DIR_PIN, working_seg->a[0].dir );
+      global_step_dir_0 = (working_seg->a[0].dir == HIGH) ? 1 : -1;
+
+#if NUM_MOTORS>1
+      digitalWrite( MOTOR_1_DIR_PIN, working_seg->a[1].dir );
+      global_step_dir_1 = (working_seg->a[1].dir == HIGH) ? 1 : -1;
+#endif
+#if NUM_MOTORS>2
+      digitalWrite( MOTOR_2_DIR_PIN, working_seg->a[2].dir );
+      global_step_dir_2 = (working_seg->a[2].dir == HIGH) ? 1 : -1;
+#endif
+#if NUM_MOTORS>3
+      digitalWrite( MOTOR_3_DIR_PIN, working_seg->a[3].dir );
+      global_step_dir_3 = (working_seg->a[3].dir == HIGH) ? 1 : -1;
+#endif
+#if NUM_MOTORS>4
+      digitalWrite( MOTOR_4_DIR_PIN, working_seg->a[4].dir );
+      global_step_dir_4 = (working_seg->a[4].dir == HIGH) ? 1 : -1;
+#endif
+#if NUM_MOTORS>5
+      digitalWrite( MOTOR_5_DIR_PIN, working_seg->a[5].dir );
+      global_step_dir_5 = (working_seg->a[5].dir == HIGH) ? 1 : -1;
+#endif
+
+#if NUM_SERVOS>0
+#ifdef ESP8266
+      analogWrite(SERVO0_PIN, working_seg->a[NUM_MOTORS].step_count);
+#else
+      servos[0].write(working_seg->a[NUM_MOTORS].step_count);
+#endif  // ESP8266
+#endif  // NUM_SERVOS>0
+
+      start_feed_rate = working_seg->initial_rate;
+      end_feed_rate = working_seg->final_rate;
+      current_feed_rate = start_feed_rate;
+      current_acceleration = working_seg->acceleration_rate;
+      accel_until = working_seg->accel_until;
+      decel_after = working_seg->decel_after;
+      time_accelerating = 0;
+      time_decelerating = 0;
+      isr_nominal_rate = 0;
+
+      // defererencing some data so the loop runs faster.
+      steps_total = working_seg->steps_total;
+      steps_taken = 0;
+      delta0 = working_seg->a[0].absdelta;      over0 = -(steps_total >> 1);
+#if NUM_MOTORS>1
+      delta1 = working_seg->a[1].absdelta;      over1 = -(steps_total >> 1);
+#endif
+#if NUM_MOTORS>2
+      delta2 = working_seg->a[2].absdelta;      over2 = -(steps_total >> 1);
+#endif
+#if NUM_MOTORS>3
+      delta3 = working_seg->a[3].absdelta;      over3 = -(steps_total >> 1);
+#endif
+#if NUM_MOTORS>4
+      delta4 = working_seg->a[4].absdelta;      over4 = -(steps_total >> 1);
+#endif
+#if NUM_MOTORS>5
+      delta5 = working_seg->a[5].absdelta;      over5 = -(steps_total >> 1);
+#endif
+      return;
+    } else {
+      CLOCK_ADJUST(2000); // wait 1ms
+      return;
+    }
+  }
+
+  if ( working_seg != NULL ) {
+    // move each axis
+    for (uint8_t i = 0; i < isr_step_multiplier; ++i) {
+      over0 += delta0;
+      if (over0 > 0) digitalWrite(MOTOR_0_STEP_PIN, LOW);
+#if NUM_MOTORS>1
+      over1 += delta1;
+      if (over1 > 0) digitalWrite(MOTOR_1_STEP_PIN, LOW);
+#endif
+#if NUM_MOTORS>2
+      over2 += delta2;
+      if (over2 > 0) digitalWrite(MOTOR_2_STEP_PIN, LOW);
+#endif
+#if NUM_MOTORS>3
+      over3 += delta3;
+      if (over3 > 0) digitalWrite(MOTOR_3_STEP_PIN, LOW);
+#endif
+#if NUM_MOTORS>4
+      over4 += delta4;
+      if (over4 > 0) digitalWrite(MOTOR_4_STEP_PIN, LOW);
+#endif
+#if NUM_MOTORS>5
+      over5 += delta5;
+      if (over5 > 0) digitalWrite(MOTOR_5_STEP_PIN, LOW);
+#endif
+      // now that the pins have had a moment to settle, do the second half of the steps.
+      // M0
+      if (over0 > 0) {
+        over0 -= steps_total;
+        global_steps_0 += global_step_dir_0;
+        digitalWrite(MOTOR_0_STEP_PIN, HIGH);
+      }
+#if NUM_MOTORS>1
+      // M1
+      if (over1 > 0) {
+        over1 -= steps_total;
+        global_steps_1 += global_step_dir_1;
+        digitalWrite(MOTOR_1_STEP_PIN, HIGH);
+      }
+#endif
+#if NUM_MOTORS>2
+      // M2
+      if (over2 > 0) {
+        over2 -= steps_total;
+        global_steps_2 += global_step_dir_2;
+        digitalWrite(MOTOR_2_STEP_PIN, HIGH);
+      }
+#endif
+#if NUM_MOTORS>3
+      // M3
+      if (over3 > 0) {
+        over3 -= steps_total;
+        global_steps_3 += global_step_dir_3;
+        digitalWrite(MOTOR_3_STEP_PIN, HIGH);
+      }
+#endif
+#if NUM_MOTORS>4
+      // M4
+      if (over4 > 0) {
+        over4 -= steps_total;
+        global_steps_4 += global_step_dir_4;
+        digitalWrite(MOTOR_4_STEP_PIN, HIGH);
+      }
+#endif
+#if NUM_MOTORS>5
+      // M5
+      if (over5 > 0) {
+        over5 -= steps_total;
+        global_steps_5 += global_step_dir_5;
+        digitalWrite(MOTOR_5_STEP_PIN, HIGH);
+      }
+#endif
+
+      // make a step
+      steps_taken++;
+      if (steps_taken >= steps_total) break;
+    }
+
+
+    // 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;
+    }
+
+    // accel
+    uint32_t interval;
+    if ( steps_taken <= accel_until ) {
+      current_feed_rate = start_feed_rate + MultiU24X32toH16( time_accelerating, current_acceleration );
+      if (current_feed_rate > working_seg->nominal_rate) {
+        current_feed_rate = working_seg->nominal_rate;
+      }
+      interval = calc_timer(current_feed_rate, &isr_step_multiplier);
+      time_accelerating += interval;
+      CLOCK_ADJUST(interval);
+      /*
+        Serial.print("A\t");
+        Serial.print(current_feed_rate);
+        Serial.print("\t");
+        Serial.println(isr_step_multiplier);//*/
+      /*
+        Serial.print("A >> ");   Serial.print(interval);
+        Serial.print("\t");      Serial.print(isr_step_multiplier);
+        Serial.print("\t");      Serial.print(current_feed_rate);
+        Serial.print(" = ");     Serial.print(start_feed_rate);
+        Serial.print(" + ");     Serial.print(current_acceleration);
+        Serial.print(" * ");     Serial.print(time_accelerating);
+        Serial.println();//*/
+    } else if ( steps_taken > decel_after ) {
+      uint32_t end_feed_rate = current_feed_rate - MultiU24X32toH16( time_decelerating, current_acceleration );
+      if ( end_feed_rate < working_seg->final_rate ) {
+        end_feed_rate = working_seg->final_rate;
+      }
+      interval = calc_timer(end_feed_rate, &isr_step_multiplier);
+      time_decelerating += interval;
+      CLOCK_ADJUST(interval);
+      /*
+        Serial.print("D\t");
+        Serial.print(end_feed_rate);
+        Serial.print("\t");
+        Serial.println(isr_step_multiplier);//*/
+      /*
+        Serial.print("D >> ");  Serial.print(interval);
+        Serial.print("\t");     Serial.print(isr_step_multiplier);
+        Serial.print("\t");     Serial.print(end_feed_rate);
+        Serial.print(" = ");     Serial.print(current_feed_rate);
+        Serial.print(" - ");     Serial.print(current_acceleration);
+        Serial.print(" * ");     Serial.print(time_decelerating);
+        Serial.println();//*/
+    } else {
+      if (isr_nominal_rate == 0) {
+        isr_nominal_rate = calc_timer(working_seg->nominal_rate, &isr_step_multiplier);
+      }
+      CLOCK_ADJUST(isr_nominal_rate);
+      /*
+        Serial.print("C\t");
+        Serial.print(working_seg->nominal_rate);
+        Serial.print("\t");
+        Serial.println(isr_step_multiplier);//*/
+      /*
+        Serial.print("C >> ");  Serial.println(working_seg->nominal_rate);
+        //Serial.print("\t");  Serial.print(interval);
+        //Serial.print("\t");     Serial.print(isr_step_multiplier);
+        Serial.print("\t");     Serial.print(current_feed_rate);
+        Serial.println();//*/
+    }
+#ifndef ESP8266
+    OCR1A = (OCR1A < (TCNT1 + 16)) ? (TCNT1 + 16) : OCR1A;
+#endif // ESP8266
+  }
+}
+
+
+/**
+   @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);
+}
+
+
+/**
+   Translate the XYZ through the IK to get the number of motor steps and move the motors.
+   Uses bresenham's line algorithm to move both motors
+   @input pos NUM_AXIES floats describing destination coordinates
+   @input new_feed_rate speed to travel along arc
+*/
+void motor_line(const float * const target_position, float &fr_mm_s) {
+  long steps[NUM_MOTORS + NUM_SERVOS];
+
+  // convert from the cartesian position to the motor steps
+  IK(target_position, steps);
+
+  float distance_mm = 0;
+
+  // record the new target position & feed rate for the next movement.
+  int i;
+  for (i = 0; i < NUM_AXIES; ++i) {
+    distance_mm += sq(target_position[i] - axies[i].pos);
+    axies[i].pos = target_position[i];
+  }
+  distance_mm = sqrt( distance_mm );
+  feed_rate = fr_mm_s;
+
+  // get the next available spot in the segment buffer
+  int next_segment = get_next_segment(last_segment);
+  while ( next_segment == current_segment ) {
+    // the segment buffer is full, we are way ahead of the motion system.  wait here.
+    delay(1);
+  }
+
+  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
+  fr_mm_s *= (float)speed_adjust * 0.01f;
+#endif
+
+  new_seg.busy = false;
+  new_seg.distance = distance_mm;
+  float inverse_distance_mm = 1.0 / new_seg.distance;
+  float inverse_secs = fr_mm_s * inverse_distance_mm;
+
+  // The axis that has the most steps will control the overall acceleration as per bresenham's algorithm.
+  new_seg.steps_total = 0;
+  for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    new_seg.a[i].step_count = steps[i];
+    new_seg.a[i].delta_steps = steps[i] - old_seg.a[i].step_count;
+    new_seg.a[i].delta_mm = new_seg.a[i].delta_steps * THREAD_PER_STEP;
+    new_seg.a[i].dir = ( new_seg.a[i].delta_steps < 0 ? HIGH : LOW );
+    new_seg.a[i].absdelta = abs(new_seg.a[i].delta_steps);
+    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;
+
+  new_seg.nominal_speed = new_seg.distance * inverse_secs;
+  new_seg.nominal_rate = ceil(new_seg.steps_total * inverse_secs);
+
+  // Calculate the the speed limit for each axis
+  float current_speed[NUM_MOTORS + NUM_SERVOS], speed_factor = 1.0;
+  for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    current_speed[i] = new_seg.a[i].delta_mm * inverse_secs;
+    const float cs = fabs(current_speed[i]);
+    if (cs > max_feedrate_mm_s[i]) speed_factor = min (speed_factor, max_feedrate_mm_s[i] / cs);
+    //if (cs > MAX_FEEDRATE) speed_factor = min (speed_factor, MAX_FEEDRATE / cs);
+  }
+
+  if (speed_factor < 1.0) {
+    for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+      current_speed[i] *= speed_factor;
+    }
+    new_seg.nominal_speed *= speed_factor;
+    new_seg.nominal_rate *= speed_factor;
+  }
+
+  const float steps_per_mm = new_seg.steps_total * inverse_distance_mm;
+  uint32_t accel = ceil( acceleration * steps_per_mm );
+
+  const float max_acceleration_steps_per_s2 = acceleration * steps_per_mm;
+  for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    if (new_seg.a[i].delta_steps && max_acceleration_steps_per_s2 < accel) {
+      const float comp = (float)max_acceleration_steps_per_s2 * (float)new_seg.steps_total;
+      if ((float)accel * (float)new_seg.a[i].delta_steps > comp ) {
+        accel = comp / (float)new_seg.a[i].delta_steps;
+      }
+    }
+  }
+  new_seg.acceleration_steps_per_s2 = accel;
+  new_seg.acceleration = accel / steps_per_mm;
+  new_seg.acceleration_rate = (uint32_t)(accel * (4096.0f * 4096.0f / (TIMER_RATE)));
+  new_seg.steps_taken = 0;
+
+  // TODO explain this
+  float safe_speed = new_seg.nominal_speed;
+  char limited = 0;
+  for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    const float jerk = fabs(current_speed[i]), maxj = max_xy_jerk;
+    if (jerk > maxj) {
+      if (limited) {
+        // TODO explain this
+        const float mjerk = maxj * new_seg.nominal_speed;
+        if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
+      } else {
+        ++limited;
+        safe_speed *= maxj / jerk;
+      }
+    }
+  }
+
+  // what is the maximum starting speed for this segment?
+  float vmax_junction = MIN_FEEDRATE;
+
+  int movesQueued = movesPlanned();
+  if (movesQueued > 0 && previous_safe_speed > 0.0001) {
+    // Estimate a maximum velocity allowed at a joint of two successive segments.
+    // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
+    // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
+
+    // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
+    // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
+    vmax_junction = min(new_seg.nominal_speed, previous_nominal_speed);
+    const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
+    // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
+    float v_factor = 1.0f;
+    limited = 0;
+    // Now limit the jerk in all axes.
+    for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+      // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
+      float v_exit = previous_speed[i] * smaller_speed_factor;
+      float v_entry = current_speed[i];
+      if (limited) {
+        v_exit *= v_factor;
+        v_entry *= v_factor;
+      }
+
+      // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
+      const float jerk = (v_exit > v_entry)
+                         ? //                            coasting             axis reversal
+                         ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : max(v_exit, -v_entry) )
+                         : // v_exit <= v_entry          coasting             axis reversal
+                         ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : max(-v_exit, v_entry) );
+
+      if (jerk > max_xy_jerk) {
+        v_factor *= max_xy_jerk / jerk;
+        ++limited;
+      }
+    }
+    if (limited) vmax_junction *= v_factor;
+    // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
+    // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
+    const float vmax_junction_threshold = vmax_junction * 0.99f;
+    if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
+      // Not coasting. The machine will stop and start the movements anyway,
+      // better to start the segment from start.
+      //SBI(new_seg.flag, BLOCK_BIT_START_FROM_FULL_HALT);
+      vmax_junction = safe_speed;
+    }
+  }
+
+  float allowable_speed = max_speed_allowed(-new_seg.acceleration, MIN_FEEDRATE, new_seg.distance);
+
+#if NUM_SERVOS>0
+  // come to a stop for entering or exiting a Z move
+  //if( new_seg.a[NUM_SERVOS].delta_steps != 0 || old_seg.a[NUM_SERVOS].delta_steps != 0 ) allowable_speed = MIN_FEEDRATE;
+#endif
+
+  new_seg.entry_speed_max = vmax_junction;
+  new_seg.entry_speed = min(vmax_junction, allowable_speed);
+  new_seg.nominal_length_flag = ( allowable_speed >= new_seg.nominal_speed );
+  new_seg.recalculate_flag = true;
+
+  previous_nominal_speed = new_seg.nominal_speed;
+  previous_safe_speed = safe_speed;
+  for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    previous_speed[i] = current_speed[i];
+  }
+
+  // when should we accelerate and decelerate in this segment?
+  segment_update_trapezoid(&new_seg, new_seg.entry_speed / new_seg.nominal_speed, (float)MIN_FEEDRATE / new_seg.nominal_speed);
+
+  if (current_segment == last_segment ) {
+    first_segment_delay = BLOCK_DELAY_FOR_1ST_MOVE;
+  }
+  last_segment = next_segment;
+
+  recalculate_acceleration();
+  /*
+    Serial.print("distance=");  Serial.println(new_seg.distance);
+    Serial.print("acceleration original=");  Serial.println(acceleration);
+    Serial.print("nominal_speed=");  Serial.println(new_seg.nominal_speed);
+
+    Serial.print("inverse_distance_mm=");  Serial.println(inverse_distance_mm);
+    Serial.print("inverse_secs=");  Serial.println(inverse_secs);
+    Serial.print("nominal_rate=");  Serial.println(new_seg.nominal_rate);
+    Serial.print("delta_mm=");
+    for (i = 0; i < NUM_MOTORS + NUM_SERVOS; ++i) {
+    if (i > 0) Serial.print(", ");
+    Serial.print(new_seg.a[i].delta_mm);
+    }
+    Serial.println();
+    Serial.print("speed_factor=");  Serial.println(speed_factor);
+    Serial.print("steps_per_mm=");  Serial.println(steps_per_mm);
+    Serial.print("accel=");  Serial.println(accel);
+    Serial.print("acceleration_steps_per_s2=");  Serial.println(new_seg.acceleration_steps_per_s2);
+    Serial.print("acceleration=");  Serial.println(new_seg.acceleration);
+    Serial.print("limited=");  Serial.println(limited, DEC);
+    Serial.print("nominal_speed=");  Serial.println(new_seg.nominal_speed);
+    Serial.print("vmax_junction=");  Serial.println(vmax_junction);
+    Serial.print("allowable_speed=");  Serial.println(allowable_speed);
+    Serial.print("safe_speed=");  Serial.println(safe_speed);
+    //*/
+}
+
+
+void wait_for_empty_segment_buffer() {
+  while ( current_segment != last_segment );
+}
diff --git a/motor.h b/motor.h
new file mode 100644
index 0000000..060a35b
--- /dev/null
+++ b/motor.h
@@ -0,0 +1,84 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+//------------------------------------------------------------------------------
+// Makelangelo - firmware for various robot kinematic models
+// dan@marginallycelver.com 2013-12-26
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+//------------------------------------------------------------------------------
+
+#include <Arduino.h>
+
+//------------------------------------------------------------------------------
+// CONSTANTS
+//------------------------------------------------------------------------------
+
+
+typedef struct {
+  int step_pin;
+  int dir_pin;
+  int enable_pin;
+  int limit_switch_pin;
+} Motor;
+
+
+// for line()
+typedef struct {
+  int32_t step_count;  // current motor position, in steps.
+  int32_t delta_steps;  // steps
+  int32_t delta_mm;  // mm
+  uint32_t absdelta;
+  int dir;
+} SegmentAxis;
+
+
+typedef struct {
+  SegmentAxis a[NUM_MOTORS+NUM_SERVOS];
+
+  float distance;         // mm
+  float nominal_speed;    // mm/s
+  float entry_speed;      // mm/s
+  float entry_speed_max;  // mm/s
+  float acceleration;     // mm/sec^2
+
+  uint32_t steps_total;    // steps
+  uint32_t steps_taken;    // steps
+  uint32_t accel_until;    // steps
+  uint32_t decel_after;    // steps
+  
+  uint32_t nominal_rate;   // steps/s
+  uint32_t initial_rate;     // steps/s
+  uint32_t final_rate;      // steps/s
+  uint32_t acceleration_steps_per_s2;  // steps/s^2
+  uint32_t acceleration_rate;  // 
+  
+  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 Motor motors[NUM_MOTORS+NUM_SERVOS];
+extern const char *AxisNames;
+extern const char *MotorNames;
+extern float maxFeedRate[NUM_MOTORS];
+extern float max_xy_jerk;
+extern float max_feedrate_mm_s[NUM_MOTORS+NUM_SERVOS];
+
+extern void motor_set_step_count(long *a);
+extern void wait_for_empty_segment_buffer();
+extern char segment_buffer_full();
+extern void motor_line(const float * const target_position,float &fr_mm_s);
+extern void motor_engage();
+extern void motor_disengage();
+extern void motor_setup();
+extern void setPenAngle(int arg0);
+
+#endif // MOTOR_H
diff --git a/motor.ino b/motor.ino
deleted file mode 100644
index a83be7a..0000000
--- a/motor.ino
+++ /dev/null
@@ -1,926 +0,0 @@
-//------------------------------------------------------------------------------
-// Makelangelo - firmware for various robot kinematic models
-// dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
-//------------------------------------------------------------------------------
-
-
-//------------------------------------------------------------------------------
-// INCLUDES
-//------------------------------------------------------------------------------
-#include "MServo.h"
-
-
-//------------------------------------------------------------------------------
-// GLOBALS
-//------------------------------------------------------------------------------
-
-Motor motors[NUM_MOTORS+NUM_SERVOS];
-Servo servos[NUM_SERVOS];
-
-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;
-
-// used by timer1 to optimize interrupt inner loop
-int steps_total;
-int steps_taken;
-int accel_until,decel_after;
-long current_feed_rate;
-long current_acceleration;
-long old_feed_rate=0;
-long start_feed_rate,end_feed_rate;
-long time_accelerating,time_decelerating;
-float max_xy_jerk = MAX_JERK;
-
-int delta0;
-int over0;
-long global_steps_0;
-int global_step_dir_0;
-#if NUM_MOTORS>1
-int delta1;
-int over1;
-long global_steps_1;
-int global_step_dir_1;
-#endif
-#if NUM_MOTORS>2
-int delta2;
-int over2;
-long global_steps_2;
-int global_step_dir_2;
-#endif
-#if NUM_MOTORS>3
-int delta3;
-int over3;
-long global_steps_3;
-int global_step_dir_3;
-#endif
-#if NUM_MOTORS>4
-int delta4;
-int over4;
-long global_steps_4;
-int global_step_dir_4;
-#endif
-#if NUM_MOTORS>5
-int delta5;
-int over5;
-long global_steps_5;
-int global_step_dir_5;
-#endif
-
-
-const char *MotorNames="LRUVWT";
-const char *AxisNames="XYZUVWT";
-float maxFeedRate[NUM_MOTORS];
-
-
-//------------------------------------------------------------------------------
-// METHODS
-//------------------------------------------------------------------------------
-
-
-// for reasons I don't understand... if i put this method in the .h 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 );
-}
-
-/**
- * Calculate the maximum allowable speed at this point, in order
- * to reach 'target_velocity' using 'acceleration' within a given
- * 'distance'.
- * @param acc acceleration
- * @param target_velocity 
- * @param distance 
- */
-float max_speed_allowed(float acc, float target_velocity, float distance) {
-  return sqrt( target_velocity*target_velocity - 2 * acc * 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;
-#if NUM_MOTORS>1
-  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;
-#endif
-#if NUM_MOTORS>2
-  motors[2].step_pin        =MOTOR_2_STEP_PIN;
-  motors[2].dir_pin         =MOTOR_2_DIR_PIN;
-  motors[2].enable_pin      =MOTOR_2_ENABLE_PIN;
-  motors[2].limit_switch_pin=MOTOR_2_LIMIT_SWITCH_PIN;
-#endif
-#if NUM_MOTORS>3
-  motors[3].step_pin        =MOTOR_3_STEP_PIN;
-  motors[3].dir_pin         =MOTOR_3_DIR_PIN;
-  motors[3].enable_pin      =MOTOR_3_ENABLE_PIN;
-  motors[3].limit_switch_pin=MOTOR_3_LIMIT_SWITCH_PIN;
-#endif
-#if NUM_MOTORS>4
-  motors[4].step_pin        =MOTOR_4_STEP_PIN;
-  motors[4].dir_pin         =MOTOR_4_DIR_PIN;
-  motors[4].enable_pin      =MOTOR_4_ENABLE_PIN;
-  motors[4].limit_switch_pin=MOTOR_4_LIMIT_SWITCH_PIN;
-#endif
-#if NUM_MOTORS>5
-  motors[5].step_pin        =MOTOR_5_STEP_PIN;
-  motors[5].dir_pin         =MOTOR_5_DIR_PIN;
-  motors[5].enable_pin      =MOTOR_5_ENABLE_PIN;
-  motors[5].limit_switch_pin=MOTOR_5_LIMIT_SWITCH_PIN;
-#endif
-
-  int i;
-  for(i=0;i<NUM_MOTORS;++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
-    pinMode(motors[i].limit_switch_pin,INPUT);
-    digitalWrite(motors[i].limit_switch_pin,HIGH);
-    maxFeedRate[i] = MAX_FEEDRATE;
-  }
-
-  long steps[NUM_MOTORS+NUM_SERVOS];
-  memset(steps,0,(NUM_MOTORS+NUM_SERVOS)*sizeof(long));
-  motor_set_step_count(steps);
-
-  // 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;
-#if NUM_MOTORS>1
-  old_seg.a[1].step_count=0;
-#endif
-#if NUM_MOTORS>2
-  old_seg.a[2].step_count=0;
-#endif
-#if NUM_MOTORS>3
-  old_seg.a[3].step_count=0;
-#endif
-#if NUM_MOTORS>4
-  old_seg.a[4].step_count=0;
-#endif
-#if NUM_MOTORS>5
-  old_seg.a[5].step_count=0;
-#endif
-
-#if NUM_SERVOS>0
-  old_seg.a[NUM_MOTORS].step_count=0;
-#endif
-
-  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);
-
-  interrupts();  // enable global interrupts
-}
-
-
-// turn on power to the motors (make them immobile)
-void motor_engage() {
-  int i;
-  for(i=0;i<NUM_MOTORS;++i) {
-    digitalWrite(motors[i].enable_pin,LOW);
-  }
-/*
-#if MACHINE_STYLE == ARM6
-  // DM320T drivers want high for enabled
-  digitalWrite(motors[4].enable_pin,HIGH);
-  digitalWrite(motors[5].enable_pin,HIGH);
-#endif*/
-}
-
-
-// turn off power to the motors (make them move freely)
-void motor_disengage() {
-  int i;
-  for(i=0;i<NUM_MOTORS;++i) {
-    digitalWrite(motors[i].enable_pin,HIGH);
-  }/*
-#if MACHINE_STYLE == ARM6
-  // DM320T drivers want low for disabled
-  digitalWrite(motors[4].enable_pin,LOW);
-  digitalWrite(motors[5].enable_pin,LOW);
-#endif*/
-}
-
-
-// Change pen state.
-void setPenAngle(int arg0) {
-#if NUM_AXIES>=3
-  if(arg0 < axies[2].limitMin) arg0=axies[2].limitMin;
-  if(arg0 > axies[2].limitMax) arg0=axies[2].limitMax;
-  
-  axies[2].pos = arg0;
-  
-#if NUM_SERVOS>0
-  servos[0].write(arg0);
-#endif
-#endif
-}
-
-
-
-
-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_time(float accel,float distance,float start_speed,float end_speed) {
-#if 0
-  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
-}
-
-
-void segment_update_trapezoid(Segment *s,float start_speed,float end_speed) {
-  if(start_speed<MIN_FEEDRATE) start_speed=MIN_FEEDRATE;
-  if(end_speed<MIN_FEEDRATE) end_speed=MIN_FEEDRATE;
-
-  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 ) / acceleration );
-  //int steps_to_decel = floor( ( end_speed - s->feed_rate_max ) / -acceleration );
-
-  int steps_at_top_speed = s->steps_total - steps_to_accel - steps_to_decel;
-  if(steps_at_top_speed<0) {
-    steps_to_accel = ceil( intersection_time(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 = start_speed;
-    s->feed_rate_end = end_speed;
-  }
-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 *a) {
-  wait_for_empty_segment_buffer();
-
-  Segment &old_seg = line_segments[get_prev_segment(last_segment)];
-  old_seg.a[0].step_count=a[0];
-#if NUM_MOTORS>1
-  old_seg.a[1].step_count=a[1];
-#endif
-#if NUM_MOTORS>2
-  old_seg.a[2].step_count=a[2];
-#endif
-#if NUM_MOTORS>3
-  old_seg.a[3].step_count=a[3];
-#endif
-#if NUM_MOTORS>4
-  old_seg.a[4].step_count=a[4];
-#endif
-#if NUM_MOTORS>5
-  old_seg.a[5].step_count=a[5];
-#endif
-#if NUM_SERVOS>0
-  old_seg.a[NUM_MOTORS].step_count=a[NUM_MOTORS];
-#endif
-
-  global_steps_0=0;
-#if NUM_MOTORS>1
-  global_steps_1=0;
-#endif
-#if NUM_MOTORS>2
-  global_steps_2=0;
-#endif
-#if NUM_MOTORS>3
-  global_steps_3=0;
-#endif
-#if NUM_MOTORS>4
-  global_steps_4=0;
-#endif
-#if NUM_MOTORS>5
-  global_steps_5=0;
-#endif
-}
-
-
-/**
- * Step one motor one time in the currently set direction.
- * @input newx the destination x position
- * @input newy the destination y position
- **/
-void motor_onestep(int motor) {
-#ifdef VERBOSE
-  Serial.print(motorNames[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;
-  }
-
-  return counter_value;
-}
-
-
-/**
- * Process all line segments in the ring buffer.  Uses bresenham's line algorithm to move all motors.
- */
-ISR(TIMER1_COMPA_vect) {
-  // 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 );
-      global_step_dir_0 = (working_seg->a[0].dir==HIGH)?1:-1;
-
-      #if NUM_MOTORS>1
-      digitalWrite( MOTOR_1_DIR_PIN, working_seg->a[1].dir );
-      global_step_dir_1 = (working_seg->a[1].dir==HIGH)?1:-1;
-      #endif
-      #if NUM_MOTORS>2
-      digitalWrite( MOTOR_2_DIR_PIN, working_seg->a[2].dir );
-      global_step_dir_2 = (working_seg->a[2].dir==HIGH)?1:-1;
-      #endif
-      #if NUM_MOTORS>3
-      digitalWrite( MOTOR_3_DIR_PIN, working_seg->a[3].dir );
-      global_step_dir_3 = (working_seg->a[3].dir==HIGH)?1:-1;
-      #endif
-      #if NUM_MOTORS>4
-      digitalWrite( MOTOR_4_DIR_PIN, working_seg->a[4].dir );
-      global_step_dir_4 = (working_seg->a[4].dir==HIGH)?1:-1;
-      #endif
-      #if NUM_MOTORS>5
-      digitalWrite( MOTOR_5_DIR_PIN, working_seg->a[5].dir );
-      global_step_dir_5 = (working_seg->a[5].dir==HIGH)?1:-1;
-      #endif
-
-      #if NUM_SERVOS>0
-      servos[0].write(working_seg->a[NUM_MOTORS].step_count);
-      #endif
-
-      // 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;
-      current_acceleration = acceleration;
-      time_decelerating = 0;
-      time_accelerating = calc_timer(start_feed_rate);
-      OCR1A = time_accelerating;
-
-      // defererencing some data so the loop runs faster.
-      steps_total=working_seg->steps_total;
-      steps_taken=0;
-      delta0 = working_seg->a[0].absdelta;      over0 = -(steps_total>>1);
-      #if NUM_MOTORS>1
-      delta1 = working_seg->a[1].absdelta;      over1 = -(steps_total>>1);
-      #endif
-      #if NUM_MOTORS>2
-      delta2 = working_seg->a[2].absdelta;      over2 = -(steps_total>>1);
-      #endif
-      #if NUM_MOTORS>3
-      delta3 = working_seg->a[3].absdelta;      over3 = -(steps_total>>1);
-      #endif
-      #if NUM_MOTORS>4
-      delta4 = working_seg->a[4].absdelta;      over4 = -(steps_total>>1);
-      #endif
-      #if NUM_MOTORS>5
-      delta5 = working_seg->a[5].absdelta;      over5 = -(steps_total>>1);
-      #endif
-      accel_until=working_seg->accel_until;
-      decel_after=working_seg->decel_after;
-      return;
-    } else {
-      OCR1A = 2000; // wait 1ms
-      return;
-    }
-  }
-
-  if( working_seg != NULL ) {
-    // move each axis
-    for(uint8_t i=0;i<step_multiplier;++i) {
-      over0 += delta0;
-      if(over0 > 0) digitalWrite(MOTOR_0_STEP_PIN,LOW);
-#if NUM_MOTORS>1
-      over1 += delta1;
-      if(over1 > 0) digitalWrite(MOTOR_1_STEP_PIN,LOW);
-#endif
-#if NUM_MOTORS>2
-      over2 += delta2;
-      if(over2 > 0) digitalWrite(MOTOR_2_STEP_PIN,LOW);
-#endif
-#if NUM_MOTORS>3
-      over3 += delta3;
-      if(over3 > 0) digitalWrite(MOTOR_3_STEP_PIN,LOW);
-#endif
-#if NUM_MOTORS>4
-      over4 += delta4;
-      if(over4 > 0) digitalWrite(MOTOR_4_STEP_PIN,LOW);
-#endif
-#if NUM_MOTORS>5
-      over5 += delta5;
-      if(over5 > 0) digitalWrite(MOTOR_5_STEP_PIN,LOW);
-#endif
-      // now that the pins have had a moment to settle, do the second half of the steps.
-      // M0
-      if(over0 > 0) {
-        over0 -= steps_total;
-        global_steps_0+=global_step_dir_0;
-        digitalWrite(MOTOR_0_STEP_PIN,HIGH);
-      }
-#if NUM_MOTORS>1
-      // M1
-      if(over1 > 0) {
-        over1 -= steps_total;
-        global_steps_1+=global_step_dir_1;
-        digitalWrite(MOTOR_1_STEP_PIN,HIGH);
-      }
-#endif
-#if NUM_MOTORS>2
-      // M2
-      if(over2 > 0) {
-        over2 -= steps_total;
-        global_steps_2+=global_step_dir_2;
-        digitalWrite(MOTOR_2_STEP_PIN,HIGH);
-      }
-#endif
-#if NUM_MOTORS>3
-      // M3
-      if(over3 > 0) {
-        over3 -= steps_total;
-        global_steps_3+=global_step_dir_3;
-        digitalWrite(MOTOR_3_STEP_PIN,HIGH);
-      }
-#endif
-#if NUM_MOTORS>4
-      // M4
-      if(over4 > 0) {
-        over4 -= steps_total;
-        global_steps_4+=global_step_dir_4;
-        digitalWrite(MOTOR_4_STEP_PIN,HIGH);
-      }
-#endif
-#if NUM_MOTORS>5
-      // M5
-      if(over5 > 0) {
-        over5 -= steps_total;
-        global_steps_5+=global_step_dir_5;
-        digitalWrite(MOTOR_5_STEP_PIN,HIGH);
-      }
-#endif
-      
-      // make a step
-      steps_taken++;
-      if(steps_taken>=steps_total) break;
-    }
-
-    // accel
-    unsigned short t;
-    if( steps_taken <= accel_until ) {
-      current_feed_rate = start_feed_rate + (current_acceleration * time_accelerating / 1000000);
-      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;
-      time_accelerating+=t;
-    } else if( steps_taken > decel_after ) {
-      long end_feed_rate = current_feed_rate - (current_acceleration * time_decelerating / 1000000);
-      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;
-      time_decelerating+=t;
-    } else {
-      OCR1A = nominal_OCR1A;
-      step_multiplier = nominal_step_multiplier;
-    }
-
-    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
- * @param n (NUM_MOTORS+NUM_SERVOS) number of steps, one for each motor/servo
- **/
-void motor_line(long *n,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
-    delay(1);
-  }
-
-  int prev_segment = get_prev_segment(last_segment);
-  Segment &new_seg = line_segments[last_segment];
-  Segment &old_seg = line_segments[prev_segment];
-
-/*
-  int k;
-  for(k=0;k<NUM_MOTORS+NUM_SERVOS;++k) {
-    Serial.print(n[k]);
-    Serial.print(",\t");
-  }
-  Serial.print('\n');//*/
-  
-  // use LCD to adjust speed while drawing
-#ifdef HAS_LCD
-  new_feed_rate *= (float)speed_adjust * 0.01f;
-#endif
-
-  new_seg.a[0].step_count = n[0];
-  new_seg.a[0].delta = n[0] - old_seg.a[0].step_count;
-#if NUM_MOTORS>1
-  new_seg.a[1].step_count = n[1];
-  new_seg.a[1].delta = n[1] - old_seg.a[1].step_count;
-#endif
-#if NUM_MOTORS>2
-  new_seg.a[2].step_count = n[2];
-  new_seg.a[2].delta = n[2] - old_seg.a[2].step_count;
-#endif
-#if NUM_MOTORS>3
-  new_seg.a[3].step_count = n[3];
-  new_seg.a[3].delta = n[3] - old_seg.a[3].step_count;
-#endif
-#if NUM_MOTORS>4
-  new_seg.a[4].step_count = n[4];
-  new_seg.a[4].delta = n[4] - old_seg.a[4].step_count;
-#endif
-#if NUM_MOTORS>5
-  new_seg.a[5].step_count = n[5];
-  new_seg.a[5].delta = n[5] - old_seg.a[5].step_count;
-#endif
-
-#if NUM_SERVOS>0
-  new_seg.a[NUM_MOTORS].step_count = n[NUM_MOTORS];
-  new_seg.a[NUM_MOTORS].delta = n[NUM_MOTORS] - old_seg.a[NUM_MOTORS].step_count;
-#endif
-
-  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_MOTORS+NUM_SERVOS;++i) {
-    new_seg.a[i].dir = ( new_seg.a[i].delta < 0 ? HIGH : LOW );
-    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;
-    }
-    //Serial.print(i);
-    //Serial.print('\t');    Serial.print(n[i]);
-    //Serial.print('\t');    Serial.print(old_seg.a[i].step_count);
-    //Serial.print('\t');    Serial.print(new_seg.a[i].step_count);
-    //Serial.print('\t');    Serial.print(new_seg.a[i].dir);
-    //Serial.print('\t');    Serial.print(new_seg.a[i].absdelta);
-    //Serial.println();
-  }
-
-  // No steps?  No work!  Stop now.
-  if( new_seg.steps_total == 0 ) return;
-
-  //Serial.println(new_seg.steps_total);
-
-  len = sqrt( len );
-  float ilen = 1.0f / len;
-  float iSecond = new_feed_rate * ilen;
-  
-  for(i=0;i<NUM_MOTORS;++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 sum=0, d;
-    d = new_seg.a[0].delta_normalized - old_seg.a[0].delta_normalized;    sum += d*d;
-#if NUM_MOTORS>1
-    d = new_seg.a[1].delta_normalized - old_seg.a[1].delta_normalized;    sum += d*d;
-#endif
-#if NUM_MOTORS>2
-    d = new_seg.a[2].delta_normalized - old_seg.a[2].delta_normalized;    sum += d*d;
-#endif
-#if NUM_MOTORS>3
-    d = new_seg.a[3].delta_normalized - old_seg.a[3].delta_normalized;    sum += d*d;
-#endif
-#if NUM_MOTORS>4
-    d = new_seg.a[4].delta_normalized - old_seg.a[4].delta_normalized;    sum += d*d;
-#endif
-#if NUM_MOTORS>5
-    d = new_seg.a[5].delta_normalized - old_seg.a[5].delta_normalized;    sum += d*d;
-#endif
-
-#if NUM_SERVOS>0
-    d = new_seg.a[NUM_SERVOS].delta_normalized - old_seg.a[NUM_SERVOS].delta_normalized;    sum += d*d;
-#endif
-
-    
-    float jerk = sqrt(sum);
-    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);
-
-#if NUM_SERVOS>0
-  // come to a stop for entering or exiting a Z move
-  //if( new_seg.a[NUM_SERVOS].delta != 0 || old_seg.a[NUM_SERVOS].delta != 0 ) allowable_speed = MIN_FEEDRATE;
-#endif
-
-  //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 );
-}
-
-
-/**
- * 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/robot_arm3.ino b/robot_arm3.cpp
similarity index 95%
rename from robot_arm3.ino
rename to robot_arm3.cpp
index e6b3878..e9ffba2 100644
--- a/robot_arm3.ino
+++ b/robot_arm3.cpp
@@ -1,12 +1,13 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == ARM3
+#include "configure.h"
+#include "robot_arm3.h"
 
+#if MACHINE_STYLE == ARM3
 
 /**
  * Inverse Kinematics turns XY coordinates into step counts from each motor
@@ -14,7 +15,7 @@
  * @param y cartesian coordinate
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *axies, long *motorStepArray) {
+void IK(const float *const axies, long *motorStepArray) {
   float x = axies[0];
   float y = axies[1];
   float z = axies[2];
diff --git a/robot_arm3.h b/robot_arm3.h
index eae2f2c..16d4057 100644
--- a/robot_arm3.h
+++ b/robot_arm3.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == ARM3
diff --git a/robot_arm6.ino b/robot_arm6.cpp
similarity index 79%
rename from robot_arm6.ino
rename to robot_arm6.cpp
index 2bc09ff..d290200 100644
--- a/robot_arm6.ino
+++ b/robot_arm6.cpp
@@ -1,10 +1,12 @@
   //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
+#include "configure.h"
+#include "robot_arm6.h"
+
 #if MACHINE_STYLE == ARM6
 
 #include "Vector3.h"
@@ -15,17 +17,41 @@
    @param axies the cartesian coordinate
    @param motorStepArray a measure of each belt to that plotter position
 */
-void IK(float *axies, long *motorStepArray) {
+void IK(const float *const axies, long *motorStepArray) {
+#if MACHINE_HARDWARE_VERSION==5
   float x = -axies[0];
   float y = -axies[1];
   float z = -axies[2];
   float u = -axies[3];
   float v =  axies[4];
   float w = -axies[5];
+#endif
+
+#if MACHINE_HARDWARE_VERSION==6
+  // each of the xyz motors are differential to each other.
+  // to move only one motor means applying the negative of that value to the other two motors
+
+  // consider a two motor differential: 
+  // if x moves, subtract x from y.
+  // if y moves, subtract y from x.
+  // so for three axis,
+  // for any axis N subtract the other two axies from this axis.
+
+  float a=axies[0];  // hand (G0 X*)
+  float b=axies[1];  // wrist (G0 Y*)
+  float c=axies[2];  // ulna (G0 Z*)
+  
+  float x = a+b+c;  // supposed to move hand
+  float y = b+c;  // supposed to move wrist
+  float z = c;  // supposed to move ulna
+  float u = -axies[3];
+  float v =  axies[4];
+  float w = -axies[5];
+#endif
   
-  motorStepArray[0] = x * MOTOR_0_STEPS_PER_TURN / 360.0;
-  motorStepArray[1] = y * MOTOR_1_STEPS_PER_TURN / 360.0;
-  motorStepArray[2] = z * MOTOR_2_STEPS_PER_TURN / 360.0;
+  motorStepArray[0] = y * MOTOR_0_STEPS_PER_TURN / 360.0;  // WRIST
+  motorStepArray[1] = x * MOTOR_1_STEPS_PER_TURN / 360.0;  // HAND
+  motorStepArray[2] = z * MOTOR_2_STEPS_PER_TURN / 360.0;  // ULNA
   motorStepArray[3] = u * MOTOR_3_STEPS_PER_TURN / 360.0;
   motorStepArray[4] = v * MOTOR_4_STEPS_PER_TURN / 360.0;
   motorStepArray[5] = w * MOTOR_5_STEPS_PER_TURN / 360.0;
@@ -65,7 +91,7 @@ void robot_findHome() {
 
   int homeDirections[NUM_MOTORS] = {HOME_DIR_0,HOME_DIR_1,HOME_DIR_2,HOME_DIR_3,HOME_DIR_4,HOME_DIR_5};
   
-  char i;
+  uint8_t i;
   // for each stepper,
   for (i = 0; i < NUM_MOTORS; ++i) {
     Serial.print("Homing ");
diff --git a/robot_arm6.h b/robot_arm6.h
index 4734ada..045b1b3 100644
--- a/robot_arm6.h
+++ b/robot_arm6.h
@@ -3,21 +3,21 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == ARM6
 
 #define MACHINE_STYLE_NAME           "ARM6"
-#define MACHINE_HARDWARE_VERSION     5
+//#define MACHINE_HARDWARE_VERSION     5  // red sixi 2017-2018
+#define MACHINE_HARDWARE_VERSION     6  // yellow sixi 2019
 
 #define STEP_DELAY           (1000)  // delay between steps, in milliseconds, when doing fixed tasks like homing
 
 #define MAX_ACCELERATION     (5000)
 #define MIN_ACCELERATION     (100)
 
-#define SUBDIVIDE_LINES
+//#define SUBDIVIDE_LINES
 #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?
 
@@ -26,15 +26,97 @@
 #define NUM_SERVOS           (1)
 #define NUM_TOOLS            (1)
 
-#define MAX_FEEDRATE         (4000.0)  // depends on timer interrupt & hardware
-#define MIN_FEEDRATE         (100)
+#define MAX_FEEDRATE         (400.0)  // depends on timer interrupt & hardware
+#define MIN_FEEDRATE         (0)
 #define MAX_JERK             (5.0)
-#define DEFAULT_FEEDRATE     (1500.0)
-#define DEFAULT_ACCELERATION (1500)
+#define DEFAULT_FEEDRATE     (100.0)
+#define DEFAULT_ACCELERATION (500)
 
 //#define HAS_LCD
 #define HAS_SD
 
+#if MACHINE_HARDWARE_VERSION==6
+
+#define MOTOR_MICROSTEPS_EXT      ((1.0*200.0))  // motor full steps * microstepping setting
+#define MOTOR_MICROSTEPS_MID      ((1.0*400.0))  // motor full steps * microstepping setting
+
+#define NEMA17_CYCLOID_GEARBOX_RATIO (40.0)
+#define ELBOW_DOWNGEAR_RATIO         (30.0/14.0)
+#define NEMA17_RATIO                 (NEMA17_CYCLOID_GEARBOX_RATIO*ELBOW_DOWNGEAR_RATIO)
+
+#define MOTOR_0_STEPS_PER_TURN    (MOTOR_MICROSTEPS_MID*NEMA17_RATIO)
+#define MOTOR_1_STEPS_PER_TURN    (MOTOR_MICROSTEPS_MID*NEMA17_RATIO)
+#define MOTOR_2_STEPS_PER_TURN    (MOTOR_MICROSTEPS_EXT*NEMA17_RATIO)
+
+#define MOTOR_3_STEPS_PER_TURN    (MOTOR_MICROSTEPS_MID*NEMA17_RATIO)
+#define MOTOR_4_STEPS_PER_TURN    (MOTOR_MICROSTEPS_EXT*NEMA17_RATIO)
+#define MOTOR_5_STEPS_PER_TURN    (MOTOR_MICROSTEPS_EXT*NEMA17_RATIO)
+
+
+// DIMENSIONS
+
+#define ADJUST_WRIST_ELBOW_ANGLE      (14.036243)
+#define ADJUST_SHOULDER_ELBOW_ANGLE   (11.309932)
+#define ADJUST_ULNA_ELBOW_ANGLE       (26.56505117707799)
+
+// Relative offset from joint N to joint N+1.
+// shoulder pan
+#define OFFSET_0X    0
+#define OFFSET_0Y    0
+#define OFFSET_0Z    0
+// shoulder tilt
+#define OFFSET_1X    0
+#define OFFSET_1Y    0
+#define OFFSET_1Z    258
+// elbow tilt
+#define OFFSET_2X    0
+#define OFFSET_2Y    50
+#define OFFSET_2Z    250
+// elbow pan
+#define OFFSET_3X    0
+#define OFFSET_3Y    -50
+#define OFFSET_3Z    100
+// wrist
+#define OFFSET_4X    0
+#define OFFSET_4Y    0
+#define OFFSET_4Z    100
+// wrist to wrist face
+#define OFFSET_5X    0
+#define OFFSET_5Y    0
+#define OFFSET_5Z    50
+
+// angle (degrees) when homing switches report in
+#define MIN_ANGLE_0 0
+#define MIN_ANGLE_1 -90
+#define MIN_ANGLE_2 -11.1
+#define MIN_ANGLE_3 0
+#define MIN_ANGLE_4 -90
+#define MIN_ANGLE_5 0
+
+#define MAX_ANGLE_0 180
+#define MAX_ANGLE_1 90
+#define MAX_ANGLE_2 (180-11.1)
+#define MAX_ANGLE_3 180
+#define MAX_ANGLE_4 90
+#define MAX_ANGLE_5 180
+
+#define HOME_ANGLE_0 MIN_ANGLE_0
+#define HOME_ANGLE_1 MIN_ANGLE_1
+#define HOME_ANGLE_2 MIN_ANGLE_2
+#define HOME_ANGLE_3 MIN_ANGLE_3
+#define HOME_ANGLE_4 MIN_ANGLE_4
+#define HOME_ANGLE_5 MIN_ANGLE_5
+
+#define HOME_DIR_0 HIGH
+#define HOME_DIR_1 HIGH
+#define HOME_DIR_2 LOW
+#define HOME_DIR_3 HIGH
+#define HOME_DIR_4 LOW
+#define HOME_DIR_5 HIGH
+
+#endif  // MACHINE_HARDWARE_VERSION==6
+#if MACHINE_HARDWARE_VERSION==5
+
 #define MOTOR_MICROSTEPS_EXT      (1600)  // motor full steps * microstepping setting
 #define MOTOR_MICROSTEPS_MID      (3200)  // motor full steps * microstepping setting
 #define MOTOR_MICROSTEPS_INT      ( 200)  // motor full steps * microstepping setting
@@ -49,6 +131,7 @@
 #define MOTOR_4_STEPS_PER_TURN    ((unsigned long)MOTOR_MICROSTEPS_INT*NEMA14_RATIO)
 #define MOTOR_5_STEPS_PER_TURN    ((unsigned long)MOTOR_MICROSTEPS_INT*NEMA08_RATIO)
 
+
 // DIMENSIONS
 
 #define ADJUST_WRIST_ELBOW_ANGLE      (14.036243)
@@ -110,8 +193,9 @@
 #define HOME_DIR_4 LOW
 #define HOME_DIR_5 HIGH
 
+#endif  // MACHINE_HARDWARE_VERSION==5
+
 #endif  // #ifdef ARM6
 
 
 #endif  // #ifndef ROBOT_ARM6_H
-
diff --git a/robot_corexy.ino b/robot_corexy.cpp
similarity index 86%
rename from robot_corexy.ino
rename to robot_corexy.cpp
index a132172..52cf593 100644
--- a/robot_corexy.ino
+++ b/robot_corexy.cpp
@@ -1,19 +1,20 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == COREXY
+#include "configure.h"
+#include "robot_corexy.h"
 
+#if MACHINE_STYLE == COREXY
 
 /**
  * Inverse Kinematics turns XY coordinates into lengths L1,L2
  * @param axies the cartesian coordinate
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *axies, long *motorStepArray) {
+void IK(const float *const axies, long *motorStepArray) {
   float x = axies[0];
   float y = axies[1];
   float z = axies[2];
diff --git a/robot_corexy.h b/robot_corexy.h
index 750c6bb..07cb484 100644
--- a/robot_corexy.h
+++ b/robot_corexy.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == COREXY
diff --git a/robot_delta.ino b/robot_delta.cpp
similarity index 96%
rename from robot_delta.ino
rename to robot_delta.cpp
index bad1e75..64ef835 100644
--- a/robot_delta.ino
+++ b/robot_delta.cpp
@@ -1,12 +1,13 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == DELTA
+#include "configure.h"
+#include "robot_delta.h"
 
+#if MACHINE_STYLE == DELTA
 
 #define SQRT3   (sqrt(3.0))
 #define SIN120  (SQRT3/2.0)
@@ -21,7 +22,7 @@
  * @param axies the cartesian coordinate
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *axies, long *motorStepArray) {
+void IK(const float *const axies, long *motorStepArray) {
   float x = axies[0];
   float y = axies[1];
   float z = axies[2];
diff --git a/robot_delta.h b/robot_delta.h
index 6ef5e30..2e909e3 100644
--- a/robot_delta.h
+++ b/robot_delta.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == DELTA
diff --git a/robot_polargraph.ino b/robot_polargraph.cpp
similarity index 97%
rename from robot_polargraph.ino
rename to robot_polargraph.cpp
index 95f1264..16f004c 100644
--- a/robot_polargraph.ino
+++ b/robot_polargraph.cpp
@@ -1,19 +1,21 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == POLARGRAPH
+#include "configure.h"
+#include "robot_polargraph.h"
+#include "eeprom.h"
 
+#if MACHINE_STYLE == POLARGRAPH
 
 /**
  * Inverse Kinematics turns XY coordinates into step counts from each motor
  * @param axies the cartesian coordinate
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *cartesian, long *motorStepArray) {
+void IK(const float *const cartesian, long *motorStepArray) {
   float dy,dx;
   // find length to M1
   float limit_xmin = axies[0].limitMin;
@@ -76,6 +78,7 @@ int FK(long *motorStepArray,float *cartesian) {
   Serial.print("C1=");      Serial.println(cartesian[1]);
   Serial.print("C2=");      Serial.println(cartesian[2]);
   */
+  return 0;
 }
 
 
@@ -255,7 +258,7 @@ void robot_findHome() {
   offset[2]=axies[2].pos;
   Serial.print(F("Homing to "));  Serial.print  (axies[0].homePos);
   Serial.print(',');              Serial.println(axies[1].homePos);
-  lineSafe(offset, feed_rate);
+  lineSafe(offset, DEFAULT_FEEDRATE);
   
   Serial.println(F("Done."));
 #endif // USER_LIMIT_SWITCH
diff --git a/robot_polargraph.h b/robot_polargraph.h
index b826f6f..82a1004 100644
--- a/robot_polargraph.h
+++ b/robot_polargraph.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == POLARGRAPH
@@ -15,7 +14,7 @@
 #define MACHINE_HAS_LIFTABLE_PEN
 
 #define SUBDIVIDE_LINES
-#define SEGMENT_PER_CM_LINE  (2)  // lines are subdivided.  How long are the divisions?
+#define SEGMENT_PER_CM_LINE  (0.5)  // lines are subdivided.  How long are the divisions?
 #define SEGMENT_PER_CM_ARC   (3)  // Arcs are subdivided.  How long are the divisions?
 
 // servo angles for pen control
@@ -27,28 +26,31 @@
 #define NUM_SERVOS           (1)
 #define NUM_TOOLS            (1)
 
-#define MAX_FEEDRATE         (20000.0)  // depends on timer interrupt & hardware
-#define MIN_FEEDRATE         (100)
-#define DEFAULT_FEEDRATE     (9000.0)
+#define MAX_FEEDRATE         (100.0)  // depends on timer interrupt & hardware
+#define MIN_FEEDRATE         (0.0)
+#define DEFAULT_FEEDRATE     (90.0)
 
-#define MAX_ACCELERATION     (5000)
-#define MIN_ACCELERATION     (100)
-#define DEFAULT_ACCELERATION (2500)
+#define MAX_ACCELERATION     (500.0)
+#define MIN_ACCELERATION     (0.0)
+#define DEFAULT_ACCELERATION (180.0)
 
-#define MAX_JERK             (5.0)
+#define MAX_JERK             (10.0)
 
 
 #if MACHINE_HARDWARE_VERSION == 5
-#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
+#define USE_LIMIT_SWITCH
+#define HAS_SD
+#define HAS_LCD
 #endif
 #if MACHINE_HARDWARE_VERSION == 3
-#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
+#define HAS_SD
+#define HAS_LCD
 #endif
 
 
+extern void calibrateBelts();
+extern void recordHome();
+
 
 #endif  // #ifdef POLARGRAPH
 
diff --git a/robot_skycam.ino b/robot_skycam.cpp
similarity index 90%
rename from robot_skycam.ino
rename to robot_skycam.cpp
index 9f19816..d1409a0 100644
--- a/robot_skycam.ino
+++ b/robot_skycam.cpp
@@ -1,20 +1,20 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == SKYCAM
-
+#include "configure.h"
+#include "robot_skycam.h"
 
+#if MACHINE_STYLE == SKYCAM
 
 /**
  * Inverse Kinematics turns XY coordinates into lengths of belt from each motor
  * @param axies the cartesian coordinates
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *cartesian, long *motorStepArray) {
+void IK(const float *const cartesian, long *motorStepArray) {
   float x = cartesian[0];
   float y = cartesian[1];
   float z = cartesian[2];
diff --git a/robot_skycam.h b/robot_skycam.h
index cf50374..1ba52e7 100644
--- a/robot_skycam.h
+++ b/robot_skycam.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == SKYCAM
diff --git a/robot_stewart.cpp b/robot_stewart.cpp
new file mode 100644
index 0000000..a7a95ca
--- /dev/null
+++ b/robot_stewart.cpp
@@ -0,0 +1,427 @@
+//------------------------------------------------------------------------------
+// Makelangelo - firmware for various robot kinematic models
+// dan@marginallycelver.com 2013-12-26
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+//------------------------------------------------------------------------------
+
+#include "configure.h"
+#include "robot_stewart.h"
+
+#if MACHINE_STYLE == STEWART
+
+#include "Vector3.h"
+
+#define NUM_ARMS  (6)
+
+struct EndEffector  {
+  Vector3 up;
+  Vector3 left;
+  Vector3 forward;
+  Vector3 pos;
+  Vector3 relative;
+  float r, p, y; // roll, pitch, yaw
+};
+
+struct StewartArm {
+  Vector3 shoulder;
+  Vector3 elbow;
+  Vector3 shoulder_to_elbow;
+  Vector3 wrist;
+
+  float angle;
+};
+
+struct StewartPlatform {
+  StewartArm arms[NUM_ARMS];
+  EndEffector ee;
+};
+
+
+StewartPlatform robot;
+
+
+void stewartDemo() {
+  float pos[NUM_AXIES] = {0,0,0,0,0,0};
+  int i,j;
+
+  // linear moves
+  for(j=0;j<3;++j) {
+    for(i=0;i<5;++i) {
+      pos[j]=2;
+      lineSafe(pos,feed_rate);
+      pos[j]=-2;
+      lineSafe(pos,feed_rate);
+    }
+    pos[j]=0;
+    lineSafe(pos,feed_rate);
+  }
+  
+  // tilting
+  for(j=4;j<6;++j) {
+    for(i=0;i<5;++i) {
+      pos[j]=10;
+      lineSafe(pos,feed_rate);
+      pos[j]=-10;
+      lineSafe(pos,feed_rate);
+    }
+    pos[j]=0;
+    lineSafe(pos,feed_rate);
+  }
+  
+  // combos
+  for(j=0;j<3;++j) {
+    for(i=0;i<360*5;i+=10) {
+      pos[ j     ]=2*sin(((float)i/180.0)*PI);
+      pos[(j+1)%3]=2*cos(((float)i/180.0)*PI);
+      lineSafe(pos,feed_rate);
+    }
+    pos[ j     ]=0;
+    pos[(j+1)%3]=0;
+    lineSafe(pos,feed_rate);
+  }
+
+  // combos rotation
+  for(j=4;j<6;++j) {
+    for(i=0;i<360*5;i+=10) {
+      pos[    j        ]=10.0*sin(((float)i/180.0)*PI);
+      pos[3+((j-3+1)%3)]=10.0*cos(((float)i/180.0)*PI);
+      lineSafe(pos,feed_rate);
+    }
+    pos[    j        ]=0;
+    pos[3+((j-3+1)%3)]=0;
+    lineSafe(pos,feed_rate);
+  }
+
+  // combos random
+  for(j=0;j<50;++j) {
+    pos[0]=2*cos(((float)random(360)/180.0)*PI);
+    pos[1]=2*cos(((float)random(360)/180.0)*PI);
+    pos[2]=1.5*cos(((float)random(360)/180.0)*PI);
+    pos[3]=10.0*cos(((float)random(360)/180.0)*PI);
+    pos[4]=10.0*cos(((float)random(360)/180.0)*PI);
+    pos[5]=10.0*cos(((float)random(360)/180.0)*PI);
+    lineSafe(pos,feed_rate);
+  }
+  for(i=0;i<NUM_AXIES;++i) {
+    pos[i]=0;
+  }
+  lineSafe(pos,feed_rate);
+}
+
+/**
+   Update the end effector according to the desired motion
+   @input mov final end effector position
+   @input rpy final end effector roll pitch yaw (relative to base)
+*/
+void stewart_update_endeffector(Vector3 &mov, Vector3 &rpy) {
+  // translation
+  robot.ee.pos = mov;
+
+  // roll pitch & yaw
+  robot.ee.r = radians(rpy.x);
+  robot.ee.p = radians(rpy.y);
+  robot.ee.y = radians(rpy.z);
+  robot.ee.up.set(0, 0, 1);
+  robot.ee.forward.set(1, 0, 0);
+  robot.ee.left.set(0, 1, 0);
+
+  // roll
+  Vector3 axis;
+  axis.set(1, 0, 0);
+  robot.ee.up.rotate(axis, robot.ee.r);
+  robot.ee.forward.rotate(axis, robot.ee.r);
+  robot.ee.left.rotate(axis, robot.ee.r);
+
+  // pitch
+  axis.set(0, 1, 0);
+  robot.ee.up.rotate(axis, robot.ee.p);
+  robot.ee.forward.rotate(axis, robot.ee.p);
+  robot.ee.left.rotate(axis, robot.ee.p);
+
+  // yaw
+  axis.set(0, 0, 1);
+  robot.ee.up.rotate(axis, robot.ee.y);
+  robot.ee.forward.rotate(axis, robot.ee.y);
+  robot.ee.left.rotate(axis, robot.ee.y);
+}
+
+
+/**
+   Update the wrist positions according to the end effector
+*/
+void stewart_update_wrists() {
+  Vector3 n1, o1;
+  float c, s;
+  int i;
+  for (i = 0; i < NUM_ARMS / 2; ++i) {
+    StewartArm &arma = robot.arms[i * 2 + 0];
+    StewartArm &armb = robot.arms[i * 2 + 1];
+
+    c = cos(i * PI * 2.0f / 3.0f);
+    s = sin(i * PI * 2.0f / 3.0f);
+
+    n1 = robot.ee.forward * c + robot.ee.left * s;
+    o1 = robot.ee.forward * -s + robot.ee.left * c;
+
+    arma.wrist = robot.ee.pos + robot.ee.relative + n1 * T2W_X - o1 * T2W_Y + robot.ee.up * T2W_Z;
+    armb.wrist = robot.ee.pos + robot.ee.relative + n1 * T2W_X + o1 * T2W_Y + robot.ee.up * T2W_Z;
+  }
+
+#if VERBOSE > 0
+  for (i = 0; i < NUM_ARMS; ++i) {
+    StewartArm &arm = robot.arms[i];
+    Serial.print(i);
+    Serial.print("\twrist =");
+    Serial.print(arm.wrist.x);
+    Serial.print(F(","));
+    Serial.print(arm.wrist.y);
+    Serial.print(F(","));
+    Serial.println(arm.wrist.z);
+  }
+#endif
+}
+
+
+/**
+   update the elbow positions according to the wrists and shoulders, then find shoulder angle.
+*/
+void stewart_update_shoulder_angles() {
+  Vector3 ortho, w, wop, temp, r;
+  float a, b, d, r1, r0, hh, y, x;
+
+  int i;
+  for (i = 0; i < NUM_ARMS; ++i) {
+    StewartArm &arm = robot.arms[i];
+
+#if VERBOSE > 0
+    Serial.print(i);
+#endif
+
+    // project wrist position onto plane of bicep (wop)
+    ortho.x = cos((i / 2) * PI * 2.0f / 3.0f);
+    ortho.y = sin((i / 2) * PI * 2.0f / 3.0f);
+    ortho.z = 0;
+
+    w = arm.wrist - arm.shoulder;
+
+    a = w | ortho; //endeffector' distance
+    wop = w - (ortho * a);
+    //arm.wop.pos=wop + arm.shoulder;  // e' location
+    //vector_add(arm.wop,wop,arm.shoulder);
+
+    // because wop is projected onto the bicep plane, wop-elbow is not the same as wrist-elbow.
+    // we need to find wop-elbow to calculate the angle at the shoulder.
+    b = sqrt(FOREARM_LENGTH * FOREARM_LENGTH - a * a); // e'j distance
+
+    // use intersection of circles to find elbow point.
+    //a = (r0r0 - r1r1 + d*d ) / (2 d)
+    r1 = b; // circle 1 centers on e'
+    r0 = BICEP_LENGTH; // circle 0 centers on shoulder
+    d = wop.Length();
+    // distance from shoulder to the midpoint between the two possible intersections
+    a = ( r0 * r0 - r1 * r1 + d * d ) / ( 2 * d );
+
+#if VERBOSE > 0
+    Serial.print("\tb =");
+    Serial.println(b);
+    Serial.print("\td =");
+    Serial.println(d);
+    Serial.print("\ta =");
+    Serial.println(a);
+#endif
+
+    // normalize wop
+    wop /= d;
+    // find the midpoint
+    temp = arm.shoulder + (wop * a);
+    // with a and r0 we can find h, the distance from midpoint to intersections.
+    hh = sqrt(r0 * r0 - a * a);
+    // get a normal to the line wop in the plane orthogonal to ortho
+    r = ortho ^ wop;
+    if (i % 2 == 0) arm.elbow = temp + r * hh;
+    else       arm.elbow = temp - r * hh;
+
+    // use atan2 to find theta
+    temp = arm.elbow - arm.shoulder;
+    y = -temp.z;
+    temp.z = 0;
+    x = temp.Length();
+    if ( ( arm.shoulder_to_elbow | temp ) < 0 ) x = -x;
+    arm.angle = degrees(atan2(-y, x));
+#if VERBOSE > 0
+    Serial.print(i);
+    Serial.print("\tangle =");
+    Serial.println(arm.angle);
+#endif
+  }
+}
+
+
+/**
+   Inverse Kinematics turns XY coordinates into step counts from each motor
+   @param axies the cartesian coordinate
+   @param motorStepArray a measure of each belt to that plotter position
+*/
+void IK(const float *const cartesian, long *motorStepArray) {
+  float x = cartesian[0];
+  float y = cartesian[1];
+  float z = cartesian[2];
+  float u = cartesian[3];
+  float v = cartesian[4];
+  float w = cartesian[5];
+
+  Vector3 mov(x, y, z);
+  Vector3 rpy(u, v, w);
+
+  stewart_update_endeffector(mov, rpy);
+  stewart_update_wrists();
+  stewart_update_shoulder_angles();
+
+  int i;
+  for (i = 0; i < NUM_ARMS; ++i) {
+    motorStepArray[i] = robot.arms[i].angle * MICROSTEP_PER_DEGREE;
+  }
+}
+
+
+/**
+   Forward Kinematics - turns step counts into XY coordinates
+   @param motorStepArray a measure of each belt to that plotter position
+   @param axies the resulting cartesian coordinate
+   @return 0 if no problem, 1 on failure.
+*/
+int FK(long *motorStepArray, float *axies) {
+  return 0;
+}
+
+
+/**
+   Build a virtual model of the stewart shoulders for calculating angles later.
+*/
+void stewart_build_shoulders() {
+  Vector3 n, o, n1, o1;
+  float c, s;
+  int i;
+  for (i = 0; i < 3; ++i) {
+    StewartArm &arma = robot.arms[i * 2 + 0];
+    StewartArm &armb = robot.arms[i * 2 + 1];
+
+    c = cos(i * PI * 2.0f / 3.0f);
+    s = sin(i * PI * 2.0f / 3.0f);
+
+    n = robot.ee.forward;
+    o = robot.ee.up ^ robot.ee.forward;
+    o.Normalize();
+
+    n1 = n * c + o * s;
+    o1 = n * -s + o * c;
+
+    arma.shoulder = n1 * B2S_X - o1 * B2S_Y + robot.ee.up * B2S_Z;
+    armb.shoulder = n1 * B2S_X + o1 * B2S_Y + robot.ee.up * B2S_Z;
+    arma.elbow = n1 * B2S_X - o1 * (B2S_Y + BICEP_LENGTH) + robot.ee.up * B2S_Z;
+    armb.elbow = n1 * B2S_X + o1 * (B2S_Y + BICEP_LENGTH) + robot.ee.up * B2S_Z;
+    arma.shoulder_to_elbow = -o1;
+    armb.shoulder_to_elbow = o1;
+  }
+
+#if VERBOSE > 0
+  for (i = 0; i < 6; ++i) {
+    StewartArm &arm = robot.arms[i];
+    Serial.print(i);
+    Serial.print("\ts =");
+    Serial.print(arm.shoulder.x);
+    Serial.print(F(","));
+    Serial.print(arm.shoulder.y);
+    Serial.print(F(","));
+    Serial.print(arm.shoulder.z);
+
+    Serial.print("\te =");
+    Serial.print(arm.elbow.x);
+    Serial.print(F(","));
+    Serial.print(arm.elbow.y);
+    Serial.print(F(","));
+    Serial.println(arm.elbow.z);
+  }
+#endif
+}
+
+
+void robot_setup() {
+}
+
+
+void robot_findHome() {
+  wait_for_empty_segment_buffer();
+  motor_engage();
+
+  findStepDelay();
+
+  Serial.println(F("Finding..."));
+
+  uint8_t i, hits;
+  // back up until all switches are hitG
+  do {
+    hits = 0;
+    // for each stepper,
+    for (i = 0; i < NUM_MOTORS; ++i) {
+      digitalWrite(motors[i].dir_pin, HIGH);
+      // if this switch hasn't been hit yet
+      if (digitalRead(motors[i].limit_switch_pin) == HIGH) {
+        // move "down"
+        //Serial.print('|');
+        digitalWrite(motors[i].step_pin, HIGH);
+        digitalWrite(motors[i].step_pin, LOW);
+      } else {
+        ++hits;
+        //Serial.print('*');
+      }
+    }
+    //Serial.println();
+    pause(step_delay);
+  } while (hits < NUM_MOTORS);
+
+  // The arms are 19.69 degrees from straight down when they hit the switcrobot.
+  // @TODO: This could be better customized in firmware.
+  uint32_t steps_to_zero = SWITCH_ANGLE * MICROSTEP_PER_DEGREE;
+  
+  Serial.println(F("Homing..."));
+#if VERBOSE > 0
+  Serial.print("steps=");
+  Serial.println(steps_to_zero);
+#endif
+  for (uint32_t j = 0; j < steps_to_zero; ++j) {
+    for (i = 0; i < NUM_MOTORS; ++i) {
+      digitalWrite(motors[i].dir_pin, LOW);
+      digitalWrite(motors[i].step_pin, HIGH);
+      digitalWrite(motors[i].step_pin, LOW);
+    }
+    //Serial.println(steps_to_zero-j,DEC);
+    pause(step_delay);
+  }
+
+  // set robot to home position
+
+  Vector3 zero(0, 0, 0);
+  stewart_update_endeffector(zero, zero);
+  stewart_build_shoulders();
+  stewart_update_wrists();
+
+  // find the starting height of the end effector at home position
+  // @TODO: project wrist-on-bicep to get more accurate distance
+  Vector3 el = robot.arms[0].elbow;
+  Vector3 wr = robot.arms[0].wrist;
+  float aa = (el.y - wr.y);
+  float cc = FOREARM_LENGTH;
+  float bb = sqrt((cc * cc) - (aa * aa));
+  aa = el.x - wr.x;
+  cc = bb;
+  bb = sqrt((cc * cc) - (aa * aa));
+  robot.ee.relative.set(0, 0, bb + B2S_Z - T2W_Z);
+
+  float zeros[6] = {0, 0, 0, 0, 0, 0};
+  teleport(zeros);
+}
+
+
+
+#endif
diff --git a/robot_stewart.h b/robot_stewart.h
index 3d09fe4..3a97b54 100644
--- a/robot_stewart.h
+++ b/robot_stewart.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == STEWART
@@ -12,11 +11,6 @@
 #define MACHINE_STYLE_NAME           "STEWART"
 #define MACHINE_HARDWARE_VERSION     5
 
-#define STEP_DELAY           (50)  // delay between steps, in milliseconds, when doing fixed tasks like homing
-
-#define MAX_ACCELERATION     (5000)
-#define MIN_ACCELERATION     (100)
-
 #define SUBDIVIDE_LINES
 #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?
@@ -30,16 +24,21 @@
 #define NUM_MOTORS           (6)
 #define NUM_SERVOS           (0)
 
-#define MAX_FEEDRATE         (9000.0)  // depends on timer interrupt & hardware
-#define MIN_FEEDRATE         (100)
+#define MAX_FEEDRATE         (2000.0)  // depends on timer interrupt & hardware
+#define MIN_FEEDRATE         (0)
+#define DEFAULT_FEEDRATE     (60.0)
+
+#define MAX_ACCELERATION     (1000)
+#define MIN_ACCELERATION     (0)
+#define DEFAULT_ACCELERATION (500)
+
 #define MAX_JERK             (5.0)
-#define DEFAULT_FEEDRATE     (7000.0)
-#define DEFAULT_ACCELERATION (2500)
 
+#define MAX_SEGMENTS         (16)
 
 #define BICEP_LENGTH         ( 5.000)
 #define FOREARM_LENGTH       (16.750)
-#define SWITCH_ANGLE         (18.690)
+#define SWITCH_ANGLE         (90-18.690)
 // top center to wrist hole (relative): X7.635 Y+/-0.553 Z0.87
 #define T2W_X                ( 7.635)
 #define T2W_Y                ( 0.553)
@@ -50,8 +49,9 @@
 #define B2S_Z                ( 6.618)
 
 
+extern void stewartDemo();
+
 #endif  // #ifdef STEWART
 
 
 #endif  // #ifndef ROBOT_STEWART_H
-
diff --git a/robot_stewart.ino b/robot_stewart.ino
deleted file mode 100644
index 63aa2f5..0000000
--- a/robot_stewart.ino
+++ /dev/null
@@ -1,346 +0,0 @@
-//------------------------------------------------------------------------------
-// Makelangelo - firmware for various robot kinematic models
-// dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
-//------------------------------------------------------------------------------
-
-#if MACHINE_STYLE == STEWART
-
-
-#define NUM_ARMS  (6)
-
-struct EndEffector  {
-  Vector3 up;
-  Vector3 left;
-  Vector3 forward;
-  Vector3 pos;
-  Vector3 relative;
-  float r,p,y;  // roll, pitch, yaw
-};
-
-struct StewartArm {
-  Vector3 shoulder;
-  Vector3 elbow;
-  Vector3 shoulder_to_elbow;
-  Vector3 wrist;
-
-  float angle;
-};
-
-struct StewartPlatform {
-  StewartArm arms[NUM_ARMS];
-  EndEffector ee;
-};
-
-
-StewartPlatform robot;
-
-
-/**
- * Inverse Kinematics turns XY coordinates into step counts from each motor
- * @param axies the cartesian coordinate
- * @param motorStepArray a measure of each belt to that plotter position
- */
-void IK(float *cartesian, long *motorStepArray) {
-  float x = cartesian[0];
-  float y = cartesian[1];
-  float z = cartesian[2];
-  float u = cartesian[3];
-  float v = cartesian[4];
-  float w = cartesian[5];
-
-  Vector3 mov(x,y,z);
-  Vector3 rpy(u,v,w);
-  
-  stewart_update_endeffector(mov,rpy);
-  stewart_update_wrists();
-  stewart_update_shoulder_angles();
-
-  int i;
-  for(i=0;i<NUM_ARMS;++i) {
-    motorStepArray[i] = robot.arms[i].angle * MICROSTEP_PER_DEGREE;
-  }
-}
-
-
-/**
- * Update the end effector according to the desired motion
- * @input mov final end effector position
- * @input rpy final end effector roll pitch yaw (relative to base)
- */
-void stewart_update_endeffector(Vector3 &mov,Vector3 &rpy) {
-  // translation
-  robot.ee.pos = mov;
-  
-  // roll pitch & yaw  
-  robot.ee.r=radians(rpy.x);
-  robot.ee.p=radians(rpy.y);
-  robot.ee.y=radians(rpy.z);
-  robot.ee.up.set(0,0,1);
-  robot.ee.forward.set(1,0,0);
-  robot.ee.left.set(0,1,0);
-
-  // roll
-  Vector3 axis;
-  axis.set(1,0,0);
-  robot.ee.up.rotate(axis,robot.ee.r);
-  robot.ee.forward.rotate(axis,robot.ee.r);
-  robot.ee.left.rotate(axis,robot.ee.r);
-
-  // pitch
-  axis.set(0,1,0);
-  robot.ee.up.rotate(axis,robot.ee.p);
-  robot.ee.forward.rotate(axis,robot.ee.p);
-  robot.ee.left.rotate(axis,robot.ee.p);
-
-  // yaw
-  axis.set(0,0,1);
-  robot.ee.up.rotate(axis,robot.ee.y);
-  robot.ee.forward.rotate(axis,robot.ee.y);
-  robot.ee.left.rotate(axis,robot.ee.y);
-}
-
-
-/**
- * Update the wrist positions according to the end effector
- */
-void stewart_update_wrists() {
-  Vector3 n1,o1;
-  float c,s;
-  int i;
-  for(i=0;i<NUM_ARMS/2;++i) {
-    StewartArm &arma=robot.arms[i*2+0];
-    StewartArm &armb=robot.arms[i*2+1];
-
-    c=cos(i*PI*2.0f/3.0f);
-    s=sin(i*PI*2.0f/3.0f);
-
-    n1 = robot.ee.forward* c + robot.ee.left*s;
-    o1 = robot.ee.forward*-s + robot.ee.left*c;
-
-    arma.wrist = robot.ee.pos + robot.ee.relative + n1*T2W_X - o1*T2W_Y + robot.ee.up*T2W_Z;
-    armb.wrist = robot.ee.pos + robot.ee.relative + n1*T2W_X + o1*T2W_Y + robot.ee.up*T2W_Z;
-  }
-  
-#if VERBOSE > 0
-  for(i=0;i<NUM_ARMS;++i) {
-    StewartArm &arm=robot.arms[i];
-    Serial.print(i);
-    Serial.print("\twrist =");
-    Serial.print(arm.wrist.x);
-    Serial.print(F(","));
-    Serial.print(arm.wrist.y);
-    Serial.print(F(","));
-    Serial.println(arm.wrist.z);
-  }
-#endif
-}
-
-
-/**
- * update the elbow positions according to the wrists and shoulders, then find shoulder angle.
- */
-void stewart_update_shoulder_angles() {
-  Vector3 ortho,w,wop,temp,r;
-  float a,b,d,r1,r0,hh,y,x;
-  
-  int i;
-  for(i=0;i<NUM_ARMS;++i) {
-    StewartArm &arm=robot.arms[i];
-    
-#if VERBOSE > 0
-    Serial.print(i);
-#endif
-
-    // project wrist position onto plane of bicep (wop)
-    ortho.x=cos((i/2)*PI*2.0f/3.0f);
-    ortho.y=sin((i/2)*PI*2.0f/3.0f);
-    ortho.z=0;
-    
-    w = arm.wrist - arm.shoulder;
-    
-    a=w | ortho;  //endeffector' distance
-    wop = w - (ortho * a);
-    //arm.wop.pos=wop + arm.shoulder;  // e' location
-    //vector_add(arm.wop,wop,arm.shoulder);
-
-    // because wop is projected onto the bicep plane, wop-elbow is not the same as wrist-elbow.
-    // we need to find wop-elbow to calculate the angle at the shoulder.
-    b=sqrt(FOREARM_LENGTH*FOREARM_LENGTH-a*a);  // e'j distance
-
-    // use intersection of circles to find elbow point.
-    //a = (r0r0 - r1r1 + d*d ) / (2 d) 
-    r1=b;  // circle 1 centers on e'
-    r0=BICEP_LENGTH;  // circle 0 centers on shoulder
-    d=wop.Length();
-    // distance from shoulder to the midpoint between the two possible intersections
-    a = ( r0 * r0 - r1 * r1 + d*d ) / ( 2*d );
-
-#if VERBOSE > 0
-    Serial.print("\tb =");
-    Serial.println(b);
-    Serial.print("\td =");
-    Serial.println(d);
-    Serial.print("\ta =");
-    Serial.println(a);
-#endif
-
-    // normalize wop    
-    wop /= d;
-    // find the midpoint
-    temp=arm.shoulder+(wop*a);
-    // with a and r0 we can find h, the distance from midpoint to intersections.
-    hh=sqrt(r0*r0-a*a);
-    // get a normal to the line wop in the plane orthogonal to ortho
-    r = ortho ^ wop;
-    if(i%2==0) arm.elbow=temp + r * hh;
-    else       arm.elbow=temp - r * hh;
-    
-    // use atan2 to find theta
-    temp=arm.elbow-arm.shoulder;
-    y=-temp.z;
-    temp.z=0;
-    x=temp.Length();
-    if( ( arm.shoulder_to_elbow | temp ) < 0 ) x=-x;
-    arm.angle = degrees(atan2(-y,x));
-#if VERBOSE > 0
-    Serial.print(i);
-    Serial.print("\tangle =");
-    Serial.println(arm.angle);
-#endif
-  }
-}
-
-
-/** 
- * Forward Kinematics - turns step counts into XY coordinates
- * @param motorStepArray a measure of each belt to that plotter position
- * @param axies the resulting cartesian coordinate
- * @return 0 if no problem, 1 on failure.
- */
-int FK(long *motorStepArray,float *axies) {
-  return 0;
-}
-
-
-void robot_findHome() {
-  motor_engage();
-  
-  char i,hits;
-  // back up until all switches are hit
-  do {
-    hits=0;
-    // for each stepper,
-    for(i=0;i<NUM_MOTORS;++i) {
-      // if this switch hasn't been hit yet
-      if(digitalRead(motors[i].limit_switch_pin) == HIGH) {
-        // move "down"
-        digitalWrite(motors[i].dir_pin,LOW);
-        digitalWrite(motors[i].step_pin,HIGH);
-        digitalWrite(motors[i].step_pin,LOW);
-      }
-    }
-    pause(STEP_DELAY);
-  } while(hits>0);
-
-  // The arms are 19.69 degrees from straight down when they hit the switcrobot.
-  // @TODO: This could be better customized in firmware.
-  long steps_to_zero = SWITCH_ANGLE * MICROSTEP_PER_DEGREE;
-  
-  Serial.println(F("Homing..."));
-#if VERBOSE > 0
-  Serial.print("steps=");
-  Serial.println(steps_to_zero);
-#endif
-  char keepGoing;
-  for(int j=0;i<steps_to_zero;++j) {
-    for(i=0;i<NUM_MOTORS;++i) {
-        digitalWrite(motors[i].dir_pin,HIGH);
-        digitalWrite(motors[i].step_pin,HIGH);
-        digitalWrite(motors[i].step_pin,LOW);
-    }
-    pause(STEP_DELAY);
-  }
-
-  // set robot to home position
-  
-  Vector3 zero(0,0,0);
-  stewart_update_endeffector(zero,zero);
-  stewart_build_shoulders();
-  stewart_update_wrists();
-
-  // find the starting height of the end effector at home position
-  // @TODO: project wrist-on-bicep to get more accurate distance
-  Vector3 el=robot.arms[0].elbow;
-  Vector3 wr=robot.arms[0].wrist;
-  float aa=(el.y-wr.y);
-  float cc=FOREARM_LENGTH;
-  float bb=sqrt((cc*cc)-(aa*aa));
-  aa=el.x-wr.x;
-  cc=bb;
-  bb=sqrt((cc*cc)-(aa*aa));
-  robot.ee.relative.set(0,0,bb+B2S_Z-T2W_Z);
-
-  float zeros[6] = {0,0,0,0,0,0};
-  teleport(zeros);
-}
-
-
-/**
- * Build a virtual model of the stewart shoulders for calculating angles later.
- */
-void stewart_build_shoulders() {
-  Vector3 n,o,n1,o1;
-  float c,s;
-  int i;
-  for(i=0;i<3;++i) {
-    StewartArm &arma=robot.arms[i*2+0];
-    StewartArm &armb=robot.arms[i*2+1];
-
-    c=cos(i*PI*2.0f/3.0f);
-    s=sin(i*PI*2.0f/3.0f);
-
-    n=robot.ee.forward;
-    o=robot.ee.up ^ robot.ee.forward;
-    o.Normalize();
-
-    n1 = n* c + o*s;
-    o1 = n*-s + o*c;
-
-    arma.shoulder = n1*B2S_X - o1*B2S_Y + robot.ee.up*B2S_Z;
-    armb.shoulder = n1*B2S_X + o1*B2S_Y + robot.ee.up*B2S_Z;
-    arma.elbow = n1*B2S_X - o1*(B2S_Y+BICEP_LENGTH) + robot.ee.up*B2S_Z;
-    armb.elbow = n1*B2S_X + o1*(B2S_Y+BICEP_LENGTH) + robot.ee.up*B2S_Z;
-    arma.shoulder_to_elbow=-o1;
-    armb.shoulder_to_elbow=o1;
-  }
-  
-#if VERBOSE > 0
-  for(i=0;i<6;++i) {
-    StewartArm &arm=robot.arms[i];
-    Serial.print(i);
-    Serial.print("\ts =");
-    Serial.print(arm.shoulder.x);
-    Serial.print(F(","));
-    Serial.print(arm.shoulder.y);
-    Serial.print(F(","));
-    Serial.print(arm.shoulder.z);
-
-    Serial.print("\te =");
-    Serial.print(arm.elbow.x);
-    Serial.print(F(","));
-    Serial.print(arm.elbow.y);
-    Serial.print(F(","));
-    Serial.println(arm.elbow.z);
-  }
-#endif
-}
-
-
-void robot_setup() {
-}
-
-
-
-#endif
diff --git a/robot_traditionalxy.ino b/robot_traditionalxy.cpp
similarity index 77%
rename from robot_traditionalxy.ino
rename to robot_traditionalxy.cpp
index 79f1a8e..2839067 100644
--- a/robot_traditionalxy.ino
+++ b/robot_traditionalxy.cpp
@@ -1,26 +1,28 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == TRADITIONALXY
+#include "configure.h"
+#include "robot_traditionalxy.h"
 
+#if MACHINE_STYLE == TRADITIONALXY
 
+#include <Arduino.h>
 /**
  * Inverse Kinematics turns XY coordinates into step counts from each motor
  * @param x cartesian coordinate
  * @param y cartesian coordinate
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *cartesian, long *motorStepArray) {
+void IK(const float *const cartesian, long *motorStepArray) {
   float x = cartesian[0];
   float y = cartesian[1];
   float z = cartesian[2];
   
-  motorStepArray[0] = lround((x) / THREAD_PER_STEP);
-  motorStepArray[1] = lround((y) / THREAD_PER_STEP);
+  motorStepArray[0] = lround(x / THREAD_PER_STEP);
+  motorStepArray[1] = lround(y / THREAD_PER_STEP);
 
   motorStepArray[NUM_MOTORS] = z;
 }
diff --git a/robot_traditionalxy.h b/robot_traditionalxy.h
index b124367..89158a1 100644
--- a/robot_traditionalxy.h
+++ b/robot_traditionalxy.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 #if MACHINE_STYLE == TRADITIONALXY
@@ -15,9 +14,6 @@
 
 #define STEP_DELAY           (50)  // delay between steps, in milliseconds, when doing fixed tasks like homing
 
-#define MAX_ACCELERATION     (5000)
-#define MIN_ACCELERATION     (100)
-
 // servo angles for pen control
 #define PEN_UP_ANGLE         (90)
 #define PEN_DOWN_ANGLE       (50)  // Some steppers don't like 0 degrees
@@ -27,15 +23,21 @@
 #define NUM_SERVOS           (1)
 #define NUM_TOOLS            (1)
 
-#define MAX_FEEDRATE         (9000.0)  // depends on timer interrupt & hardware
-#define MIN_FEEDRATE         (100)
+#define MAX_FEEDRATE         (1500.0)  // depends on timer interrupt & hardware
+#define MIN_FEEDRATE         (0.0)
+#define DEFAULT_FEEDRATE     (200.0)
+
+#define MAX_ACCELERATION     (2000.0)
+#define MIN_ACCELERATION     (0.0)
+#define DEFAULT_ACCELERATION (1000.0)
 #define MAX_JERK             (5.0)
-#define DEFAULT_FEEDRATE     (7000.0)
-#define DEFAULT_ACCELERATION (2500)
+
+#define SUBDIVIDE_LINES
+#define SEGMENT_PER_CM_LINE  (1)  // lines are subdivided.  How long are the divisions?
+#define SEGMENT_PER_CM_ARC   (3)  // Arcs are subdivided.  How long are the divisions?
 
 
 #endif  // #ifdef TRADITIONALXY
 
 
 #endif  // #ifndef ROBOT_TRADITIONALXY_H
-
diff --git a/robot_zarplotter.ino b/robot_zarplotter.cpp
similarity index 93%
rename from robot_zarplotter.ino
rename to robot_zarplotter.cpp
index fe17c74..a60ace7 100644
--- a/robot_zarplotter.ino
+++ b/robot_zarplotter.cpp
@@ -1,20 +1,20 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
-#if MACHINE_STYLE == ZARPLOTTER
-
+#include "configure.h"
+#include "robot_zarplotter.h"
 
+#if MACHINE_STYLE == ZARPLOTTER
 
 /**
  * Inverse Kinematics turns XY coordinates into lengths of belt from each motor
  * @param axies the cartesian coordinates
  * @param motorStepArray a measure of each belt to that plotter position
  */
-void IK(float *cartesian, long *motorStepArray) {
+void IK(const float *const cartesian, long *motorStepArray) {
   float limit_xmin = axies[0].limitMin;
   float limit_xmax = axies[0].limitMax;
   float limit_ymin = axies[1].limitMin;
diff --git a/robot_zarplotter.h b/robot_zarplotter.h
index a7bfda1..b3ac899 100644
--- a/robot_zarplotter.h
+++ b/robot_zarplotter.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
@@ -44,4 +43,3 @@
 
 
 #endif  // #ifndef ROBOT_ZARPLOTTER_H
-
diff --git a/sdcard.ino b/sdcard.cpp
similarity index 79%
rename from sdcard.ino
rename to sdcard.cpp
index e9b07ba..73d9f11 100644
--- a/sdcard.ino
+++ b/sdcard.cpp
@@ -1,13 +1,13 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 //------------------------------------------------------------------------------
 // INCLUDES
 //------------------------------------------------------------------------------
+#include "configure.h"
 #include "sdcard.h"
 #include <SD.h>
 
@@ -34,7 +34,7 @@ long sd_bytes_read;
 //------------------------------------------------------------------------------
 
 // initialize the SD card and print some info.
-void SD_init() {
+void SD_setup() {
 #ifdef HAS_SD
   pinMode(SDSS, OUTPUT);
   pinMode(SDCARDDETECT,INPUT);
@@ -86,8 +86,11 @@ void SD_check() {
     int c;
     while(sd_print_file.peek() != -1) {
       c=sd_print_file.read();
-      serialBuffer[sofar++]=c;
       sd_bytes_read++;
+      if(c=='\r') continue;
+      if(sofar<MAX_BUF) {
+        serialBuffer[sofar++]=c;
+      }/*
       if(c==';') {
         // eat to the end of the line
         while(sd_print_file.peek() != -1) {
@@ -95,15 +98,15 @@ void SD_check() {
           sd_bytes_read++;
           if(c=='\n' || c=='\r') break;
         }
-      }
-      if(c=='\n' || c=='\r') {
+      }*/
+      if(c=='\n') {
         // update the % visible on the LCD.
         sd_percent_complete = (float)sd_bytes_read * 100.0 / (float)sd_file_size;
 
         // end string
-        serialBuffer[sofar]=0;
+        serialBuffer[sofar-1]=0;
         // print for our benefit
-        Serial.println(serialBuffer);
+        //Serial.println(serialBuffer);
         // process command
         processCommand();
         // reset buffer for next line
@@ -114,6 +117,7 @@ void SD_check() {
     }
 
     if(sd_print_file.peek() == -1) {
+      Serial.println("EOF Drawing done.");
       sd_print_file.close();
       sd_printing_now=false;
       if(sd_inserted) {
@@ -127,7 +131,7 @@ void SD_check() {
 }
 
 
-void SD_StartPrintingFile(char *filename) {
+void SD_StartPrintingFile(const char *filename) {
 #ifdef HAS_SD
   sd_print_file=SD.open(filename);
   if(!sd_print_file) {
@@ -169,21 +173,3 @@ void SD_listFiles() {
   }
 #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/>.
- */
diff --git a/sdcard.h b/sdcard.h
index 4f730c3..409e7ce 100644
--- a/sdcard.h
+++ b/sdcard.h
@@ -3,8 +3,7 @@
 //------------------------------------------------------------------------------
 // Makelangelo - firmware for various robot kinematic models
 // dan@marginallycelver.com 2013-12-26
-// Copyright at end of file.  Please see
-// http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
+// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
 //------------------------------------------------------------------------------
 
 
@@ -20,5 +19,9 @@ extern char sd_printing_paused;
 extern float sd_percent_complete;
 #endif
 
+extern void SD_check();
+extern void SD_setup();
+extern void SD_listFiles();
+extern void SD_StartPrintingFile(const char *);
 
 #endif // SDCARD_H