diff --git a/MachineDefs.h b/MachineDefs.h
new file mode 100644
index 0000000..01572bd
--- /dev/null
+++ b/MachineDefs.h
@@ -0,0 +1,43 @@
+
+#define SERIAL_DEBUG
+
+#ifdef SERIAL_DEBUG
+#define SER_PRINT(X) Serial.print((X))
+#define SER_PRINT2(X,Y) Serial.print((X),(Y))
+#define SER_PRINTLN(X) Serial.println((X))
+#define SER_PRINTLN2(X,Y) Serial.println((X),(Y))
+#else
+#define SER_PRINT(X)
+#define SER_PRINT2(X,Y)
+#define SER_PRINTLN(X)
+#define SER_PRINTLN2(X,Y)
+#endif
+
+//define this if plotter has a battery voltage feedback connected
+//#define HAS_BATTERY_MEASUREMENT 
+ 
+//which remote control to use
+//#define PIONEER_DVD_REMOTE
+#define NONAME_WHITE_REMOTE
+
+//servo pin
+#define SERVO_PIN A5
+
+//servo values for pen up/down
+#define PEN_UP 80
+#define PEN_DOWN 60
+
+
+//IR-receiver pin
+#define RECV_PIN  A0
+ 
+//stepper motor pins
+#define LEFT_STEP_PIN_1 6
+#define LEFT_STEP_PIN_2 7
+#define LEFT_STEP_PIN_3 8
+#define LEFT_STEP_PIN_4 9
+#define RIGHT_STEP_PIN_1 2
+#define RIGHT_STEP_PIN_2 3
+#define RIGHT_STEP_PIN_3 4
+#define RIGHT_STEP_PIN_4 5
+
diff --git a/data/MachineDefs.h b/data/MachineDefs.h
new file mode 100644
index 0000000..01572bd
--- /dev/null
+++ b/data/MachineDefs.h
@@ -0,0 +1,43 @@
+
+#define SERIAL_DEBUG
+
+#ifdef SERIAL_DEBUG
+#define SER_PRINT(X) Serial.print((X))
+#define SER_PRINT2(X,Y) Serial.print((X),(Y))
+#define SER_PRINTLN(X) Serial.println((X))
+#define SER_PRINTLN2(X,Y) Serial.println((X),(Y))
+#else
+#define SER_PRINT(X)
+#define SER_PRINT2(X,Y)
+#define SER_PRINTLN(X)
+#define SER_PRINTLN2(X,Y)
+#endif
+
+//define this if plotter has a battery voltage feedback connected
+//#define HAS_BATTERY_MEASUREMENT 
+ 
+//which remote control to use
+//#define PIONEER_DVD_REMOTE
+#define NONAME_WHITE_REMOTE
+
+//servo pin
+#define SERVO_PIN A5
+
+//servo values for pen up/down
+#define PEN_UP 80
+#define PEN_DOWN 60
+
+
+//IR-receiver pin
+#define RECV_PIN  A0
+ 
+//stepper motor pins
+#define LEFT_STEP_PIN_1 6
+#define LEFT_STEP_PIN_2 7
+#define LEFT_STEP_PIN_3 8
+#define LEFT_STEP_PIN_4 9
+#define RIGHT_STEP_PIN_1 2
+#define RIGHT_STEP_PIN_2 3
+#define RIGHT_STEP_PIN_3 4
+#define RIGHT_STEP_PIN_4 5
+
diff --git a/data/data.ino b/data/data.ino
new file mode 100644
index 0000000..93f9e30
--- /dev/null
+++ b/data/data.ino
@@ -0,0 +1,371 @@
+#include "MachineDefs.h"
+ 
+#ifndef USE_DATA_FROM_DISK
+
+#include <avr/pgmspace.h>
+#include <SPI.h>
+#include <SD.h>
+
+#endif
+
+bool getDataInternal(int plotNo, int point, float *x, float* y, int* pen);
+bool getSvgData(int plotNo, int point, float *x, float* y, int* pen);
+bool getMemoryData(int plotNo, int point, int *x, int* y, int* pen);
+
+static int currentlySelectedPlot = -1;
+
+static File svgFile;
+
+static int reachedTheEndAt = -10;
+
+bool getData(int plotNo, int point, float *x, float* y, int* pen)
+{
+  if(point == (reachedTheEndAt+1)) {
+    reachedTheEndAt = -10; 
+    return false;
+  }
+  else {
+    if(!getDataInternal(plotNo, point, x, y, pen)) {
+      //return to origo on end
+      *x = *y = 0.0;
+      *pen = 0;
+      reachedTheEndAt = point; 
+    }    
+    return true;
+  }
+}
+
+bool getDataInternal(int plotNo, int point, float *x, float* y, int* pen)
+{  
+  if(currentlySelectedPlot != plotNo) {
+    //first call, check if we have plot stored on SD
+    
+    if(svgFile) {
+      svgFile.close();
+      svgFile = SD.open("dummy_fail_this_open", FILE_READ);;      
+    }
+    
+    char* svgName = (char*)"1.svg";
+    svgName[0] = '0'+plotNo;    
+    svgFile = SD.open(svgName, FILE_READ);
+    if(svgFile) {
+        //found svg
+      SER_PRINT("Found: ");      
+      SER_PRINTLN(svgName);      
+    }
+    else {
+      makePenNoise(3);
+      SER_PRINT("No such file: ");      
+      SER_PRINTLN(svgName);      
+      return false;
+    }
+
+    currentlySelectedPlot = plotNo;
+  }
+  
+  if(svgFile) {
+    return getSvgData(plotNo, point, x, y, pen);    
+  }
+  else
+  {
+    return false;
+  }
+}
+
+#ifdef HAS_BATTERY_MEASUREMENT
+
+static File batteryFile;
+unsigned long lastBatteryLog = 0;
+
+static float batteryAverage=800; //just start with something above threshold
+#define BATTERY_THRESHOLD 650
+
+void logBattery(int secsSinceStart) {
+#ifndef USE_DATA_FROM_DISK  
+  int batt = analogRead(0);
+  batteryFile.print(secsSinceStart);
+  batteryFile.print(' ');
+  batteryFile.println(batt);
+  batteryFile.flush();
+  
+  batteryAverage = 0.99*batteryAverage + 0.01*batt;
+  if(batteryAverage < BATTERY_THRESHOLD) {
+    batteryFile.println("LOW BATTERY!");
+    batteryFile.flush();
+    stopPressed = true; //stop plot and persist state to eeprom to allow resume after battery has been changed
+  }  
+#endif
+}
+
+#endif //HAS_BATTERY_MEASUREMENT
+
+void setupData()
+{
+  // make sure that the default chip select pin is set to
+  // output, even if you don't use it:
+  pinMode(10, OUTPUT);
+
+  // see if the card is present and can be initialized:
+  if (!SD.begin(10)) {
+    SER_PRINTLN("SD fail");
+    // don't do anything more:
+    return;
+  }  
+#ifdef HAS_BATTERY_MEASUREMENT  
+#ifndef USE_DATA_FROM_DISK
+  batteryFile = SD.open("battery.log", FILE_WRITE);;      
+#endif
+#endif //HAS_BATTERY_MEASUREMENT
+}
+
+// **************** Svg path ***************************
+static int lastReadPoint = -1;
+
+bool
+seekTo(char* pattern)
+{
+  //yes, this is flawed string matching
+  char* tmp = pattern;
+
+  while(*tmp != '\0') {
+      char c = svgFile.read();
+      if(c < 0) {
+         return false;
+      }
+      if(c == *tmp) {
+        tmp++;
+      }
+      else {
+        tmp = pattern;        
+      }      
+  }
+  return true;  
+}
+
+bool
+seekToPathStart(bool rewindFile) {
+  if(rewindFile) {
+    svgFile.seek(0);  //rewind (we could have paused)
+  }
+    if(!seekTo((char*)"<path")) {
+//      SER_PRINTLN("No <path> tag");    
+      return false;
+    }
+    
+    if(!seekTo((char*)" d=\"")) {
+      SER_PRINTLN("No d=\" in path");    
+      return false;
+    }
+  return true;  
+}
+
+//If didn't have program space for using atof
+bool
+readFloat(float* ret) {
+  char tmp[20];
+  float f=0;
+  bool pastPoint=false;
+  bool exp=0;
+  float div = 1;
+  
+  for(int i=0 ; i<20 ; i++) {
+    tmp[i] = svgFile.read();
+    if(tmp[i]<0) {
+      return false;
+    } 
+    else if((tmp[i] >= '0') && (tmp[i] <= '9')) {
+      if(exp > 0) {
+	for (int e=(tmp[i]-'0') ; e > 0; e--) {
+	  div = div/10;
+	}
+      }
+      if(exp <  0) {
+	for (int e=(tmp[i]-'0') ; e > 0; e--) {
+	  div = div*10;
+	}
+      }
+      else if(div < 100) { 
+        f = f*10+(tmp[i]-'0');      
+        if(pastPoint) {
+           div = div*10;
+        }
+      }
+      else {
+        //only care for two decimals
+      }
+    }
+    else if(tmp[i] == '.') {
+      pastPoint=true;
+    }
+    else if(tmp[i] == '-') {
+      if(exp != 0) {
+	exp = -1;
+      }
+      else {
+	div = -1;
+      }
+    }
+    else if(tmp[i] == 'e') {
+      exp = 1;
+    }
+    else {
+      break;
+    }   
+  }  
+  *ret = f/div;
+  
+  return true;
+}
+
+bool
+getNextPathSegment(float *x, float *y, int *line, bool first)
+{
+  char c;
+  bool rel = false;
+  static float lastX, lastY;
+
+  while(true) {
+    if(first) {
+      lastX=lastY=0.0;
+    }
+
+    c = svgFile.read();
+    //    fputc(c,stdout);
+    if(c == 'M') {
+      *line = 0;     
+    }
+    else if(c=='m') {
+      *line = 0;     
+      svgFile.read(); //eat a space byte
+      rel = true;
+    }
+    else if(c == 'L') {
+      *line = 1;
+    }
+    else if(c==' ' || c=='l') {
+      *line = 1;
+      rel = true;
+      continue;
+    }
+    else if(((c >= '0') && (c <= '9')) || c=='-') {
+      svgFile.seek(svgFile.position()-1);       
+    }
+    else {
+      //reached end, look for another <path
+      if(seekToPathStart(false)) {
+	first = true;
+	continue;
+      }
+      else {
+	return false;
+      }
+    }      
+    break;
+  }
+  
+  if(!readFloat(x)) {
+    return false;    
+  } 
+  if(!readFloat(y)) {
+    return false;    
+  }
+
+  if(rel) {
+    *x += lastX;
+    *y += lastY;
+  }
+  lastX = *x;
+  lastY = *y;
+ 
+  //rewind one byte that was eaten by last float read  
+  svgFile.seek(svgFile.position()-1); 
+    
+  return true; 
+}
+
+static float min_x = 100000000.0;
+static float min_y = 100000000.0;
+static float scaleFactor = 1.0;
+
+static float lastX,lastY;
+static int lastPen;
+
+bool getSvgData(int plotNo, int point, float *x, float* y, int* pen)
+{
+  if(point == 0) {
+    long pathPosition;
+    long segments = 0;
+    float max_x, max_y;
+    
+    lastReadPoint = -1;
+    
+    //first read, get dimensions
+    if(!seekToPathStart(true)) {      
+      SER_PRINTLN("No path found!");    
+      return false;
+    }    
+    SER_PRINTLN("Found <path>!");    
+    pathPosition = svgFile.position();
+
+    min_y = min_x = 100000000.0;
+    max_y = max_x = -100000000.0;
+
+    while(true) {
+      if(getNextPathSegment(x, y, pen,segments==0)) {
+        segments++;        
+        min_x = min(min_x, *x);
+        max_x = max(max_x, *x);
+        min_y = min(min_y, *y);
+        max_y = max(max_y, *y);
+      }
+      else {
+        break;
+      }
+    }
+    scaleFactor = (disparity*0.4) / (max_x-min_x); //fill 40% of disparity as default
+    
+    SER_PRINT("Segments=");    
+    SER_PRINTLN(segments);    
+
+    SER_PRINT("Scale=");    
+    SER_PRINTLN(scaleFactor);    
+
+#ifdef USE_MOCKED_STEPPERS
+      fprintf(stderr,"segments=%3ld scale=%2.2f x=[%2.2f , %2.2f] y=[%2.2f , %2.2f] disparity=%ld\n", segments, scaleFactor, min_x,max_x, min_y, max_y, disparity);
+#endif        
+
+    svgFile.seek(pathPosition);    
+  }
+
+  if(point != lastReadPoint) {
+
+    lastReadPoint = point;
+
+    if(getNextPathSegment(x, y, pen,point==0)) {
+      *x = (*x-min_x)*scaleFactor;   
+      *y = (*y-min_y)*scaleFactor;
+   
+      lastX = *x;
+      lastY = *y;
+      lastPen = *pen;   
+    }
+    else {
+      lastX = 0;
+      lastY = 0;
+      lastPen = 0;
+      // rewind the file:
+      svgFile.seek(0);    
+      
+      return false;
+    }    
+  }
+  else {
+    *x = lastX;    
+    *y = lastY;    
+    *pen = lastPen;    
+  }
+
+  SER_PRINT(*pen ? "L " : "M ");    
+
+  return true;
+}
diff --git a/data/ir_control.ino b/data/ir_control.ino
new file mode 100644
index 0000000..de9680a
--- /dev/null
+++ b/data/ir_control.ino
@@ -0,0 +1,154 @@
+#include <IRremote.h>
+#include "MachineDefs.h"
+ 
+IRrecv irrecv(RECV_PIN);
+decode_results results;  
+
+#ifdef PIONEER_DVD_REMOTE
+/******************** PIONEER_DVD_REMOTE **********************/
+#define CODE_BIG_PRINT 39D41DC6    //up
+#define CODE_SMALL_PRINT 0xF50ACF30  //downd
+#define CODE_LEFT_REEL_IN E0984BB6   //step
+#define CODE_LEFT_REEL_OUT 0xf50aed12  //slow
+#define CODE_RIGHT_REEL_IN 0xc53ad926   //rev
+#define CODE_RIGHT_REEL_OUT 0xc53a59a6  //fwd
+#define CODE_LEFT_CALIBRATION A32AB931  //prev
+#define CODE_RIGHT_CALIBRATION 1D2FEFF6 //next
+#define CODE_ENABLE_CONT_DRIVE 371A3C86  //return
+#define CODE_DISABLE_CONT_DRIVE 0xf50af708 //enter
+#define CODE_STOP 0xC53A19E6 //pause
+#define CODE_RESUME 0xc53a7986 //play
+#define CODE_1 0xf50a857a
+#define CODE_2 0xf50a45ba
+#define CODE_3 0xf50ac53a
+#define CODE_4 0xF50A25DA
+#define CODE_5 0xF50AA55A
+#define CODE_6 0xF50A659A
+#define CODE_7 0xF50AE51A
+#define CODE_8 0xF50A15EA
+#define CODE_9 0xF50A956A
+#define CODE_0 0xF50A05FA
+
+#elif defined NONAME_WHITE_REMOTE
+/******************** NONAME_WHITE_REMOTE **********************/
+#define CODE_BIG_PRINT 0xff629d //+
+#define CODE_SMALL_PRINT 0xffe21d //-
+#define CODE_LEFT_REEL_IN 0xfff20d //left
+#define CODE_LEFT_REEL_OUT 0xffea15 //right
+#define CODE_RIGHT_REEL_IN 0xffca35 //up
+#define CODE_RIGHT_REEL_OUT 0xff18e7 //down
+#define CODE_LEFT_CALIBRATION 0xff32cd //title
+#define CODE_RIGHT_CALIBRATION 0xff0af5 //subtitle
+#define CODE_ENABLE_CONT_DRIVE 0xff10ef //open/close
+#define CODE_DISABLE_CONT_DRIVE 0xff00ff //standby
+#define CODE_STOP 0xff827d //setup
+#define CODE_RESUME 0xffa25d //center button of setup
+#define CODE_1 0xff807f
+#define CODE_2 0xffa05f
+#define CODE_3 0xff906f 
+#define CODE_4 0xff40bf
+#define CODE_5 0xff609f
+#define CODE_6 0xff50af 
+#define CODE_7 0xffc03f
+#define CODE_8 0xffe01f 
+#define CODE_9 0xffd02f
+
+#else
+#error What remote?
+#endif 
+
+
+void setupIR() 
+{
+  irrecv.enableIRIn(); // Start the receiver  
+}
+
+void readIR()
+{
+  float lDist;
+  bool fail = false;
+  
+  if (irrecv.decode(&results)) {         
+    switch(results.value) {
+#ifndef NO_REMOTE      
+       case 0xF50A3DC2:  //power
+         storePositionInEEPROM();
+               
+         break;         
+       case CODE_BIG_PRINT:  //up
+         printSize = 2;
+         break;
+       case CODE_SMALL_PRINT:  //down
+         printSize = 0.5;
+         break;
+       case CODE_LEFT_REEL_IN: //left -
+         manualLeft = -1;
+         break;
+       case CODE_LEFT_REEL_OUT: //left +
+         manualLeft = 1;
+         break;
+       case CODE_RIGHT_REEL_IN: //right -
+         manualRight = -1;
+         break;
+       case CODE_RIGHT_REEL_OUT: //right +
+         manualRight = 1;
+         break;
+       case CODE_LEFT_CALIBRATION:  //prev - calibrate 200 mm from left
+         currentLeftSteps = 200*stepsPerMM;
+         testPen();         
+         break;
+       case CODE_RIGHT_CALIBRATION:  //next - calibrate 200 mm from right
+         currentRightSteps = 200*stepsPerMM;              
+
+         lDist = currentLeftSteps/stepsPerMM;
+         disparity = (long)sqrt(lDist*lDist-200L*200L);
+
+         SER_PRINT("Cal: lDist)");
+         SER_PRINTLN(lDist);
+
+         SER_PRINT("Cal: Disp=");
+         SER_PRINTLN(disparity);
+
+         break;   
+       case CODE_DISABLE_CONT_DRIVE: 
+         continousManualDrive = false;
+         break;
+       case CODE_ENABLE_CONT_DRIVE: 
+         continousManualDrive = true;
+         break;
+       case CODE_STOP:  
+         stopPressed = true;
+#if  CODE_DISABLE_CONT_DRIVE == 0xBADC0DE
+        //just disable continous drive when pressing stop. Re-enable with CODE_ENABLE_CONT_DRIVE again
+         continousManualDrive = false;
+#endif
+         break;
+       case CODE_RESUME:
+          //resume print, or start new
+          program = 1; //start print
+          resumePlot = true;       
+         break;
+       case CODE_1: program = 1; currentPlot = 1; break;
+       case CODE_2: program = 1; currentPlot = 2; break;
+       case CODE_3: program = 1; currentPlot = 3; break;
+       case CODE_4: program = 1; currentPlot = 4; break;
+       case CODE_5: program = 1; currentPlot = 5; break;
+       case CODE_6: program = 1; currentPlot = 6; break;
+       case CODE_7: program = 1; currentPlot = 7; break;
+       case CODE_8: program = 1; currentPlot = 8; break;
+       case CODE_9: program = 1; currentPlot = 9; break;       
+       default:
+          fail=true;           
+#endif //NO_REMOTE         
+    }   
+    if(fail) {
+       SER_PRINT("???: ");
+    }
+    else {
+       makePenNoise(1);
+    }
+    SER_PRINTLN2(results.value, HEX);      
+    
+    irrecv.resume(); // Receive the next value
+  }  
+}
diff --git a/data/persistent_storage.ino b/data/persistent_storage.ino
new file mode 100644
index 0000000..6a09be1
--- /dev/null
+++ b/data/persistent_storage.ino
@@ -0,0 +1,27 @@
+#include <EEPROM.h>
+#include "MachineDefs.h"
+ 
+void eepromWriteLong(int addr, long data) {
+  for(int b=0 ; b<4 ; b++) {
+    EEPROM.write(addr+b, data & 0xff);
+    data = data >> 8;
+  }
+}
+
+void eepromWriteFloat(int addr, float data) {
+  long tmp = *(long*)&data;
+  eepromWriteLong(addr, tmp);
+}
+
+long eepromReadLong(int addr) {
+  long ret = 0;
+  for(int b=0 ; b<4 ; b++) {
+    ret = (ret<<8) | EEPROM.read(addr+(3-b));
+  }
+  return ret;
+}
+
+float eepromReadFloat(int addr) {
+  long tmp = eepromReadLong(addr);  
+  return *(float*)&tmp;
+}
diff --git a/data/plotter.ino b/data/plotter.ino
new file mode 100644
index 0000000..0f288d3
--- /dev/null
+++ b/data/plotter.ino
@@ -0,0 +1,261 @@
+#include "MachineDefs.h"
+ 
+//circumference of spool (2πr)mm
+#define spoolCirc 94.2 
+
+//steps per full rotation (number from https://github.com/robjampar/Stepper)
+#define stepsPerRotation 4075.7728395
+
+//number of steps for each full rotation
+#define stepsPerMM (stepsPerRotation/spoolCirc)
+
+//longest allowed line segment before splitting
+#define maxSegmentLength 10 
+
+#define EEPROM_LEFT_ADDR 0
+#define EEPROM_RIGHT_ADDR 4
+#define EEPROM_DISPARITY_ADDR 8
+#define EEPROM_STOPPED_AT_ADDR 12
+#define EEPROM_CURRENT_PLOT_ADDR 16
+#define EEPROM_PRINT_SIZE_ADDR 20
+#define EEPROM_CENTER_X_ADDR 24
+#define EEPROM_CENTER_Y_ADDR 28
+
+long state=0;
+long stoppedAt=0; //save of where we stopped, to allow resuming
+int currentPlot = 0;
+bool resumePlot = false;
+
+//setup was done by having exactly 1m of string reeled out on both left and right and a fix disparity of 1m between anchors. This is now set up by user instead.
+long disparity = 1000;  //distance between anchor points 
+long currentLeftSteps  = 1000*stepsPerMM; 
+long currentRightSteps = 1000*stepsPerMM;
+float centerX = 500; //starting x pos
+float centerY = 866; //starting y pos
+
+bool stopPressed = false;
+
+int program = 0; //0 is start program, responding to IR 
+
+//manual control
+int manualLeft = 0, manualRight = 0;
+float printSize = 1.0;
+bool continousManualDrive = true;
+
+//bookkeeping for sub segmenting lines
+static float prevX = 0;
+static float prevY = 0;
+static int currentSubSegment = 0;
+
+void storePositionInEEPROM();
+
+void setup()
+{  
+#ifdef USE_DATA_FROM_DISK
+  //store constant values into mock eeprom, wait 1sec first to allow storage
+  delayMicroseconds(1000000);  
+  storePositionInEEPROM();
+  printf("[disparity, currentLeftSteps, currentRightSteps, centerX, centerY]\n");
+  printf("%ld, %ld, %ld, %f, %f\n", disparity, currentLeftSteps, currentRightSteps, centerX, centerY);  
+#endif
+  
+#ifdef SERIAL_DEBUG
+  //Initialize serial and wait for port to open:
+    Serial.begin(9600); 
+    SER_PRINTLN("Setup");
+#endif
+  
+  //initialize IR  
+  setupIR();
+
+  //initialize steppers
+  setupStep();
+
+  //initialize servo
+  setupServo();
+  
+  //initialize SD card
+  setupData();
+  
+  //read stored position from EEPROM
+  currentLeftSteps = eepromReadLong(EEPROM_LEFT_ADDR);  
+  currentRightSteps = eepromReadLong(EEPROM_RIGHT_ADDR);  
+  disparity = eepromReadLong(EEPROM_DISPARITY_ADDR);  
+  stoppedAt = eepromReadLong(EEPROM_STOPPED_AT_ADDR);  
+  currentPlot = eepromReadLong(EEPROM_CURRENT_PLOT_ADDR);    
+  if(stoppedAt > 0) {
+    //preserve chosen size iff we stopped in the middle of a plot
+    printSize = eepromReadFloat(EEPROM_PRINT_SIZE_ADDR);        
+  }
+  centerX = eepromReadFloat(EEPROM_CENTER_X_ADDR);    
+  centerY = eepromReadFloat(EEPROM_CENTER_Y_ADDR);    
+  
+#ifdef SERIAL_DEBUG
+  Serial.print("Disparity=");
+  Serial.println(disparity);
+#endif
+
+#ifdef NO_REMOTE
+   //fake start a print since we dont have IR control
+   printSize = 1;
+   program = 1; //start print
+   currentPlot = 4; 
+#endif  
+}
+
+unsigned long lastEEPromStore = 0;
+
+void storePositionInEEPROM() {
+  unsigned long now = micros();
+  //dissallow storing to eeprom more than once a second (avoid repeated ir keystrokes)
+  if((lastEEPromStore+1000000) <= now) {
+
+    SER_PRINTLN("EEPROM: Store");
+
+    lastEEPromStore = now;
+    eepromWriteLong(EEPROM_LEFT_ADDR, currentLeftSteps);  
+    eepromWriteLong(EEPROM_RIGHT_ADDR, currentRightSteps);  
+    eepromWriteLong(EEPROM_DISPARITY_ADDR, disparity);  
+    eepromWriteLong(EEPROM_STOPPED_AT_ADDR, stoppedAt);  
+    eepromWriteLong(EEPROM_CURRENT_PLOT_ADDR, currentPlot);  
+    eepromWriteFloat(EEPROM_PRINT_SIZE_ADDR, printSize);  
+    eepromWriteFloat(EEPROM_CENTER_X_ADDR, centerX);  
+    eepromWriteFloat(EEPROM_CENTER_Y_ADDR, centerY);  
+  }
+}
+
+void setOrigo() {
+    float currentLeft  = currentLeftSteps / stepsPerMM;
+    float currentRight = currentRightSteps / stepsPerMM;
+    float tmp1 = (currentRight*currentRight-disparity*disparity-currentLeft*currentLeft);
+    float tmp2 = (-2*currentLeft*disparity);
+    float a = acos(tmp1/tmp2);    
+    centerX = currentLeft*cos(a);
+    centerY = currentLeft*sin(a);
+}
+
+static int prevPen=0;
+
+extern unsigned long lastBatteryLog;
+
+void loop()
+{        
+    float tmpX, tmpY;
+    int tmpPen;
+
+#ifdef HAS_BATTERY_MEASUREMENT    
+    unsigned long now = micros();
+    if((lastBatteryLog+1000000) <= now) {
+      lastBatteryLog = now;
+      logBattery(now/1000000);
+    }
+#endif    
+    readIR(); 
+
+    if(program == 0) {
+      float left = (manualLeft/spoolCirc) * 360.0;    
+      float right = (manualRight/spoolCirc) * 360.0;    
+      
+      if(manualLeft != 0 || manualRight != 0) {
+        currentLeftSteps += manualLeft*stepsPerMM;
+        currentRightSteps += manualRight*stepsPerMM;
+             
+        step(manualLeft*stepsPerMM,manualRight*stepsPerMM,false);
+        setOrigo();             
+      }
+
+      if(stopPressed || (!continousManualDrive)) {             
+        manualLeft = manualRight = 0;
+        stopPressed = false;
+      }      
+    }
+    else { 
+      if(!getData(currentPlot, state, &tmpX, &tmpY, &tmpPen) || stopPressed) {
+
+        stoppedAt = stopPressed ? state : 0;
+                
+        //reached the end, go back to manual mode
+        state = 0;
+        program = 0;
+        resumePlot = false; //make sure to not end up in loop if plot cannot be resumed due to missing file or corrupt data        
+
+        //stop with pen up        
+        movePen(false, false);
+    
+        step(0, 0, false); //flush out last line segment
+
+        SER_PRINTLN("Plot done");
+        
+        //store current position in eeprom 
+        storePositionInEEPROM();
+      }
+      else {
+         if(resumePlot && stoppedAt > state) {
+           //just skip points until we are at the point where we stopped
+             state++;
+             prevX = tmpX*printSize;
+             prevY = tmpY*printSize;
+         }
+         else {        
+          resumePlot = false;      
+          float nextX = tmpX*printSize;
+          float nextY = tmpY*printSize;
+          boolean nextPen = tmpPen > 0;
+          boolean advancePoint = true;
+  
+          if(state > 0) { //don't try to split first          
+            float dx = nextX-prevX;
+            float dy = nextY-prevY;
+            float len = sqrt(dx*dx+dy*dy);
+  
+            if(len > maxSegmentLength) {
+                //split segment
+                int subSegments = 1 + (int)(len/maxSegmentLength);
+                if(currentSubSegment == subSegments) {
+                    //last segment
+                    currentSubSegment = 0; //reset                                                                        
+                }
+                else {
+                    advancePoint = false; //stay on same point        
+                    currentSubSegment++;                  
+                    
+                    nextX = prevX + dx*currentSubSegment/subSegments;
+                    nextY = prevY + dy*currentSubSegment/subSegments;
+                }
+            }
+          }
+  
+          if(advancePoint) {
+            state = state+1; //next point
+            prevX = nextX;
+            prevY = nextY;                   
+          }
+                  
+          float xL = nextX+centerX;
+          float xR = nextX+centerX-disparity;
+          float y = nextY+centerY;
+  
+          long newLeft  = sqrt(xL*xL + y*y)*stepsPerMM;
+          long newRight = sqrt(xR*xR + y*y)*stepsPerMM;
+          
+          long dLeft  = (newLeft  - currentLeftSteps);            
+          long dRight = (newRight - currentRightSteps);
+                  
+          currentLeftSteps = newLeft;
+          currentRightSteps = newRight;
+    
+          if(((dLeft == 0) && (dRight == 0))) {
+            //no move, ignore
+          }
+          else {
+            movePen(prevPen, false); //adjust pen as necessary  
+            step(dLeft, dRight, prevPen != nextPen); //move steppers
+            prevPen=nextPen;
+          }
+      }
+    }      
+  } 
+
+  //check and disable steppers if idle
+  checkDisableSteppers();
+}
diff --git a/data/servo_control.ino b/data/servo_control.ino
new file mode 100644
index 0000000..9153d13
--- /dev/null
+++ b/data/servo_control.ino
@@ -0,0 +1,61 @@
+#include "MachineDefs.h"
+
+#include "MachineDefs.h"
+#include <Servo.h>  
+Servo myservo;
+
+//disable to preserve space
+#define USE_SMOOTH_SERVO
+
+static int oldPos = 0;
+
+void setupServo() 
+{
+  myservo.attach(SERVO_PIN);  
+  movePen(false, true);  
+  makePenNoise(2);
+}
+
+void makePenNoise(int n)
+{
+  int p = oldPos;  
+  for(int i=0; i<n; i++) {
+    myservo.write((int)p+5);    
+    delay(100); 
+    myservo.write((int)p);    
+    delay(100); 
+  }  
+}
+
+void testPen()
+{
+    movePen(true, false);
+    delay(1000); 
+    movePen(false, false);
+}
+
+void movePen(boolean down, boolean fast)
+{ 
+  int pos = down ? PEN_DOWN : PEN_UP;
+  if(pos != oldPos) {
+
+#ifdef USE_SMOOTH_SERVO    
+    if(fast || !down) {
+        myservo.write((int)pos);
+    }
+    else {
+      for(float i=0 ; i<=1.0 ; i+=0.02) {
+        float i2 = (1-cos(i*3.14))/2;
+        float tmpPos = (oldPos*(1.0-i2)+(pos*i2));
+        myservo.write((int)tmpPos);
+        delayMicroseconds(10000); 
+      }
+    }
+#else //USE_SMOOTH_SERVO
+    myservo.write((int)pos);
+#endif //USE_SMOOTH_SERVO
+
+    oldPos = pos;
+  }
+}
+
diff --git a/data/step_control.ino b/data/step_control.ino
new file mode 100644
index 0000000..c0678a6
--- /dev/null
+++ b/data/step_control.ino
@@ -0,0 +1,140 @@
+#include "MachineDefs.h"
+ 
+//current stepper position in step sequence (not global position)
+long currLeftPos = 0;
+long currRightPos = 0;
+
+byte stepSequence[8] = {B1000, B1100, B0100, B0110, B0010, B0011, B0001, B1001};
+//byte stepSequence[8] = {B1001, B1100, B1100, B0110, B0110, B0011, B0011, B1001};
+
+byte leftPins[4]  = {LEFT_STEP_PIN_1,  LEFT_STEP_PIN_2,  LEFT_STEP_PIN_3,  LEFT_STEP_PIN_4};
+byte rightPins[4] = {RIGHT_STEP_PIN_1, RIGHT_STEP_PIN_2, RIGHT_STEP_PIN_3, RIGHT_STEP_PIN_4};
+
+//shortest reliable delay seems to be around 600 (1220 used previously)
+#define MIN_DELAY 900
+#define MAX_DELAY 20000
+
+//Allowed speed in steps/us
+#define MAX_SPEED (1.0/MIN_DELAY)
+#define MIN_SPEED (1.0/MAX_DELAY)
+
+//Current speed in steps / ms
+float currentSpeed = MIN_SPEED; 
+
+//distance travelled before reaching full speed
+//#define FULL_SPEED_DIST (maxSegmentLength/2.0)
+#define FULL_SPEED_DIST 5.0
+
+//acceleration in speed / step (yes, weird unit).
+#define D_SPEED ((MAX_SPEED-MIN_SPEED) / (stepsPerMM*FULL_SPEED_DIST))
+
+unsigned int delayMicros = 1.0/MAX_SPEED;
+
+//disable steppers after half a second of inactivity
+#define DISABLE_TIMEOUT 500000
+unsigned long lastStepChange = 0;
+
+void setupStep() 
+{
+    for(int pin=0 ; pin<4 ; pin++) {
+      pinMode(leftPins[pin], OUTPUT);
+      pinMode(rightPins[pin], OUTPUT);
+    } 
+}
+
+static float prevDir;
+static long leftSteps=0;
+static long rightSteps=0;
+static bool accStop=true; //did we break the last segment
+
+#define ACC_DIR_THRESHOLD (PI/4)
+#define DIR_CHANGE_WAIT 1000
+
+void stepWithFraction(float *fraction, long *steps, long *currPos, byte *pins)
+{
+  if(*fraction >= 1.0) { //step
+     *fraction -= 1.0;        
+
+     if(*steps > 0) {
+       (*steps)--;
+       (*currPos)++;
+     }
+     else if(*steps < 0) {
+       (*steps)++;
+       (*currPos)--;
+     }
+     byte mask = stepSequence[*currPos & 0x7];
+
+     for(int bit=0 ; bit<4 ; bit++) {        
+       digitalWrite(pins[bit], mask & (B1000 >> bit));
+     }
+     
+     lastStepChange = micros();
+   }
+}
+
+void step(long nextLeftSteps, long nextRightSteps, boolean forceStop)
+{
+#ifdef USE_MOCKED_STEPPERS
+ //   printf("step %3ld %3ld\n", nextLeftSteps, nextRightSteps);
+#endif
+  
+  accStop = forceStop;
+
+  //number of steps for this line segment
+  long numSteps = max(abs(leftSteps), abs(rightSteps));
+
+  float nextDir = atan2(nextLeftSteps,nextRightSteps);  
+  float diff = abs(nextDir-prevDir);
+  if(diff > PI) {
+    diff = 2*PI-diff;
+  }
+
+  //check if we need to brake towards end of line segment
+  if(diff > ACC_DIR_THRESHOLD) {
+    //sharp turn, break
+    accStop = true;
+  } 
+  prevDir = nextDir;  
+    
+  if(numSteps > 0) {
+     //current logic is to step the fastest moving stepper every iteration of the loop while stepping the slower moving one as the "fraction" is accumulated to more than 1.
+     //A better logic calculating timings and running the steppers more asynch from one another might result in a more efficient drive of the slow steppper? In some distant future...
+     float leftPerStep = abs(leftSteps) / (float)numSteps;
+     float rightPerStep = abs(rightSteps) / (float)numSteps;     
+     float leftFraction = 0;
+     float rightFraction = 0;
+    
+     while(abs(leftSteps) > 0 || abs(rightSteps) > 0) {
+        leftFraction += leftPerStep;
+        stepWithFraction(&leftFraction, &leftSteps, &currLeftPos, leftPins);
+        
+        rightFraction += rightPerStep;
+        stepWithFraction(&rightFraction, &rightSteps, &currRightPos, rightPins);       
+       
+        numSteps = max(abs(leftSteps), abs(rightSteps));     
+        if(accStop && currentSpeed >= (numSteps*D_SPEED + MIN_SPEED)) {    
+          //start breaking
+          currentSpeed = max(currentSpeed-(currentSpeed/numSteps), MIN_SPEED);
+        }      
+        else {
+          currentSpeed = min(currentSpeed+D_SPEED, MAX_SPEED);
+        }  
+        
+        delayMicroseconds(1.0 / currentSpeed); 
+      }
+   }  
+   
+   leftSteps = nextLeftSteps;
+   rightSteps = nextRightSteps;
+}
+
+void checkDisableSteppers() { 
+   if((micros()-lastStepChange) > DISABLE_TIMEOUT) {     
+     //disable steppers
+     for(int pin=0 ; pin<4 ; pin++) {
+       digitalWrite(leftPins[pin], 0); 
+       digitalWrite(rightPins[pin], 0); 
+     }
+   }
+}
diff --git a/ir_control.ino b/ir_control.ino
new file mode 100644
index 0000000..de9680a
--- /dev/null
+++ b/ir_control.ino
@@ -0,0 +1,154 @@
+#include <IRremote.h>
+#include "MachineDefs.h"
+ 
+IRrecv irrecv(RECV_PIN);
+decode_results results;  
+
+#ifdef PIONEER_DVD_REMOTE
+/******************** PIONEER_DVD_REMOTE **********************/
+#define CODE_BIG_PRINT 39D41DC6    //up
+#define CODE_SMALL_PRINT 0xF50ACF30  //downd
+#define CODE_LEFT_REEL_IN E0984BB6   //step
+#define CODE_LEFT_REEL_OUT 0xf50aed12  //slow
+#define CODE_RIGHT_REEL_IN 0xc53ad926   //rev
+#define CODE_RIGHT_REEL_OUT 0xc53a59a6  //fwd
+#define CODE_LEFT_CALIBRATION A32AB931  //prev
+#define CODE_RIGHT_CALIBRATION 1D2FEFF6 //next
+#define CODE_ENABLE_CONT_DRIVE 371A3C86  //return
+#define CODE_DISABLE_CONT_DRIVE 0xf50af708 //enter
+#define CODE_STOP 0xC53A19E6 //pause
+#define CODE_RESUME 0xc53a7986 //play
+#define CODE_1 0xf50a857a
+#define CODE_2 0xf50a45ba
+#define CODE_3 0xf50ac53a
+#define CODE_4 0xF50A25DA
+#define CODE_5 0xF50AA55A
+#define CODE_6 0xF50A659A
+#define CODE_7 0xF50AE51A
+#define CODE_8 0xF50A15EA
+#define CODE_9 0xF50A956A
+#define CODE_0 0xF50A05FA
+
+#elif defined NONAME_WHITE_REMOTE
+/******************** NONAME_WHITE_REMOTE **********************/
+#define CODE_BIG_PRINT 0xff629d //+
+#define CODE_SMALL_PRINT 0xffe21d //-
+#define CODE_LEFT_REEL_IN 0xfff20d //left
+#define CODE_LEFT_REEL_OUT 0xffea15 //right
+#define CODE_RIGHT_REEL_IN 0xffca35 //up
+#define CODE_RIGHT_REEL_OUT 0xff18e7 //down
+#define CODE_LEFT_CALIBRATION 0xff32cd //title
+#define CODE_RIGHT_CALIBRATION 0xff0af5 //subtitle
+#define CODE_ENABLE_CONT_DRIVE 0xff10ef //open/close
+#define CODE_DISABLE_CONT_DRIVE 0xff00ff //standby
+#define CODE_STOP 0xff827d //setup
+#define CODE_RESUME 0xffa25d //center button of setup
+#define CODE_1 0xff807f
+#define CODE_2 0xffa05f
+#define CODE_3 0xff906f 
+#define CODE_4 0xff40bf
+#define CODE_5 0xff609f
+#define CODE_6 0xff50af 
+#define CODE_7 0xffc03f
+#define CODE_8 0xffe01f 
+#define CODE_9 0xffd02f
+
+#else
+#error What remote?
+#endif 
+
+
+void setupIR() 
+{
+  irrecv.enableIRIn(); // Start the receiver  
+}
+
+void readIR()
+{
+  float lDist;
+  bool fail = false;
+  
+  if (irrecv.decode(&results)) {         
+    switch(results.value) {
+#ifndef NO_REMOTE      
+       case 0xF50A3DC2:  //power
+         storePositionInEEPROM();
+               
+         break;         
+       case CODE_BIG_PRINT:  //up
+         printSize = 2;
+         break;
+       case CODE_SMALL_PRINT:  //down
+         printSize = 0.5;
+         break;
+       case CODE_LEFT_REEL_IN: //left -
+         manualLeft = -1;
+         break;
+       case CODE_LEFT_REEL_OUT: //left +
+         manualLeft = 1;
+         break;
+       case CODE_RIGHT_REEL_IN: //right -
+         manualRight = -1;
+         break;
+       case CODE_RIGHT_REEL_OUT: //right +
+         manualRight = 1;
+         break;
+       case CODE_LEFT_CALIBRATION:  //prev - calibrate 200 mm from left
+         currentLeftSteps = 200*stepsPerMM;
+         testPen();         
+         break;
+       case CODE_RIGHT_CALIBRATION:  //next - calibrate 200 mm from right
+         currentRightSteps = 200*stepsPerMM;              
+
+         lDist = currentLeftSteps/stepsPerMM;
+         disparity = (long)sqrt(lDist*lDist-200L*200L);
+
+         SER_PRINT("Cal: lDist)");
+         SER_PRINTLN(lDist);
+
+         SER_PRINT("Cal: Disp=");
+         SER_PRINTLN(disparity);
+
+         break;   
+       case CODE_DISABLE_CONT_DRIVE: 
+         continousManualDrive = false;
+         break;
+       case CODE_ENABLE_CONT_DRIVE: 
+         continousManualDrive = true;
+         break;
+       case CODE_STOP:  
+         stopPressed = true;
+#if  CODE_DISABLE_CONT_DRIVE == 0xBADC0DE
+        //just disable continous drive when pressing stop. Re-enable with CODE_ENABLE_CONT_DRIVE again
+         continousManualDrive = false;
+#endif
+         break;
+       case CODE_RESUME:
+          //resume print, or start new
+          program = 1; //start print
+          resumePlot = true;       
+         break;
+       case CODE_1: program = 1; currentPlot = 1; break;
+       case CODE_2: program = 1; currentPlot = 2; break;
+       case CODE_3: program = 1; currentPlot = 3; break;
+       case CODE_4: program = 1; currentPlot = 4; break;
+       case CODE_5: program = 1; currentPlot = 5; break;
+       case CODE_6: program = 1; currentPlot = 6; break;
+       case CODE_7: program = 1; currentPlot = 7; break;
+       case CODE_8: program = 1; currentPlot = 8; break;
+       case CODE_9: program = 1; currentPlot = 9; break;       
+       default:
+          fail=true;           
+#endif //NO_REMOTE         
+    }   
+    if(fail) {
+       SER_PRINT("???: ");
+    }
+    else {
+       makePenNoise(1);
+    }
+    SER_PRINTLN2(results.value, HEX);      
+    
+    irrecv.resume(); // Receive the next value
+  }  
+}
diff --git a/persistent_storage.ino b/persistent_storage.ino
new file mode 100644
index 0000000..6a09be1
--- /dev/null
+++ b/persistent_storage.ino
@@ -0,0 +1,27 @@
+#include <EEPROM.h>
+#include "MachineDefs.h"
+ 
+void eepromWriteLong(int addr, long data) {
+  for(int b=0 ; b<4 ; b++) {
+    EEPROM.write(addr+b, data & 0xff);
+    data = data >> 8;
+  }
+}
+
+void eepromWriteFloat(int addr, float data) {
+  long tmp = *(long*)&data;
+  eepromWriteLong(addr, tmp);
+}
+
+long eepromReadLong(int addr) {
+  long ret = 0;
+  for(int b=0 ; b<4 ; b++) {
+    ret = (ret<<8) | EEPROM.read(addr+(3-b));
+  }
+  return ret;
+}
+
+float eepromReadFloat(int addr) {
+  long tmp = eepromReadLong(addr);  
+  return *(float*)&tmp;
+}
diff --git a/plotter.ino b/plotter.ino
new file mode 100644
index 0000000..0f288d3
--- /dev/null
+++ b/plotter.ino
@@ -0,0 +1,261 @@
+#include "MachineDefs.h"
+ 
+//circumference of spool (2πr)mm
+#define spoolCirc 94.2 
+
+//steps per full rotation (number from https://github.com/robjampar/Stepper)
+#define stepsPerRotation 4075.7728395
+
+//number of steps for each full rotation
+#define stepsPerMM (stepsPerRotation/spoolCirc)
+
+//longest allowed line segment before splitting
+#define maxSegmentLength 10 
+
+#define EEPROM_LEFT_ADDR 0
+#define EEPROM_RIGHT_ADDR 4
+#define EEPROM_DISPARITY_ADDR 8
+#define EEPROM_STOPPED_AT_ADDR 12
+#define EEPROM_CURRENT_PLOT_ADDR 16
+#define EEPROM_PRINT_SIZE_ADDR 20
+#define EEPROM_CENTER_X_ADDR 24
+#define EEPROM_CENTER_Y_ADDR 28
+
+long state=0;
+long stoppedAt=0; //save of where we stopped, to allow resuming
+int currentPlot = 0;
+bool resumePlot = false;
+
+//setup was done by having exactly 1m of string reeled out on both left and right and a fix disparity of 1m between anchors. This is now set up by user instead.
+long disparity = 1000;  //distance between anchor points 
+long currentLeftSteps  = 1000*stepsPerMM; 
+long currentRightSteps = 1000*stepsPerMM;
+float centerX = 500; //starting x pos
+float centerY = 866; //starting y pos
+
+bool stopPressed = false;
+
+int program = 0; //0 is start program, responding to IR 
+
+//manual control
+int manualLeft = 0, manualRight = 0;
+float printSize = 1.0;
+bool continousManualDrive = true;
+
+//bookkeeping for sub segmenting lines
+static float prevX = 0;
+static float prevY = 0;
+static int currentSubSegment = 0;
+
+void storePositionInEEPROM();
+
+void setup()
+{  
+#ifdef USE_DATA_FROM_DISK
+  //store constant values into mock eeprom, wait 1sec first to allow storage
+  delayMicroseconds(1000000);  
+  storePositionInEEPROM();
+  printf("[disparity, currentLeftSteps, currentRightSteps, centerX, centerY]\n");
+  printf("%ld, %ld, %ld, %f, %f\n", disparity, currentLeftSteps, currentRightSteps, centerX, centerY);  
+#endif
+  
+#ifdef SERIAL_DEBUG
+  //Initialize serial and wait for port to open:
+    Serial.begin(9600); 
+    SER_PRINTLN("Setup");
+#endif
+  
+  //initialize IR  
+  setupIR();
+
+  //initialize steppers
+  setupStep();
+
+  //initialize servo
+  setupServo();
+  
+  //initialize SD card
+  setupData();
+  
+  //read stored position from EEPROM
+  currentLeftSteps = eepromReadLong(EEPROM_LEFT_ADDR);  
+  currentRightSteps = eepromReadLong(EEPROM_RIGHT_ADDR);  
+  disparity = eepromReadLong(EEPROM_DISPARITY_ADDR);  
+  stoppedAt = eepromReadLong(EEPROM_STOPPED_AT_ADDR);  
+  currentPlot = eepromReadLong(EEPROM_CURRENT_PLOT_ADDR);    
+  if(stoppedAt > 0) {
+    //preserve chosen size iff we stopped in the middle of a plot
+    printSize = eepromReadFloat(EEPROM_PRINT_SIZE_ADDR);        
+  }
+  centerX = eepromReadFloat(EEPROM_CENTER_X_ADDR);    
+  centerY = eepromReadFloat(EEPROM_CENTER_Y_ADDR);    
+  
+#ifdef SERIAL_DEBUG
+  Serial.print("Disparity=");
+  Serial.println(disparity);
+#endif
+
+#ifdef NO_REMOTE
+   //fake start a print since we dont have IR control
+   printSize = 1;
+   program = 1; //start print
+   currentPlot = 4; 
+#endif  
+}
+
+unsigned long lastEEPromStore = 0;
+
+void storePositionInEEPROM() {
+  unsigned long now = micros();
+  //dissallow storing to eeprom more than once a second (avoid repeated ir keystrokes)
+  if((lastEEPromStore+1000000) <= now) {
+
+    SER_PRINTLN("EEPROM: Store");
+
+    lastEEPromStore = now;
+    eepromWriteLong(EEPROM_LEFT_ADDR, currentLeftSteps);  
+    eepromWriteLong(EEPROM_RIGHT_ADDR, currentRightSteps);  
+    eepromWriteLong(EEPROM_DISPARITY_ADDR, disparity);  
+    eepromWriteLong(EEPROM_STOPPED_AT_ADDR, stoppedAt);  
+    eepromWriteLong(EEPROM_CURRENT_PLOT_ADDR, currentPlot);  
+    eepromWriteFloat(EEPROM_PRINT_SIZE_ADDR, printSize);  
+    eepromWriteFloat(EEPROM_CENTER_X_ADDR, centerX);  
+    eepromWriteFloat(EEPROM_CENTER_Y_ADDR, centerY);  
+  }
+}
+
+void setOrigo() {
+    float currentLeft  = currentLeftSteps / stepsPerMM;
+    float currentRight = currentRightSteps / stepsPerMM;
+    float tmp1 = (currentRight*currentRight-disparity*disparity-currentLeft*currentLeft);
+    float tmp2 = (-2*currentLeft*disparity);
+    float a = acos(tmp1/tmp2);    
+    centerX = currentLeft*cos(a);
+    centerY = currentLeft*sin(a);
+}
+
+static int prevPen=0;
+
+extern unsigned long lastBatteryLog;
+
+void loop()
+{        
+    float tmpX, tmpY;
+    int tmpPen;
+
+#ifdef HAS_BATTERY_MEASUREMENT    
+    unsigned long now = micros();
+    if((lastBatteryLog+1000000) <= now) {
+      lastBatteryLog = now;
+      logBattery(now/1000000);
+    }
+#endif    
+    readIR(); 
+
+    if(program == 0) {
+      float left = (manualLeft/spoolCirc) * 360.0;    
+      float right = (manualRight/spoolCirc) * 360.0;    
+      
+      if(manualLeft != 0 || manualRight != 0) {
+        currentLeftSteps += manualLeft*stepsPerMM;
+        currentRightSteps += manualRight*stepsPerMM;
+             
+        step(manualLeft*stepsPerMM,manualRight*stepsPerMM,false);
+        setOrigo();             
+      }
+
+      if(stopPressed || (!continousManualDrive)) {             
+        manualLeft = manualRight = 0;
+        stopPressed = false;
+      }      
+    }
+    else { 
+      if(!getData(currentPlot, state, &tmpX, &tmpY, &tmpPen) || stopPressed) {
+
+        stoppedAt = stopPressed ? state : 0;
+                
+        //reached the end, go back to manual mode
+        state = 0;
+        program = 0;
+        resumePlot = false; //make sure to not end up in loop if plot cannot be resumed due to missing file or corrupt data        
+
+        //stop with pen up        
+        movePen(false, false);
+    
+        step(0, 0, false); //flush out last line segment
+
+        SER_PRINTLN("Plot done");
+        
+        //store current position in eeprom 
+        storePositionInEEPROM();
+      }
+      else {
+         if(resumePlot && stoppedAt > state) {
+           //just skip points until we are at the point where we stopped
+             state++;
+             prevX = tmpX*printSize;
+             prevY = tmpY*printSize;
+         }
+         else {        
+          resumePlot = false;      
+          float nextX = tmpX*printSize;
+          float nextY = tmpY*printSize;
+          boolean nextPen = tmpPen > 0;
+          boolean advancePoint = true;
+  
+          if(state > 0) { //don't try to split first          
+            float dx = nextX-prevX;
+            float dy = nextY-prevY;
+            float len = sqrt(dx*dx+dy*dy);
+  
+            if(len > maxSegmentLength) {
+                //split segment
+                int subSegments = 1 + (int)(len/maxSegmentLength);
+                if(currentSubSegment == subSegments) {
+                    //last segment
+                    currentSubSegment = 0; //reset                                                                        
+                }
+                else {
+                    advancePoint = false; //stay on same point        
+                    currentSubSegment++;                  
+                    
+                    nextX = prevX + dx*currentSubSegment/subSegments;
+                    nextY = prevY + dy*currentSubSegment/subSegments;
+                }
+            }
+          }
+  
+          if(advancePoint) {
+            state = state+1; //next point
+            prevX = nextX;
+            prevY = nextY;                   
+          }
+                  
+          float xL = nextX+centerX;
+          float xR = nextX+centerX-disparity;
+          float y = nextY+centerY;
+  
+          long newLeft  = sqrt(xL*xL + y*y)*stepsPerMM;
+          long newRight = sqrt(xR*xR + y*y)*stepsPerMM;
+          
+          long dLeft  = (newLeft  - currentLeftSteps);            
+          long dRight = (newRight - currentRightSteps);
+                  
+          currentLeftSteps = newLeft;
+          currentRightSteps = newRight;
+    
+          if(((dLeft == 0) && (dRight == 0))) {
+            //no move, ignore
+          }
+          else {
+            movePen(prevPen, false); //adjust pen as necessary  
+            step(dLeft, dRight, prevPen != nextPen); //move steppers
+            prevPen=nextPen;
+          }
+      }
+    }      
+  } 
+
+  //check and disable steppers if idle
+  checkDisableSteppers();
+}
diff --git a/servo_control.ino b/servo_control.ino
new file mode 100644
index 0000000..9153d13
--- /dev/null
+++ b/servo_control.ino
@@ -0,0 +1,61 @@
+#include "MachineDefs.h"
+
+#include "MachineDefs.h"
+#include <Servo.h>  
+Servo myservo;
+
+//disable to preserve space
+#define USE_SMOOTH_SERVO
+
+static int oldPos = 0;
+
+void setupServo() 
+{
+  myservo.attach(SERVO_PIN);  
+  movePen(false, true);  
+  makePenNoise(2);
+}
+
+void makePenNoise(int n)
+{
+  int p = oldPos;  
+  for(int i=0; i<n; i++) {
+    myservo.write((int)p+5);    
+    delay(100); 
+    myservo.write((int)p);    
+    delay(100); 
+  }  
+}
+
+void testPen()
+{
+    movePen(true, false);
+    delay(1000); 
+    movePen(false, false);
+}
+
+void movePen(boolean down, boolean fast)
+{ 
+  int pos = down ? PEN_DOWN : PEN_UP;
+  if(pos != oldPos) {
+
+#ifdef USE_SMOOTH_SERVO    
+    if(fast || !down) {
+        myservo.write((int)pos);
+    }
+    else {
+      for(float i=0 ; i<=1.0 ; i+=0.02) {
+        float i2 = (1-cos(i*3.14))/2;
+        float tmpPos = (oldPos*(1.0-i2)+(pos*i2));
+        myservo.write((int)tmpPos);
+        delayMicroseconds(10000); 
+      }
+    }
+#else //USE_SMOOTH_SERVO
+    myservo.write((int)pos);
+#endif //USE_SMOOTH_SERVO
+
+    oldPos = pos;
+  }
+}
+
diff --git a/step_control.ino b/step_control.ino
new file mode 100644
index 0000000..c0678a6
--- /dev/null
+++ b/step_control.ino
@@ -0,0 +1,140 @@
+#include "MachineDefs.h"
+ 
+//current stepper position in step sequence (not global position)
+long currLeftPos = 0;
+long currRightPos = 0;
+
+byte stepSequence[8] = {B1000, B1100, B0100, B0110, B0010, B0011, B0001, B1001};
+//byte stepSequence[8] = {B1001, B1100, B1100, B0110, B0110, B0011, B0011, B1001};
+
+byte leftPins[4]  = {LEFT_STEP_PIN_1,  LEFT_STEP_PIN_2,  LEFT_STEP_PIN_3,  LEFT_STEP_PIN_4};
+byte rightPins[4] = {RIGHT_STEP_PIN_1, RIGHT_STEP_PIN_2, RIGHT_STEP_PIN_3, RIGHT_STEP_PIN_4};
+
+//shortest reliable delay seems to be around 600 (1220 used previously)
+#define MIN_DELAY 900
+#define MAX_DELAY 20000
+
+//Allowed speed in steps/us
+#define MAX_SPEED (1.0/MIN_DELAY)
+#define MIN_SPEED (1.0/MAX_DELAY)
+
+//Current speed in steps / ms
+float currentSpeed = MIN_SPEED; 
+
+//distance travelled before reaching full speed
+//#define FULL_SPEED_DIST (maxSegmentLength/2.0)
+#define FULL_SPEED_DIST 5.0
+
+//acceleration in speed / step (yes, weird unit).
+#define D_SPEED ((MAX_SPEED-MIN_SPEED) / (stepsPerMM*FULL_SPEED_DIST))
+
+unsigned int delayMicros = 1.0/MAX_SPEED;
+
+//disable steppers after half a second of inactivity
+#define DISABLE_TIMEOUT 500000
+unsigned long lastStepChange = 0;
+
+void setupStep() 
+{
+    for(int pin=0 ; pin<4 ; pin++) {
+      pinMode(leftPins[pin], OUTPUT);
+      pinMode(rightPins[pin], OUTPUT);
+    } 
+}
+
+static float prevDir;
+static long leftSteps=0;
+static long rightSteps=0;
+static bool accStop=true; //did we break the last segment
+
+#define ACC_DIR_THRESHOLD (PI/4)
+#define DIR_CHANGE_WAIT 1000
+
+void stepWithFraction(float *fraction, long *steps, long *currPos, byte *pins)
+{
+  if(*fraction >= 1.0) { //step
+     *fraction -= 1.0;        
+
+     if(*steps > 0) {
+       (*steps)--;
+       (*currPos)++;
+     }
+     else if(*steps < 0) {
+       (*steps)++;
+       (*currPos)--;
+     }
+     byte mask = stepSequence[*currPos & 0x7];
+
+     for(int bit=0 ; bit<4 ; bit++) {        
+       digitalWrite(pins[bit], mask & (B1000 >> bit));
+     }
+     
+     lastStepChange = micros();
+   }
+}
+
+void step(long nextLeftSteps, long nextRightSteps, boolean forceStop)
+{
+#ifdef USE_MOCKED_STEPPERS
+ //   printf("step %3ld %3ld\n", nextLeftSteps, nextRightSteps);
+#endif
+  
+  accStop = forceStop;
+
+  //number of steps for this line segment
+  long numSteps = max(abs(leftSteps), abs(rightSteps));
+
+  float nextDir = atan2(nextLeftSteps,nextRightSteps);  
+  float diff = abs(nextDir-prevDir);
+  if(diff > PI) {
+    diff = 2*PI-diff;
+  }
+
+  //check if we need to brake towards end of line segment
+  if(diff > ACC_DIR_THRESHOLD) {
+    //sharp turn, break
+    accStop = true;
+  } 
+  prevDir = nextDir;  
+    
+  if(numSteps > 0) {
+     //current logic is to step the fastest moving stepper every iteration of the loop while stepping the slower moving one as the "fraction" is accumulated to more than 1.
+     //A better logic calculating timings and running the steppers more asynch from one another might result in a more efficient drive of the slow steppper? In some distant future...
+     float leftPerStep = abs(leftSteps) / (float)numSteps;
+     float rightPerStep = abs(rightSteps) / (float)numSteps;     
+     float leftFraction = 0;
+     float rightFraction = 0;
+    
+     while(abs(leftSteps) > 0 || abs(rightSteps) > 0) {
+        leftFraction += leftPerStep;
+        stepWithFraction(&leftFraction, &leftSteps, &currLeftPos, leftPins);
+        
+        rightFraction += rightPerStep;
+        stepWithFraction(&rightFraction, &rightSteps, &currRightPos, rightPins);       
+       
+        numSteps = max(abs(leftSteps), abs(rightSteps));     
+        if(accStop && currentSpeed >= (numSteps*D_SPEED + MIN_SPEED)) {    
+          //start breaking
+          currentSpeed = max(currentSpeed-(currentSpeed/numSteps), MIN_SPEED);
+        }      
+        else {
+          currentSpeed = min(currentSpeed+D_SPEED, MAX_SPEED);
+        }  
+        
+        delayMicroseconds(1.0 / currentSpeed); 
+      }
+   }  
+   
+   leftSteps = nextLeftSteps;
+   rightSteps = nextRightSteps;
+}
+
+void checkDisableSteppers() { 
+   if((micros()-lastStepChange) > DISABLE_TIMEOUT) {     
+     //disable steppers
+     for(int pin=0 ; pin<4 ; pin++) {
+       digitalWrite(leftPins[pin], 0); 
+       digitalWrite(rightPins[pin], 0); 
+     }
+   }
+}