diff --git a/StepperTest/StepperTest.ino b/StepperTest/StepperTest.ino
index c42cbf8..86d3912 100644
--- a/StepperTest/StepperTest.ino
+++ b/StepperTest/StepperTest.ino
@@ -75,34 +75,39 @@ void setPosition() {
   Serial.println(currentPosition[2]);
 
   if (currentPosition[2] != nextPosition[2]) {
-    rotateCcwTo(nextPosition[2]);
-    rotateCcwTo(nextPosition[2]);
-    rotateCcwTo(nextPosition[2]);
-    rotateCcwTo(nextPosition[2]);
+    rotateCcwTo(nextPosition[2], false);
+    rotateCcwTo(nextPosition[2], true);
+    rotateCcwTo(nextPosition[2], true);
+    rotateCcwTo(nextPosition[2], true);
+    currentPosition[1] = -1;
   }
 
   if (currentPosition[1] != nextPosition[1]) {
-    rotateCwTo(nextPosition[1]);
-    rotateCwTo(nextPosition[1]);
-    rotateCwTo(nextPosition[1]);
+    rotateCwTo(nextPosition[1], false);
+    rotateCwTo(nextPosition[1], true);
+    rotateCwTo(nextPosition[1], true);
+    currentPosition[0] = -1;
   }
 
   if (currentPosition[0] != nextPosition[0]) {
-    rotateCcwTo(nextPosition[0]);
-    rotateCcwTo(nextPosition[0]);
+    rotateCcwTo(nextPosition[0], false);
+    rotateCcwTo(nextPosition[0], true);
   }
 
-  rotateCwTo(10);
+  if (stepperPosition <= 10) {
+    rotateCwTo(0, true);
+  }
+  rotateCwTo(10, false);
 
   currentPosition[0] = nextPosition[0];
   currentPosition[1] = nextPosition[1];
   currentPosition[2] = nextPosition[2];
 }
 
-void rotateCwTo(int pos) {
+void rotateCwTo(int pos, boolean forceFullRotation) {
   Serial.print("CW to ");
   Serial.println(pos);
-  if (stepperPosition == pos) {
+  if (stepperPosition == pos && forceFullRotation) {
     // make sure we actually move one whole turn
     stepper.move(-200 * microStepsFactor);
     stepper.runToPosition();
@@ -112,10 +117,10 @@ void rotateCwTo(int pos) {
   }
 }
 
-void rotateCcwTo(int pos) {
+void rotateCcwTo(int pos, boolean forceFullRotation) {
   Serial.print("CCW to ");
   Serial.println(pos);
-  if (stepperPosition == pos) {
+  if (stepperPosition == pos && forceFullRotation) {
     // make sure we actually move one whole turn
     stepper.move(200 * microStepsFactor);
     stepper.runToPosition();