diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp
index 03d69b2..7cc1853 100644
--- a/src/F11Handler.cpp
+++ b/src/F11Handler.cpp
@@ -28,7 +28,7 @@ int F11Handler::execute(Command* command) {
 		Serial.print("R99 HOME X\n");
         }
 
-	StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, true, false, false);
+	StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false);
         if (LOGGING) {
                 CurrentState::getInstance()->print();
         }
diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp
index 644af93..71673c2 100644
--- a/src/F12Handler.cpp
+++ b/src/F12Handler.cpp
@@ -28,7 +28,7 @@ int F12Handler::execute(Command* command) {
                 Serial.print("R99 HOME Y\n");
         }
 
-        StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, false, true, false);
+        StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, true, false);
         if (LOGGING) {
                 CurrentState::getInstance()->print();
         }
diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp
index 5919b7a..8f69c17 100644
--- a/src/F13Handler.cpp
+++ b/src/F13Handler.cpp
@@ -28,7 +28,7 @@ int F13Handler::execute(Command* command) {
                 Serial.print("R99 HOME Z\n");
         }
 
-        StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, false, false, true);
+        StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, false, true);
         if (LOGGING) {
                 CurrentState::getInstance()->print();
         }
diff --git a/src/F14Handler.cpp b/src/F14Handler.cpp
index bee8694..15a3282 100644
--- a/src/F14Handler.cpp
+++ b/src/F14Handler.cpp
@@ -33,7 +33,7 @@ int F14Handler::execute(Command* command) {
         ret = StepperControl::getInstance()->calibrateAxis(0);
 
 	if (ret == 0) {
-		StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, true, false, false);
+		StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false);
 	}
 
         if (LOGGING) {
diff --git a/src/G00Handler.cpp b/src/G00Handler.cpp
index 0b3a474..816fd39 100644
--- a/src/G00Handler.cpp
+++ b/src/G00Handler.cpp
@@ -40,11 +40,11 @@ int G00Handler::execute(Command* command) {
 //	Serial.print("\n");
 
 
-	StepperControl::getInstance()->moveAbsoluteConstant(
+	StepperControl::getInstance()->moveToCoords(
 			command->getX(), command->getY(), command->getZ(),
-			//0,0,0,
 			command->getA(), command->getB(), command->getC(), 
 			false, false, false);
+
 	if (LOGGING) {
 		CurrentState::getInstance()->print();
 	}
diff --git a/src/G28Handler.cpp b/src/G28Handler.cpp
index 219e12c..37eec5b 100644
--- a/src/G28Handler.cpp
+++ b/src/G28Handler.cpp
@@ -25,8 +25,8 @@ int G28Handler::execute(Command* command) {
 
 Serial.print("home\n");
 
-	StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, false, false, true);
-	StepperControl::getInstance()->moveAbsoluteConstant(0,0,0, 0,0,0, true,  true,  false);
+	StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, false, false, true);
+	StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true,  true,  false);
 	//StepperControl::getInstance()->moveAbsoluteConstant(0,0,0,HOME_MOVEMENT_SPEED_S_P_S,true);
 	if (LOGGING) {
 		CurrentState::getInstance()->print();
diff --git a/src/ParameterList.h b/src/ParameterList.h
index 0a08fab..3b6d103 100644
--- a/src/ParameterList.h
+++ b/src/ParameterList.h
@@ -39,7 +39,11 @@ enum ParamListEnum
 
 	MOVEMENT_MAX_SPD_X		= 71,
 	MOVEMENT_MAX_SPD_Y		= 72,
-	MOVEMENT_MAX_SPD_Z		= 73
+	MOVEMENT_MAX_SPD_Z		= 73,
+
+	MOVEMENT_AXIS_NR_STEPS_X	= 801,
+	MOVEMENT_AXIS_NR_STEPS_Y	= 802,
+	MOVEMENT_AXIS_NR_STEPS_Z	= 803
 
 };
 
diff --git a/src/StepperControl.cpp b/src/StepperControl.cpp
index bb10c65..5e0d08e 100644
--- a/src/StepperControl.cpp
+++ b/src/StepperControl.cpp
@@ -13,612 +13,413 @@ StepperControl * StepperControl::getInstance() {
 
 const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds
 
-int test = 0;
-
-long sourcePoint[3]             = {0,0,0};
-long currentPoint[3]            = {0,0,0};
-long destinationPoint[3]        = {0,0,0};
-unsigned long movementLength[3] = {0,0,0};
-unsigned long maxLenth          = 0;
-unsigned long moveTicks[3]      = {0,0,0};
-unsigned long stepOnTick[3]     = {0,0,0};
-unsigned long stepOffTick[3]    = {0,0,0};
-unsigned long axisSpeed[3]      = {0,0,0};
-double lengthRatio[3]           = {0,0,0};
-long speedMax[3]                = {0,0,0};
-long speedMin[3]                = {0,0,0};
-long stepsAcc[3]                = {0,0,0};
-long timeOut[3]                 = {0,0,0};
-bool homeIsUp[3]                = {false,false,false};
-bool motorInv[3]                = {false,false,false};
-bool endStInv[3]                = {false,false,false};
-bool homeAxis[3]                = {false,false,false};
-bool axisActive[3]              = {false,false,false};
-bool movementUp[3]              = {false,false,false};
-bool movementToHome[3]          = {false,false,false};
-bool home                       = false;
-
-
-// ** test code
-
-// The interrupt will blink the LED, and keep
-// track of how many times it has blinked.
-int ledState = LOW;
-//volatile unsigned long blinkCount = 0; // use volatile for shared variables
-
-void blinkLed() {
-	if (ledState == LOW) {
-		ledState = HIGH;
-//Serial.print("R99 led on");
-	} else {
-		ledState = LOW;
-//Serial.print("R99 led off");
-	}
-	digitalWrite(LED_PIN, ledState);
-}
-
-
 StepperControl::StepperControl() {
 
-}
+	axisX = StepperControlAxis();
+	axisY = StepperControlAxis();
+	axisZ = StepperControlAxis();
 
-unsigned long getMaxLength(unsigned long lengths[3]) {
-	unsigned long max = lengths[0];
-	for (int i = 1; i < 3; i++) {
-		if (lengths[i] > max) {
-			max = lengths[i];
-		}
-	}
-	return max;
-}
+	axisX.label = 'X';
+	axisY.label = 'Y';
+	axisZ.label = 'Z';
 
-bool endStopMin(int axis_nr) {
+	axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN);
+	axisY.loadPinNumbers(Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN);
+	axisZ.loadPinNumbers(Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN);
 
-        long stepPin[3] = {     X_MIN_PIN,
-                                Y_MIN_PIN,
-                                Z_MIN_PIN };
-
-	return  digitalRead(stepPin[axis_nr]);
-}
-
-bool endStopMax(int axis_nr) {
-
-        long stepPin[3] = {     X_MAX_PIN,
-                                Y_MAX_PIN,
-                                Z_MAX_PIN };
-
-        return  digitalRead(stepPin[axis_nr]);
-}
-
-int endStopAxisReached(int axis_nr, bool movement_forward) {
-
-        bool endStInv[3] = {    ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X),
-                                ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y),
-                                ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) };
-
-	bool min_endstop = false;
-	bool max_endstop = false;
-	bool invert = false;
-
-	if (endStInv[axis_nr]) {
-		invert = true;
-	}
-
-	// for the axis to check, retrieve the end stop status
-
-	if (!invert) {
-		min_endstop = endStopMin(axis_nr);
-		max_endstop = endStopMax(axis_nr);
-	} else {
-		min_endstop = endStopMax(axis_nr);
-		max_endstop = endStopMin(axis_nr);
-	}
-/*
-	if (axis_nr == 0) {
-		min_endstop=(digitalRead(X_MIN_PIN) ^ invert);
-		max_endstop=(digitalRead(X_MAX_PIN) ^ invert);
-	}
-
-	if (axis_nr == 1) {
-		min_endstop=(digitalRead(Y_MIN_PIN) ^ invert);
-		max_endstop=(digitalRead(Y_MAX_PIN) ^ invert);
-	}
-
-	if (axis_nr == 2) {
-		min_endstop=(digitalRead(Z_MIN_PIN) ^ invert);
-		max_endstop=(digitalRead(Z_MAX_PIN) ^ invert);
-	}
-*/
-
-	// if moving forward, only check the end stop max
-	// for moving backwards, check only the end stop min
-
-	if((!movement_forward && min_endstop) || (movement_forward && max_endstop)) {
-		return 1;
-	}
-	return 0;
-}
-
-void step(int axis, long &currentPoint, unsigned long steps,
-		long destinationPoint) {
-
-	if (currentPoint < destinationPoint) {
-		currentPoint += steps;
-	} else if (currentPoint > destinationPoint) {
-		currentPoint -= steps;
-	}
-
-	switch (axis) {
-	case 0:
-		digitalWrite(X_STEP_PIN, HIGH);
-		//digitalWrite(X_STEP_PIN, LOW);
-		break;
-	case 1:
-		digitalWrite(Y_STEP_PIN, HIGH);
-		//digitalWrite(Y_STEP_PIN, LOW);
-		break;
-	case 2:
-		digitalWrite(Z_STEP_PIN, HIGH);
-		//digitalWrite(Z_STEP_PIN, LOW);
-		break;
-	}
-
-	// if the home end stop is reached, set the current position
-	if (endStopAxisReached(axis, false))
-	{
-		currentPoint = 0;
-	}
-}
-
-void resetStep(int axis) {
-	switch (axis) {
-	case 0:
-		digitalWrite(X_STEP_PIN, LOW);
-		break;
-	case 1:
-		digitalWrite(Y_STEP_PIN, LOW);
-		break;
-	case 2:
-		digitalWrite(Z_STEP_PIN, LOW);
-		break;
-	}
-
-}
-
-bool pointReached(long currentPoint[3],
-		  long destinationPoint[3]) {
-	for (int i = 0; i < 3; i++) {
-		if (destinationPoint[i] != currentPoint[i]) {
-			return false;
-		}
-	}
-	return true;
-}
-
-/*
-unsigned int calculateSpeed(unsigned int &currentStepsPerSecond,
-		unsigned int maxStepsPerSecond, unsigned int currentSteps,
-		unsigned int currentMillis, unsigned int maxLength,
-		unsigned int &accelerationSteps,
-		unsigned int maxAccelerationStepsPerSecond) {
-
-	if (currentStepsPerSecond < maxStepsPerSecond) {
-		if (currentSteps < maxLength / 2) {
- 			accelerationSteps = currentSteps;
-			if (currentMillis % 100 == 0) {
-				currentStepsPerSecond += maxAccelerationStepsPerSecond / 10;
-			}
-		} else {
-			if (currentMillis % 100 == 0
-					&& currentStepsPerSecond
-							> maxAccelerationStepsPerSecond / 10) {
-				currentStepsPerSecond -= maxAccelerationStepsPerSecond / 10;
-			}
-		}
-	} else if (currentSteps > maxLength - accelerationSteps
-			&& currentStepsPerSecond > maxAccelerationStepsPerSecond / 10) {
-		if (currentMillis % 100 == 0) {
-			currentStepsPerSecond -= maxAccelerationStepsPerSecond / 10;
-		}
-	} else {
-		currentStepsPerSecond = maxStepsPerSecond;
-	}
-	if (currentStepsPerSecond < maxAccelerationStepsPerSecond) {
-		currentStepsPerSecond = maxAccelerationStepsPerSecond;
-	}
-	return currentStepsPerSecond;
-}
-*/
-
-
-unsigned int lastCalcLog = 0;
-
-unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) {
-
-	int newSpeed = 0;
-
-	long curPos = abs(currentPosition);
-
-	long staPos;
-	long endPos;
-
-	if (abs(sourcePosition) < abs(destinationPosition)) {
-		staPos = abs(sourcePosition);
-		endPos = abs(destinationPosition);;
-	} else {
-		staPos = abs(destinationPosition);;
-		endPos = abs(sourcePosition);
-	}
-
-	unsigned long halfway = ((endPos - staPos) / 2) + staPos;
-
-	// Set the minimum speed if the position would be out of bounds
-	if (curPos < staPos || curPos > endPos) {
-		newSpeed = minSpeed;
-	} else {
-		if (curPos >= staPos && curPos <= halfway) {
-			// accelerating
-			if (curPos > (staPos + stepsAccDec)) {
-				// now beyond the accelleration point to go full speed
-				newSpeed = maxSpeed + 1;
-			} else {
-				// speeding up, increase speed linear within the first period
-				newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
-			}
-		} else {
-			// decelerating
-			if (curPos < (endPos - stepsAccDec)) {
-				// still before the deceleration point so keep going at full speed
-				newSpeed = maxSpeed + 2;
-			} else {
-				// speeding up, increase speed linear within the first period
-				newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
-			}
-		}
-	}
-
-/*
-	if (millis() - lastCalcLog > 1000) {
-
-		lastCalcLog = millis();
-
-		Serial.print("R99");
-
-	//	Serial.print(" a ");
-	//	Serial.print(endPos);
-	//	Serial.print(" b ");
-	//	Serial.print((endPos - stepsAccDec));
-	//	Serial.print(" c ");
-	//	Serial.print(curPos < (endPos - stepsAccDec));
-
-
-		Serial.print(" sta ");
-		Serial.print(staPos);
-		Serial.print(" cur ");
-		Serial.print(curPos);
-		Serial.print(" end ");
-		Serial.print(endPos);
-		Serial.print(" half ");
-		Serial.print(halfway);
-		Serial.print(" len ");
-		Serial.print(stepsAccDec);
-		Serial.print(" min ");
-		Serial.print(minSpeed);
-		Serial.print(" max ");
-		Serial.print(maxSpeed);
-		Serial.print(" spd ");
-
-		Serial.print(" ");
-		Serial.print(newSpeed);
-
-		Serial.print("\n");
-	}
-*/
-
-	// Return the calculated speed, in steps per second
-	return newSpeed;
-}
-
-void enableMotors(bool enable) {
-	if (enable) {
-		digitalWrite(X_ENABLE_PIN, LOW);
-		digitalWrite(Y_ENABLE_PIN, LOW);
-		digitalWrite(Z_ENABLE_PIN, LOW);
-		delay(100);
-	} else {
-		digitalWrite(X_ENABLE_PIN, HIGH);
-		digitalWrite(Y_ENABLE_PIN, HIGH);
-		digitalWrite(Z_ENABLE_PIN, HIGH);
-	}
-}
-
-void setDirectionAxis(int dirPin, long currentPoint, long destinationPoint, bool goHome, bool homeIsUp, bool motorInv) {
-
-       if  (((!goHome && currentPoint < destinationPoint) || (goHome &&  homeIsUp)) ^ motorInv) {
-                digitalWrite(dirPin, HIGH);
-        } else {
-                digitalWrite(dirPin, LOW);
-        }
-
-
-}
-
-void setDirections(long* currentPoint, long* destinationPoint, bool* homeAxis, bool* motorInv) {
-
-        bool homeIsUp[3] = {    ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X),
-                                ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y),
-                                ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z) };
-
-	setDirectionAxis(X_DIR_PIN, currentPoint[0], destinationPoint[0], homeAxis[0], homeIsUp[0], motorInv[0]);
-	setDirectionAxis(Y_DIR_PIN, currentPoint[1], destinationPoint[1], homeAxis[1], homeIsUp[1], motorInv[1]);
-	setDirectionAxis(Z_DIR_PIN, currentPoint[2], destinationPoint[2], homeAxis[2], homeIsUp[2], motorInv[2]);
-}
-
-unsigned long getLength(long l1, long l2) {
-	if(l1 > l2) {
-		return l1 - l2;
-	} else {
-		return l2 - l1;
-	}
-}
-
-int endStopsReached() {
-
-        bool endStInv[3] = {    ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_X),
-                                ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Y),
-                                ParameterList::getInstance()->getValue(MOVEMENT_INVERT_ENDPOINTS_Z) };
-
-	bool x_min_endstop=(digitalRead(X_MIN_PIN) == endStInv[0]);
-	bool x_max_endstop=(digitalRead(X_MAX_PIN) == endStInv[0]);
-	bool y_min_endstop=(digitalRead(Y_MIN_PIN) == endStInv[1]);
-	bool y_max_endstop=(digitalRead(Y_MAX_PIN) == endStInv[1]);
-	bool z_min_endstop=(digitalRead(Z_MIN_PIN) == endStInv[2]);
-	bool z_max_endstop=(digitalRead(Z_MAX_PIN) == endStInv[2]);
-	if(x_min_endstop || x_max_endstop || y_min_endstop || y_max_endstop || z_min_endstop || z_max_endstop) {
-		return 1;
-	}
-	return 0;
-}
-
-void reportEndStops() {
-
-	CurrentState::getInstance()->printEndStops();
-}
-
-void reportPosition(){
-	CurrentState::getInstance()->printPosition();
-}
-
-void storeEndStops() {
-
-	CurrentState::getInstance()->storeEndStops();
+	loadMotorSettings();
 }
 
 /**
- * water is dosed by setting the pin for the water high for a number of miliseconds
- *
+ * xDest - destination X in steps
+ * yDest - destination Y in steps
+ * zDest - destination Z in steps
+ * maxStepsPerSecond - maximum number of steps per second
+ * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second
  */
+int StepperControl::moveToCoords(		long xDest, long yDest, long zDest, 
+						unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
+                				bool xHome, bool yHome, bool zHome) {
 
-void doseWaterByTime(long time) {
-	digitalWrite(HEATER_1_PIN, HIGH);
-	delay(time);
-	digitalWrite(HEATER_1_PIN, LOW);
-}
+ 	unsigned long currentMillis         = 0;
+	unsigned long timeStart             = millis();
 
-void checkAxisDirection(int i) {
-	if (homeAxis[i]){
-		// When home is active, the direction is fixed
-		movementUp[i]     = homeIsUp[i];
-		movementToHome[i] = true;
-	} else {
-		// For normal movement, move in direction of destination
-                movementUp[i]     = (    currentPoint[i]  <      destinationPoint[i] );
-		movementToHome[i] = (abs(currentPoint[i]) >= abs(destinationPoint[i]));
+	int incomingByte     = 0;
+	int error            = 0;
+
+	// load motor settings
+
+	loadMotorSettings();
+
+	// if a speed is given in the command, use that instead of the config speed
+
+	if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) {
+		speedMax[0] = xMaxSpd;
 	}
 
-	if (currentPoint[i] == 0) {
-		// Go slow when theoretical end point reached but there is no end stop siganl
-		axisSpeed[i] = speedMin[i];
+	if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) {
+		speedMax[1] = yMaxSpd;
 	}
+
+	if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) {
+		speedMax[2] = zMaxSpd;
+	}
+
+        axisX.setMaxSpeed(speedMax[0]);
+        axisY.setMaxSpeed(speedMax[1]);
+        axisZ.setMaxSpeed(speedMax[2]);
+
+	// Load coordinates into axis class
+
+	long sourcePoint[3]     = {0,0,0};
+	sourcePoint[0] 		= CurrentState::getInstance()->getX();
+	sourcePoint[1] 		= CurrentState::getInstance()->getY();
+	sourcePoint[2] 		= CurrentState::getInstance()->getZ();
+
+	long currentPoint[3]    = {0,0,0};
+	currentPoint[0] 	= CurrentState::getInstance()->getX();
+	currentPoint[1]		= CurrentState::getInstance()->getY();
+	currentPoint[2]		= CurrentState::getInstance()->getZ();
+
+	long destinationPoint[3]= {0,0,0};
+	destinationPoint[0] 	= xDest;
+        destinationPoint[1] 	= yDest;
+        destinationPoint[2] 	= zDest;
+
+	// Load coordinates into motor control
+
+	axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome);
+	axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome);
+	axisZ.loadCoordinates(currentPoint[2],destinationPoint[2],zHome);
+
+	// Prepare for movement
+
+	storeEndStops();
+	reportEndStops();
+
+
+	axisX.setDirectionAxis();
+	axisY.setDirectionAxis();
+	axisZ.setDirectionAxis();
+
+        enableMotors();
+
+	// Start movement
+
+	axisActive[0] = true;
+	axisActive[1] = true;
+	axisActive[2] = true;
+
+
+	axisX.checkMovement();
+	axisY.checkMovement();
+	axisZ.checkMovement();
+
+	Timer1.start();
+
+	// Let the interrupt handle all the movements
+	while (axisActive[0] || axisActive[1] || axisActive[2]) {
+
+		delay(1);
+
+		axisActive[0] = axisX.isAxisActive();
+		axisActive[1] = axisY.isAxisActive();
+		axisActive[2] = axisZ.isAxisActive();
+
+	        storeEndStops();
+
+		// Check timeouts
+		if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
+			error = 1;
+		}
+		if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
+			error = 1;
+		}
+		if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
+			error = 1;
+		}
+
+		// Check if there is an emergency stop command
+		if (Serial.available() > 0) {
+                	incomingByte = Serial.read();
+			if (incomingByte == 69 || incomingByte == 101) {
+				error = 1;
+			}
+	        }
+
+		if (error == 1) {
+			Serial.print("R99 error\n");
+			Timer1.stop();
+			axisActive[0] = false;
+			axisActive[1] = false;
+			axisActive[2] = false;
+		}
+
+		// Periodically send message still active
+		currentMillis++;
+		if (currentMillis % 2500 == 0)
+		{
+			Serial.print("R04\n");
+		}
+
+	}
+
+	Serial.print("R99 stopped\n");
+
+	Timer1.stop();
+	disableMotors();
+
+	currentPoint[0] = axisX.currentPoint();
+	currentPoint[1] = axisY.currentPoint();
+	currentPoint[2] = axisZ.currentPoint();
+
+	CurrentState::getInstance()->setX(currentPoint[0]);
+	CurrentState::getInstance()->setY(currentPoint[1]);
+	CurrentState::getInstance()->setZ(currentPoint[2]);
+
+        storeEndStops();
+	reportEndStops();
+	reportPosition();
+
+	return error;
 }
 
-void stepAxis(int i, bool state) {
+//
+// Calibration
+//
 
-	if (homeAxis[i] && currentPoint[i] == 0) {
+int StepperControl::calibrateAxis(int axis) {
+
+	// Load motor settings
+
+	loadMotorSettings();
+
+	//unsigned long timeStart             = millis();
+
+        bool movementDone    = false;
+
+	int  paramValueInt   = 0;
+	int  stepsCount	     = 0;
+	int  incomingByte    = 0;
+	int  error           = 0;
+
+
+	bool invertEndStops = false;
+	int parEndInv;
+	int parNbrStp;
+
+	// Prepare for movement
+
+	storeEndStops();
+	reportEndStops();
+
+
+	// Select the right axis
+	StepperControlAxis calibAxis;
+	switch (axis) {
+		case 0:
+			calibAxis      = axisX;
+			parEndInv      = MOVEMENT_INVERT_ENDPOINTS_X;
+			parNbrStp      = MOVEMENT_AXIS_NR_STEPS_X;
+			invertEndStops = endStInv[0];
+			break;
+		case 1:
+			calibAxis      = axisY;
+			parEndInv      = MOVEMENT_INVERT_ENDPOINTS_Y;;
+			parNbrStp      = MOVEMENT_AXIS_NR_STEPS_Y;
+			invertEndStops = endStInv[0];
+			break;
+		case 2:
+			calibAxis      = axisZ;
+			parEndInv      = MOVEMENT_INVERT_ENDPOINTS_Z;
+			parNbrStp      = MOVEMENT_AXIS_NR_STEPS_Z;
+			invertEndStops = endStInv[0];
+			break;
+		default:
+			Serial.print("R99 Calibration error: invalid axis selected\n");
+			return 1;
+	}
+
+
+	// Preliminary checks
+
+	if (calibAxis.endStopMin() || calibAxis.endStopMax()) {
+		Serial.print("R99 Calibration error: end stop active before start\n");
+		return 1;
+	}
+
+	Serial.print("R99");
+	Serial.print(" axis ");
+	Serial.print(calibAxis.label);
+	Serial.print(" calibration start");
+	Serial.print("\n");
+
+	// Move towards home
+
+        calibAxis.enableMotor();
+	calibAxis.setDirectionHome();
+
+	stepsCount = 0;
+	movementDone = false;
+	while (!movementDone && error == 0) {
+
+                // Check if there is an emergency stop command
+                if (Serial.available() > 0) {
+                        incomingByte = Serial.read();
+                        if (incomingByte == 69 || incomingByte == 101) {
+                                movementDone = true;
+				error = 1;
+                        }
+                }
+
+		// Move until the end stop for home position is reached
+		if ((!calibAxis.endStopMin() && !calibAxis.endStopMax()) && !movementDone) {
+
+			calibAxis.setMotorStep();
+
+			delayMicroseconds(1000000 / speedMin[axis] /2);
+
+			stepsCount++;
+	                if (stepsCount % (speedMin[axis] * 3) == 0)
+	                {
+				// Periodically send message still active
+	                        Serial.print("R04\n");
+	                }
+
+			calibAxis.resetMotorStep();
+			delayMicroseconds(1000000 / speedMin[axis] /2);
 
-		// Keep moving toward end stop even when position is zero
-		// but end stop is not yet active
-		if (homeIsUp[i]) {
-			currentPoint[i] = -1;
 		} else {
-			currentPoint[i] =  1;
-		}
-	}
+			movementDone = true;
 
-	// set a step on the motors
-	step(i, currentPoint[i], 1, destinationPoint[i]);
-        blinkLed();
-	//stepMade = true;
-	//lastStepMillis[i] = currentMillis;
-
-}
-
-void checkAxis(int i) {
-
-	//moveTicks[i]++;
-	checkAxisDirection(i);
-
-
-/*
-Serial.print("R99 check axis ");
-Serial.print(i);
-Serial.print(" axis active ");
-Serial.print(axisActive[i]);
-Serial.print(" current point ");
-Serial.print(currentPoint[i]);
-Serial.print(" destination point ");
-Serial.print(destinationPoint[i]);
-
-Serial.print(" home stop reached ");
-Serial.print(endStopAxisReached(i, false));
-Serial.print(" axis stop reached ");
-Serial.print(endStopAxisReached(i, true));
-Serial.print(" home axis ");
-Serial.print(homeAxis[i]);
-Serial.print(" movement to home ");
-Serial.print(movementToHome[i]);
-Serial.print("\n");
-*/
-
-
-	//if ((!pointReached(currentPoint, destinationPoint) || home) && axisActive[i]) {
-	if (((currentPoint[i] != destinationPoint[i]) || homeAxis[i]) && axisActive[i]) {
-//Serial.print("R99 point not reached yet\n");
-		// home or destination not reached, keep moving
-/*
-		Serial.print("R99 calc axis speed");
-		Serial.print(" speed max ");
-		Serial.print(speedMax[i]);
-		Serial.print("\n");
-*/
-
-		// Get the axis speed, in steps per second
-		axisSpeed[i] = calculateSpeed(	sourcePoint[i],currentPoint[i],destinationPoint[i],
-				 		speedMin[i], speedMax[i], stepsAcc[i]);
-
-//Serial.print("R99 axis speed ");
-//Serial.print(axisSpeed[i]);
-//Serial.print("\n");
-
-/*
-Serial.print("R99 check axis b  ");
-Serial.print(i);
-Serial.print(" home part true ");
-Serial.print(homeAxis[i] && !endStopAxisReached(i, false));
-Serial.print(" !homeAxis[i] ");
-Serial.print(!homeAxis[i]);
-Serial.print(" !endStopAxisReached(i, !movementToHome[i]) ");
-Serial.print(!endStopAxisReached(i, !movementToHome[i]));
-Serial.print("\n");
-*/
-		// If end stop reached, don't move anymore
-		if ((homeAxis[i] && !endStopAxisReached(i, false)) || (!homeAxis[i] &&  !endStopAxisReached(i, !movementToHome[i]))) {
-
-			// Set the moments when the step is set to true and false
-
-			if (axisSpeed[i] > 0)
+			// If end stop for home is active, set the position to zero
+			if (calibAxis.endStopMax())
 			{
-/*
-				Serial.print("R99 calculated steps ");
-				Serial.print(" ** additional ticks steps ");
-				Serial.print(1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i]);
-				Serial.print("\n");
-*/
-
-				// Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
-				// This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
-				stepOnTick[i]  = moveTicks[i] + (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i] / 2);
-				stepOffTick[i] = moveTicks[i] + (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / axisSpeed[i]    );
-				//stepOnTick[i]  = moveTicks[i] + (axisSpeed[i] * 1000 / MOVEMENT_INTERRUPT_SPEED / 2);
-				//stepOffTick[i] = moveTicks[i] + (axisSpeed[i] * 1000 / MOVEMENT_INTERRUPT_SPEED    );
-
-/*
-				Serial.print("R99 calculated steps ");
-				Serial.print(" interrupt speed ");
-				Serial.print(MOVEMENT_INTERRUPT_SPEED);
-				Serial.print(" axisSpeed ");
-				Serial.print(axisSpeed[i]);
-				Serial.print(" moveTicks ");
-				Serial.print(moveTicks[i]);
-				Serial.print(" stepOnTick ");
-				Serial.print(stepOnTick[i]);
-				Serial.print(" stepOffTick ");
-				Serial.print(stepOffTick[i]);
-				Serial.print("\n");
-*/
-			}
-		}
-		else {
-			axisActive[i] = false;
-		}
-
-	} else {
-		// Destination or home reached. Deactivate the axis.
-		axisActive[i] = false;
-	}
-
-	// If end stop for home is active, set the position to zero
-	if (endStopAxisReached(i, false)) {
-		currentPoint[i] = 0;
-	}
-
-
-}
-
-void checkTicksAxis(int i) {
-
-	moveTicks[i]++;
-
-//Serial.print("R99 checkTicksAxis ");
-//Serial.print(" destination point ");
-//Serial.print(destinationPoint[0]);
-//Serial.print(" moveTicks ");
-//Serial.print(moveTicks[i]);
-//Serial.print(" stepOnTick ");
-//Serial.print(stepOnTick[i]);
-//Serial.print(" stepOffTick ");
-//Serial.print(stepOffTick[i]);
-//Serial.print("\n");
-
-	if (axisActive[i]) {
-		if (moveTicks[i] >= stepOffTick[i]) {
-
-/*
-Serial.print("R99 reset step ");
-Serial.print(" moveTicks ");
-Serial.print(moveTicks[i]);
-Serial.print("\n");
-*/
-			// Negative flank for the steps
-			resetStep(i);
-			checkAxis(i);
-		}
-		else {
-
-			if (moveTicks[i] == stepOnTick[i]) {
-/*
-Serial.print("R99 set step ");
-Serial.print(" moveTicks ");
-Serial.print(moveTicks[i]);
-Serial.print("\n");
-*/
-				// Positive flank for the steps
-				stepAxis(i, true);
+				invertEndStops = true;
 			}
 		}
 	}
-}
 
+	Serial.print("R99");
+	Serial.print(" axis ");
+	Serial.print(calibAxis.label);
+	Serial.print(" at first end stop");
+	Serial.print("\n");
+
+	// Report back the end stop setting
+
+	if (error == 0) {
+		if (invertEndStops) {
+			paramValueInt = 1;
+		} else {
+			paramValueInt = 0;
+		}
+
+		Serial.print("R23");
+		Serial.print(" ");
+		Serial.print("P");
+		Serial.print(parEndInv);
+		Serial.print(" ");
+		Serial.print("V");
+		Serial.print(paramValueInt);
+		Serial.print("\n");
+	}
+
+	// Store the status of the system
+
+        storeEndStops();
+	reportEndStops();
+
+	// Move into the other direction now, and measure the number of steps
+
+	stepsCount = 0;
+	movementDone = false;
+	calibAxis.setDirectionAway();
+
+	while (!movementDone && error == 0) {
+
+                // Check if there is an emergency stop command
+                if (Serial.available() > 0) {
+                        incomingByte = Serial.read();
+                        if (incomingByte == 69 || incomingByte == 101) {
+                                movementDone = true;
+				error = 1;
+                        }
+                }
+
+		// Move until the end stop at the other side of the axis is reached
+		if (((!invertEndStops && !calibAxis.endStopMax()) || (invertEndStops && !calibAxis.endStopMin())) && !movementDone) {
+
+			calibAxis.setMotorStep();
+			stepsCount++;
+
+			delayMicroseconds(1000000 / speedMin[axis] /2);
+
+	                if (stepsCount % (speedMin[axis] * 3) == 0)
+        	        {
+				// Periodically send message still active
+	                        Serial.print("R04\n");
+	                }
+
+
+			calibAxis.resetMotorStep();
+			delayMicroseconds(1000000 / speedMin[axis] /2);
+
+		} else {
+			movementDone = true;
+		}
+	}
+
+
+	Serial.print("R99");
+	Serial.print(" axis ");
+	Serial.print(calibAxis.label);
+	Serial.print(" at second end stop");
+	Serial.print("\n");
+
+	// Report back the end stop setting
+
+	if (error == 0) {
+		Serial.print("R23");
+		Serial.print(" ");
+		Serial.print("P");
+		Serial.print(parNbrStp);
+		Serial.print(" ");
+		Serial.print("V");
+		Serial.print(stepsCount);
+		Serial.print("\n");
+	}
+
+	calibAxis.disableMotor();
+
+        storeEndStops();
+	reportEndStops();
+
+
+        switch (axis) {
+		case 0:
+			CurrentState::getInstance()->setX(stepsCount);
+			break;
+		case 1:
+		        CurrentState::getInstance()->setY(stepsCount);
+			break;
+		case 2:
+		        CurrentState::getInstance()->setZ(stepsCount);
+			break;
+	}
+
+	reportPosition();
+
+	return error;
+}
 
 
 // Handle movement by checking each axis
 void StepperControl::handleMovementInterrupt(void){
-//Serial.print("R99 interrupt\n");
-
-//Serial.print("R99 interrupt data ");
-//Serial.print(" destination point ");
-//Serial.print(destinationPoint[0]);
-//Serial.print("  ");
-//Serial.print("test ");
-//Serial.print(test);
-//Serial.print("\n");
-
-        checkTicksAxis(0);
-        checkTicksAxis(1);
-        checkTicksAxis(2);
-
-//      blinkLed();
 
+        axisX.checkTiming();
+        axisY.checkTiming();
+        axisZ.checkTiming();
 }
 
 bool interruptBusy = false;
@@ -641,44 +442,70 @@ void StepperControl::initInterrupt() {
 }
 
 
+
+
+unsigned long StepperControl::getMaxLength(unsigned long lengths[3]) {
+	unsigned long max = lengths[0];
+	for (int i = 1; i < 3; i++) {
+		if (lengths[i] > max) {
+			max = lengths[i];
+		}
+	}
+	return max;
+}
+
+void StepperControl::enableMotors() {
+	axisX.enableMotor();
+	axisY.enableMotor();
+	axisZ.enableMotor();
+	delay(100);
+}
+
+void StepperControl::disableMotors() {
+	axisX.disableMotor();
+	axisY.disableMotor();
+	axisZ.disableMotor();
+	delay(100);
+}
+
+bool StepperControl::endStopsReached() {
+
+	if (	axisX.endStopsReached() ||
+		axisY.endStopsReached() ||
+		axisZ.endStopsReached()) {
+		return true;
+	}
+	return false;
+
+}
+
+void StepperControl::reportEndStops() {
+	CurrentState::getInstance()->printEndStops();
+}
+
+void StepperControl::reportPosition(){
+	CurrentState::getInstance()->printPosition();
+}
+
+void StepperControl::storeEndStops() {
+	CurrentState::getInstance()->storeEndStops();
+}
+
 /**
- * xDest - destination X in steps
- * yDest - destination Y in steps
- * zDest - destination Z in steps
- * maxStepsPerSecond - maximum number of steps per second
- * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second
+ * water is dosed by setting the pin for the water high for a number of miliseconds
+ *
  */
-int StepperControl::moveAbsoluteConstant(	long xDest, long yDest, long zDest, 
-						unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
-                				bool xHome, bool yHome, bool zHome) {
+
+//void StepperControl::doseWaterByTime(long time) {
+//	digitalWrite(HEATER_1_PIN, HIGH);
+//	delay(time);
+//	digitalWrite(HEATER_1_PIN, LOW);
+//}
 
 
-//Serial.print("R99 ");
-//Serial.print(" xMaxSpd ");
-//Serial.print(xMaxSpd);
-//Serial.print("\n");
+void StepperControl::loadMotorSettings() {
 
-	sourcePoint[0] 		= CurrentState::getInstance()->getX();
-	sourcePoint[1] 		= CurrentState::getInstance()->getY();
-	sourcePoint[2] 		= CurrentState::getInstance()->getZ();
-
-	currentPoint[0] 	= CurrentState::getInstance()->getX();
-	currentPoint[1]		= CurrentState::getInstance()->getY();
-	currentPoint[2]		= CurrentState::getInstance()->getZ();
-
-	destinationPoint[0] 	= xDest;
-        destinationPoint[1] 	= yDest;
-        destinationPoint[2] 	= zDest;
-
-	movementLength[0] 	= getLength(destinationPoint[0], currentPoint[0]);
-	movementLength[1] 	= getLength(destinationPoint[1], currentPoint[1]);
-	movementLength[2] 	= getLength(destinationPoint[2], currentPoint[2]);
-
-	maxLenth 		= getMaxLength(movementLength);
-
-	lengthRatio[0]		= 1.0 * movementLength[0] / maxLenth;
-	lengthRatio[1]		= 1.0 * movementLength[1] / maxLenth;
-	lengthRatio[2]		= 1.0 * movementLength[2] / maxLenth;
+	// Load settings
 
 	homeIsUp[0]		= ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X);
 	homeIsUp[1]		= ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y);
@@ -708,452 +535,8 @@ int StepperControl::moveAbsoluteConstant(	long xDest, long yDest, long zDest,
 	timeOut[1]		= ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X);
 	timeOut[2]		= ParameterList::getInstance()->getValue(MOVEMENT_TIMEOUT_X);
 
-        homeAxis[0]		= xHome;
-	homeAxis[1]		= yHome;
-	homeAxis[2]		= zHome;
+	axisX.loadMotorSettings(speedMax[0], speedMin[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], MOVEMENT_INTERRUPT_SPEED);
+	axisY.loadMotorSettings(speedMax[1], speedMin[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], MOVEMENT_INTERRUPT_SPEED);
+	axisZ.loadMotorSettings(speedMax[2], speedMin[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], MOVEMENT_INTERRUPT_SPEED);
 
-	moveTicks[0]		= 0;
-	moveTicks[1]		= 0;
-	moveTicks[2]		= 0;
-
-        home = xHome || yHome || zHome;
-
-//Serial.print("R99 ");
-//Serial.print(" after vars ");
-//Serial.print(destinationPoint[0]);
-//Serial.print("\n");
-
-
- 	unsigned long currentMillis         = 0;
-
-	unsigned long timeStart             = millis();
-
-
-        bool movementDone    = false;
-        bool movementUp      = false;
-        bool movementToHome  = false;
-
-        bool moving          = false;
-
-	bool stepMade        = false;
-	int incomingByte     = 0;
-	//int axisSpeed        = 0;
-	int error            = 0;
-
-	// if a speed is given in the command, use that instead of the config speed
-
-	if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) {
-		speedMax[0] = xMaxSpd;
-	}
-
-	if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) {
-		speedMax[1] = yMaxSpd;
-	}
-
-	if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) {
-		speedMax[2] = zMaxSpd;
-	}
-
-	// Prepare for movement
-
-	setDirections(currentPoint, destinationPoint, homeAxis, motorInv);
-
-
-	storeEndStops();
-	reportEndStops();
-        enableMotors(true);
-
-	// Limit normal movmement to the home position
-	for (int i = 0; i < 3; i++) {
-		if (!homeIsUp[i] && destinationPoint[i] < 0) {
-			destinationPoint[i] = 0;
-		}
-
-		if ( homeIsUp[i] && destinationPoint[i] > 0) {
-			destinationPoint[i] = 0;
-		}
-	}
-
-	// Start movement
-
-	axisActive[0] = true;
-	axisActive[1] = true;
-	axisActive[2] = true;
-
-	checkAxis(0);
-	checkAxis(1);
-	checkAxis(2);
-
-	Timer1.start();
-
-
-	// Let the interrupt handle all the movements
-	while (axisActive[0] || axisActive[1] || axisActive[2]) {
-/**///Serial.print("R99 while loop\n");
-		//delay(50);
-		delayMicroseconds(100);
-
-//Serial.print("R99 ");
-//Serial.print(" maximim speed ");
-//Serial.print(speedMax[0]);
-//Serial.print("\n");
-
-
-	checkTicksAxis(0);
-	checkTicksAxis(1);
-	checkTicksAxis(2);
-
-
-//axisActive[0] = false;
-//axisActive[1] = false;
-//axisActive[2] = false;
-
-/*
-			Serial.print("R99");
-			Serial.print(" cur ");
-			Serial.print(currentPoint[0]);
-			Serial.print(" end ");
-			Serial.print(destinationPoint[0]);
-			Serial.print(" max spd ");
-			Serial.print(speedMax[0]);
-			Serial.print(" axis spd ");
-			Serial.print(axisSpeed[0]);
-			Serial.print("\n");
-*/
-
-	        storeEndStops();
-
-		// Check timeouts
-		if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
-			error = 1;
-		}
-		if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
-			error = 1;
-		}
-		if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
-			error = 1;
-		}
-
-		// Check if there is an emergency stop command
-		if (Serial.available() > 0) {
-                	incomingByte = Serial.read();
-			if (incomingByte == 69 || incomingByte == 101) {
-				error = 1;
-			}
-	        }
-
-		if (error == 1) {
-Serial.print("R99 error\n");
-			Timer1.stop();
-			axisActive[0] = false;
-			axisActive[1] = false;
-			axisActive[2] = false;
-		}
-
-		// Periodically send message still active
-		currentMillis++;
-		if (currentMillis % 2500 == 0)
-		{
-			Serial.print("R04\n");
-		}
-
-	}
-
-/**/Serial.print("R99 stopped\n");
-
-	Timer1.stop();
-	enableMotors(false);
-
-	CurrentState::getInstance()->setX(currentPoint[0]);
-	CurrentState::getInstance()->setY(currentPoint[1]);
-	CurrentState::getInstance()->setZ(currentPoint[2]);
-
-        storeEndStops();
-	reportEndStops();
-	reportPosition();
-
-	return error;
 }
-
-//
-// Calibration
-//
-
-int StepperControl::calibrateAxis(int axis) {
-
-
-        int  parMotInv[3] = { 31, 32, 33};
-        int  parEndInv[3] = { 21, 22, 23};
-        int  parNbrStp[3] = {801,802,803};
-
-	bool motorInv[3] = { 	ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_X),
-				ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Y),
-				ParameterList::getInstance()->getValue(MOVEMENT_INVERT_MOTOR_Z) };
-
-        bool homeAxis[3] = {false,false,false};
-
-        bool homeIsUp[3] = {false,false,false};
-
-        long speedMin[3] = {200,200,200};
-
-        bool endStInv[3] = {false,false,false};
-
-        long timeOut[3] = {5000,5000,5000};
-
-        long stepPin[3] = {	X_STEP_PIN,
-				Y_STEP_PIN,
-				Z_STEP_PIN };
-
-        long dirPin[3]  = {	X_DIR_PIN,
-				Y_DIR_PIN,
-				Z_DIR_PIN };
-
-	// Set the coordinate variables for homing, so the default functions can be used for settign direction
-
-        long sourcePoint[3] 		= { 0, 0, 0 };
-        long destinationPoint[3] 	= { 0, 0, 0 };
-
- 	unsigned long currentMillis         = 0;
-
-	unsigned long currentSteps          = 0;
-	unsigned long lastStepMillis        = 0;
-
-	unsigned long timeStart             = millis();
-
-        bool movementDone    = false;
-
-	int  paramValueInt   = 0;
-	bool invertEndStops  = false;
-	int  stepsCount	     = 0;
-	int  incomingByte     = 0;
-	int  error            = 0;
-
-
-	// Prepare for movement
-
-	storeEndStops();
-	reportEndStops();
-
-	// Preliminary checks
-
-	if (endStopMin(axis) || endStopMax(axis)) {
-		Serial.print("R99 Calibration error: end stop active before start\n");
-		return 1;
-	}
-
-
-
-/*
-Serial.print("R99");
-Serial.print(" ");
-Serial.print("calibration start");
-Serial.print("\n");
-*/
-
-
-	// Move towards home
-
-        enableMotors(true);
-
-	movementDone = false;
-	setDirectionAxis(dirPin[0], 0, -1, homeAxis[axis], homeIsUp[axis], motorInv[axis]);
-
-	while (!movementDone && error == 0) {
-
-                // Check if there is an emergency stop command
-                if (Serial.available() > 0) {
-                        incomingByte = Serial.read();
-                        if (incomingByte == 69 || incomingByte == 101) {
-                                movementDone = true;
-				error = 1;
-                        }
-                }
-
-		// Move until the end stop for home position is reached
-		if ((!endStopMin(axis) && !endStopMax(axis)) && !movementDone) {
-
-			if ((millis() >= timeStart && millis() - timeStart > timeOut[axis] * 1000) || (millis() < timeStart && millis() > timeOut[axis] * 1000)) {
-				movementDone = true;
-				error        = 1;
-			} else {
-
-				// Only do a step every x milliseconds (x is calculated above)
-				if (currentMillis - lastStepMillis >= MOVEMENT_SPEED_BASE_TIME / speedMin[axis]) {
-					digitalWrite(X_STEP_PIN, HIGH);
-					lastStepMillis = currentMillis;
-				}
-       	                }
-
-			delayMicroseconds(MOVEMENT_DELAY);
-
-			currentMillis++;
-	                if (currentMillis % 10000 == 0)
-	                {
-				// Periodically send message still active
-	                        Serial.print("R04\n");
-	                }
-
-			digitalWrite(X_STEP_PIN, LOW);
-			delayMicroseconds(MOVEMENT_DELAY);
-
-		} else {
-			movementDone = true;
-
-			// If end stop for home is active, set the position to zero
-			if (endStopMax(axis))
-			{
-				invertEndStops = true;
-			}
-		}
-	}
-
-
-/*
-Serial.print("R99");
-Serial.print(" ");
-Serial.print("calibration halfway");
-Serial.print(" ");
-Serial.print("end stop invert");
-Serial.print(" ");
-Serial.print(invertEndStops);
-Serial.print(" ");
-Serial.print("error");
-Serial.print(" ");
-Serial.print(error);
-Serial.print("\n");
-*/
-
-
-	// Report back the end stop setting
-
-	if (error == 0) {
-		if (invertEndStops) {
-			paramValueInt = 1;
-		} else {
-			paramValueInt = 0;
-		}
-
-		Serial.print("R23");
-		Serial.print(" ");
-		Serial.print("P");
-		Serial.print(parEndInv[axis]);
-		Serial.print(" ");
-		Serial.print("V");
-		Serial.print(paramValueInt);
-		Serial.print("\n");
-	}
-
-	// Store the status of the system
-
-        storeEndStops();
-	reportEndStops();
-	//reportPosition();
-
-	// Move into the other direction now, and measure the number of steps
-
-	stepsCount = 0;
-	movementDone = false;
-	setDirectionAxis(dirPin[0], 0, 1, homeAxis[axis], homeIsUp[axis], motorInv[axis]);
-
-	while (!movementDone && error == 0) {
-
-                // Check if there is an emergency stop command
-                if (Serial.available() > 0) {
-                        incomingByte = Serial.read();
-                        if (incomingByte == 69 || incomingByte == 101) {
-                                movementDone = true;
-				error = 1;
-                        }
-                }
-
-
-		// Move until the end stop at the other side of the axis is reached
-		if (((!invertEndStops && !endStopMax(axis)) || (invertEndStops && !endStopMin(axis))) && !movementDone) {
-
-			if (millis() - timeStart > timeOut[axis] * MOVEMENT_SPEED_BASE_TIME) {
-				movementDone = true;
-				error        = 1;
-			} else {
-
-				// Only do a step every x milliseconds (x is calculated above)
-				if (currentMillis - lastStepMillis >= MOVEMENT_SPEED_BASE_TIME / speedMin[axis]) {
-					digitalWrite(X_STEP_PIN, HIGH);
-					stepsCount++;
-					lastStepMillis = currentMillis;
-				}
-       	                }
-
-			delayMicroseconds(MOVEMENT_DELAY);
-
-			currentMillis++;
-	                if (currentMillis % 10000 == 0)
-        	        {
-				// Periodically send message still active
-	                        Serial.print("R04\n");
-	                }
-
-
-			digitalWrite(X_STEP_PIN, LOW);
-			delayMicroseconds(MOVEMENT_DELAY);
-
-		} else {
-			movementDone = true;
-		}
-	}
-
-
-
-
-/*
-Serial.print("R99");
-Serial.print(" ");
-Serial.print("calibration done");
-Serial.print(" ");
-Serial.print("nr steps");
-Serial.print(" ");
-Serial.print(stepsCount);
-Serial.print(" ");
-Serial.print("error");
-Serial.print(" ");
-Serial.print(error);
-Serial.print("\n");
-*/
-
-
-
-	// Report back the end stop setting
-
-	if (error == 0) {
-		Serial.print("R23");
-		Serial.print(" ");
-		Serial.print("P");
-		Serial.print(parNbrStp[axis]);
-		Serial.print(" ");
-		Serial.print("V");
-		Serial.print(stepsCount);
-		Serial.print("\n");
-	}
-
-	enableMotors(false);
-
-        storeEndStops();
-	reportEndStops();
-
-
-        switch (axis) {
-		case 0:
-			CurrentState::getInstance()->setX(stepsCount);
-			break;
-		case 1:
-		        CurrentState::getInstance()->setY(stepsCount);
-			break;
-		case 2:
-		        CurrentState::getInstance()->setZ(stepsCount);
-			break;
-	}
-
-	reportPosition();
-
-	return error;
-}
-
diff --git a/src/StepperControl.h b/src/StepperControl.h
index 90b4b2a..66caed7 100644
--- a/src/StepperControl.h
+++ b/src/StepperControl.h
@@ -11,6 +11,7 @@
 #include "Arduino.h"
 #include "CurrentState.h"
 #include "ParameterList.h"
+#include "StepperControlAxis.h"
 #include "pins.h"
 #include "Config.h"
 #include <stdio.h>
@@ -18,21 +19,46 @@
 
 class StepperControl {
 public:
+	StepperControl();
+	StepperControl(StepperControl const&);
+	void operator=(StepperControl const&);
+
 	static StepperControl* getInstance();
-	int moveAbsolute(long xDest, long yDest,long zDest,
-			unsigned int maxStepsPerSecond,
-			unsigned int maxAccelerationStepsPerSecond);
-	int moveAbsoluteConstant(long xDest, long yDest, long zDest, 
+	//int moveAbsolute(	long xDest, long yDest,long zDest,
+	//			unsigned int maxStepsPerSecond,
+	//			unsigned int maxAccelerationStepsPerSecond);
+	int moveToCoords(	long xDest, long yDest, long zDest, 
 				 unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
 				 bool homeX, bool homeY, bool homeZ);
 
 	void handleMovementInterrupt();
 	int calibrateAxis(int axis);
 	void initInterrupt();
+	void enableMotors();
+	void disableMotors();
+
 private:
-	StepperControl();
-	StepperControl(StepperControl const&);
-	void operator=(StepperControl const&);
+	StepperControlAxis axisX;
+	StepperControlAxis axisY;
+	StepperControlAxis axisZ;
+
+	bool axisActive[3];
+
+	void loadMotorSettings();
+        void reportPosition();
+        void storeEndStops();
+        void reportEndStops();
+
+	unsigned long getMaxLength(unsigned long lengths[3]);
+	bool endStopsReached();
+
+        bool homeIsUp[3];
+        long speedMax[3];
+        long speedMin[3];
+        long stepsAcc[3];
+        bool motorInv[3];
+        bool endStInv[3];
+        long timeOut[3];
 };
 
 #endif /* STEPPERCONTROL_H_ */
diff --git a/src/StepperControlAxis.cpp b/src/StepperControlAxis.cpp
new file mode 100644
index 0000000..10fc147
--- /dev/null
+++ b/src/StepperControlAxis.cpp
@@ -0,0 +1,383 @@
+#include "StepperControlAxis.h"
+
+long interruptSpeed          = 100;
+
+StepperControlAxis::StepperControlAxis() {
+	lastCalcLog	= 0;
+
+        pinStep         = 0;
+        pinDirection    = 0;
+        pinEnable       = 0;
+        pinMin          = 0;
+        pinMax          = 0;
+
+}
+
+void StepperControlAxis::test() {
+                Serial.print("R99 ");
+                Serial.print(label);
+                Serial.print("\n");
+}
+
+unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec) {
+
+	int newSpeed = 0;
+
+	long curPos = abs(currentPosition);
+
+	long staPos;
+	long endPos;
+
+	if (abs(sourcePosition) < abs(destinationPosition)) {
+		staPos = abs(sourcePosition);
+		endPos = abs(destinationPosition);;
+	} else {
+		staPos = abs(destinationPosition);;
+		endPos = abs(sourcePosition);
+	}
+
+	unsigned long halfway = ((endPos - staPos) / 2) + staPos;
+
+	// Set the minimum speed if the position would be out of bounds
+	if (curPos < staPos || curPos > endPos) {
+		newSpeed = minSpeed;
+	} else {
+		if (curPos >= staPos && curPos <= halfway) {
+			// accelerating
+			if (curPos > (staPos + stepsAccDec)) {
+				// now beyond the accelleration point to go full speed
+				newSpeed = maxSpeed + 1;
+			} else {
+				// speeding up, increase speed linear within the first period
+				newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
+			}
+		} else {
+			// decelerating
+			if (curPos < (endPos - stepsAccDec)) {
+				// still before the deceleration point so keep going at full speed
+				newSpeed = maxSpeed + 2;
+			} else {
+				// speeding up, increase speed linear within the first period
+				newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
+			}
+		}
+	}
+
+/*
+	if (millis() - lastCalcLog > 1000) {
+
+		lastCalcLog = millis();
+
+		Serial.print("R99");
+
+	//	Serial.print(" a ");
+	//	Serial.print(endPos);
+	//	Serial.print(" b ");
+	//	Serial.print((endPos - stepsAccDec));
+	//	Serial.print(" c ");
+	//	Serial.print(curPos < (endPos - stepsAccDec));
+
+
+		Serial.print(" sta ");
+		Serial.print(staPos);
+		Serial.print(" cur ");
+		Serial.print(curPos);
+		Serial.print(" end ");
+		Serial.print(endPos);
+		Serial.print(" half ");
+		Serial.print(halfway);
+		Serial.print(" len ");
+		Serial.print(stepsAccDec);
+		Serial.print(" min ");
+		Serial.print(minSpeed);
+		Serial.print(" max ");
+		Serial.print(maxSpeed);
+		Serial.print(" spd ");
+
+		Serial.print(" ");
+		Serial.print(newSpeed);
+
+		Serial.print("\n");
+	}
+*/
+
+	// Return the calculated speed, in steps per second
+	return newSpeed;
+}
+
+void StepperControlAxis::checkAxisDirection() {
+
+	if (coordHomeAxis){
+		// When home is active, the direction is fixed
+		movementUp     = motorHomeIsUp;
+		movementToHome = true;
+	} else {
+		// For normal movement, move in direction of destination
+                movementUp     = (    coordCurrentPoint  <      coordDestinationPoint );
+		movementToHome = (abs(coordCurrentPoint) >= abs(coordDestinationPoint));
+	}
+
+	if (coordCurrentPoint == 0) {
+		// Go slow when theoretical end point reached but there is no end stop siganl
+		axisSpeed = motorSpeedMin;
+	}
+}
+
+void StepperControlAxis::setDirectionAxis() {
+
+       if  (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis &&  motorHomeIsUp)) ^ motorMotorInv) {
+		setDirectionUp();
+        } else {
+		setDirectionDown();
+        }
+}
+
+void StepperControlAxis::checkMovement() {
+
+	checkAxisDirection();
+
+	if (((coordCurrentPoint != coordDestinationPoint) || coordHomeAxis) && axisActive) {
+
+		// home or destination not reached, keep moving
+
+		// Get the axis speed, in steps per second
+		axisSpeed = calculateSpeed(	coordSourcePoint, coordCurrentPoint, coordDestinationPoint,
+				 		motorSpeedMin, motorSpeedMax, motorStepsAcc);
+
+		// If end stop reached, don't move anymore
+		if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis &&  !endStopAxisReached(!movementToHome))) {
+
+			// Set the moments when the step is set to true and false
+
+			if (axisSpeed > 0)
+			{
+
+				// Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
+				// This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
+				stepOnTick  = moveTicks + (1000.0 * 1000.0 / interruptSpeed / axisSpeed / 2);
+				stepOffTick = moveTicks + (1000.0 * 1000.0 / interruptSpeed / axisSpeed    );
+			}
+		}
+		else {
+			axisActive = false;
+		}
+
+	} else {
+		// Destination or home reached. Deactivate the axis.
+		axisActive = false;
+	}
+
+	// If end stop for home is active, set the position to zero
+	if (endStopAxisReached(false)) {
+		coordCurrentPoint = 0;
+	}
+}
+
+void StepperControlAxis::checkTiming() {
+
+	int i;
+
+	moveTicks++;
+
+	if (axisActive) {
+		if (moveTicks >= stepOffTick) {
+
+			// Negative flank for the steps
+			resetMotorStep();
+			checkMovement();
+		}
+		else {
+
+			if (moveTicks == stepOnTick) {
+
+				// Positive flank for the steps
+				setStepAxis();
+			}
+		}
+	}
+}
+
+void StepperControlAxis::setStepAxis() {
+
+	if (coordHomeAxis && coordCurrentPoint == 0) {
+
+		// Keep moving toward end stop even when position is zero
+		// but end stop is not yet active
+		if (motorHomeIsUp) {
+			coordCurrentPoint = -1;
+		} else {
+			coordCurrentPoint =  1;
+		}
+	}
+
+	if (coordCurrentPoint < coordDestinationPoint) {
+		coordCurrentPoint++;
+	} else if (coordCurrentPoint > coordDestinationPoint) {
+		coordCurrentPoint--;
+	}
+
+	// set a step on the motors
+	setMotorStep();
+
+	// if the home end stop is reached, set the current position
+	if (endStopAxisReached(false))
+	{
+		coordCurrentPoint = 0;
+	}
+}
+
+bool StepperControlAxis::endStopAxisReached(bool movement_forward) {
+
+	bool min_endstop = false;
+	bool max_endstop = false;
+	bool invert = false;
+
+	if (motorEndStopInv) {
+		invert = true;
+	}
+
+	// for the axis to check, retrieve the end stop status
+
+	if (!invert) {
+		min_endstop = endStopMin();
+		max_endstop = endStopMax();
+	} else {
+		min_endstop = endStopMax();
+		max_endstop = endStopMin();
+	}
+
+	// if moving forward, only check the end stop max
+	// for moving backwards, check only the end stop min
+
+	if((!movement_forward && min_endstop) || (movement_forward && max_endstop)) {
+		return 1;
+	}
+	return 0;
+}
+
+void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, int enable, int min, int max) {
+        pinStep		= step;
+        pinDirection	= dir;
+        pinEnable	= enable;
+        pinMin		= min;
+        pinMax		= max;
+}
+
+
+void StepperControlAxis::loadMotorSettings(long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed) {
+        motorSpeedMax		= speedMax;
+        motorSpeedMin		= speedMin;
+        motorStepsAcc		= stepsAcc;
+        motorTimeOut		= timeOut;
+        motorHomeIsUp		= homeIsUp;
+        motorMotorInv		= motorInv;
+        motorEndStopInv		= endStInv;
+	motorInterruptSpeed 	= interruptSpeed;
+}
+
+void StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home) {
+
+	coordSourcePoint	= sourcePoint;
+        coordCurrentPoint	= sourcePoint;
+	coordDestinationPoint	= destinationPoint;
+        coordHomeAxis		= home;
+
+        // Limit normal movmement to the home position
+        if (!motorHomeIsUp && coordDestinationPoint < 0) {
+        	coordDestinationPoint = 0;
+        }
+
+        if ( motorHomeIsUp && coordDestinationPoint > 0) {
+        	coordDestinationPoint = 0;
+        }
+
+	// Initialize movement variables
+	moveTicks		= 0;
+	axisActive		= true;
+}
+
+void StepperControlAxis::enableMotor() {
+	digitalWrite(pinEnable, LOW);
+}
+
+void StepperControlAxis::disableMotor() {
+	digitalWrite(pinEnable, HIGH);
+}
+
+void StepperControlAxis::setDirectionUp() {
+	if (motorMotorInv) {
+		digitalWrite(pinDirection, LOW);
+	} else {
+		digitalWrite(pinDirection, HIGH);
+	}
+}
+
+void StepperControlAxis::setDirectionDown() {
+	if (motorMotorInv) {
+		digitalWrite(pinDirection, HIGH);
+	} else {
+		digitalWrite(pinDirection, LOW);
+	}
+}
+
+void StepperControlAxis::setDirectionHome() {
+	if (motorHomeIsUp) {
+		setDirectionUp();
+	} else {
+		setDirectionDown();
+	}
+}
+
+void StepperControlAxis::setDirectionAway() {
+	if (motorHomeIsUp) {
+		setDirectionDown();
+	} else {
+		setDirectionUp();
+	}
+}
+
+unsigned long StepperControlAxis::getLength(long l1, long l2) {
+	if (l1 > l2) {
+		return l1 - l2;
+	} else {
+		return l2 - l1;
+	}
+}
+
+bool StepperControlAxis::endStopsReached() {
+	return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
+}
+
+bool StepperControlAxis::endStopMin() {
+	//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
+	return digitalRead(pinMin);
+}
+
+bool StepperControlAxis::endStopMax() {
+	//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
+        return digitalRead(pinMax);
+}
+
+bool StepperControlAxis::isAxisActive() {
+	return axisActive;
+}
+
+void StepperControlAxis::setMotorStep() {
+	digitalWrite(pinStep, HIGH);
+}
+
+void StepperControlAxis::resetMotorStep() {
+	digitalWrite(pinStep, LOW);
+}
+
+bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint) {
+	return (destinationPoint == currentPoint);
+}
+
+long StepperControlAxis::currentPoint() {
+	return coordCurrentPoint;
+}
+
+void StepperControlAxis::setMaxSpeed(long speed) {
+	motorSpeedMax = speed;
+}
diff --git a/src/StepperControlAxis.h b/src/StepperControlAxis.h
new file mode 100644
index 0000000..70cad48
--- /dev/null
+++ b/src/StepperControlAxis.h
@@ -0,0 +1,103 @@
+/*
+ * StepperControlAxis.h
+ *
+ *  Created on: 18 juli 2015
+ *      Author: Tim Evers
+ */
+
+#ifndef STEPPERCONTROLAXIS_H_
+#define STEPPERCONTROLAXIS_H_
+
+#include "Arduino.h"
+#include "CurrentState.h"
+#include "ParameterList.h"
+#include "pins.h"
+#include "Config.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+class StepperControlAxis {
+
+public:
+
+	StepperControlAxis();
+
+	void loadPinNumbers(int step, int dir, int enable, int min, int max);
+	void loadMotorSettings(	long speedMax, long speedMin, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, long interruptSpeed);
+	void loadCoordinates(long sourcePoint, long destinationPoint, bool home);
+	void setMaxSpeed(long speed);
+
+	void enableMotor();
+	void disableMotor();
+	void checkMovement();
+	void checkTiming();
+
+	bool isAxisActive();
+	bool endStopMin();
+	bool endStopMax();
+	bool endStopsReached();
+	bool endStopAxisReached(bool movement_forward);
+	long currentPoint();
+
+	void setMotorStep();
+	void resetMotorStep();
+
+	void setDirectionUp();
+	void setDirectionDown();
+	void setDirectionHome();
+	void setDirectionAway();
+	void setDirectionAxis();
+
+	void test();
+
+	char label;
+
+private:
+
+	int lastCalcLog = 0;
+
+	// pin settings
+	int pinStep;
+	int pinDirection;
+	int pinEnable;
+	int pinMin;
+	int pinMax;
+
+	// motor settings
+	long motorSpeedMax;		// maximum speed in steps per second
+	long motorSpeedMin;		// minimum speed in steps per second
+	long motorStepsAcc;		// number of steps used for acceleration
+	long motorTimeOut;		// timeout in seconds
+	bool motorHomeIsUp;		// direction to move when reached 0 on axis but no end stop detected while homing
+	bool motorMotorInv;		// invert motot direction
+	bool motorEndStopInv;		// invert input (true/false) of end stops
+	long motorInterruptSpeed;	// period of interrupt in micro seconds
+
+	// coordinates
+	long coordSourcePoint;		// all coordinated in steps
+	long coordCurrentPoint;
+	long coordDestinationPoint;
+	bool coordHomeAxis;		// homing command
+
+	// movement handling
+	unsigned long movementLength;
+	unsigned long maxLenth;
+	unsigned long moveTicks;
+	unsigned long stepOnTick;
+	unsigned long stepOffTick;
+	unsigned long axisSpeed;
+	bool axisActive;
+	bool movementUp;
+	bool movementToHome;
+
+	void setStepAxis();
+	void step(long &currentPoint, unsigned long steps, long destinationPoint);
+	bool pointReached(long currentPoint, long destinationPoint);
+	unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec);
+	unsigned long getLength(long l1, long l2);
+	void checkAxisDirection();
+
+};
+
+#endif /* STEPPERCONTROLAXIS_H_ */
+