diff --git a/StepperTest/StepperTest.ino b/StepperTest/StepperTest.ino
index 6aabf70..c42cbf8 100644
--- a/StepperTest/StepperTest.ino
+++ b/StepperTest/StepperTest.ino
@@ -7,8 +7,8 @@
 
 // Stepper motor speed configuration
 #define microStepsFactor 4
-#define maxStepperSpeed 1000 * microStepsFactor
-#define inversionPause 20
+#define maxStepperSpeed 4000
+#define inversionPause 50
 
 // Create a new instance of the AccelStepper class:
 AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
@@ -16,6 +16,7 @@ AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
 // Runtime data
 int currentPosition[] = {-1, -1, -1};
 int nextPosition[] = {0, 0, 0};
+int stepperPosition = 0;
 
 void setup() {
   Serial.begin(115200);
@@ -29,6 +30,7 @@ void setup() {
   stepper.setCurrentPosition(0);
   // Set the maximum speed in steps per second:
   stepper.setMaxSpeed(maxStepperSpeed);
+  stepper.setAcceleration(40000);
 
   // Rotate to 0,0,0
   setPosition();
@@ -72,22 +74,24 @@ void setPosition() {
   Serial.print(",");
   Serial.println(currentPosition[2]);
 
-  if (nextPosition[2] != nextPosition[2]) {
+  if (currentPosition[2] != nextPosition[2]) {
+    rotateCcwTo(nextPosition[2]);
     rotateCcwTo(nextPosition[2]);
     rotateCcwTo(nextPosition[2]);
     rotateCcwTo(nextPosition[2]);
   }
 
-  if (nextPosition[1] != nextPosition[1]) {
+  if (currentPosition[1] != nextPosition[1]) {
+    rotateCwTo(nextPosition[1]);
     rotateCwTo(nextPosition[1]);
     rotateCwTo(nextPosition[1]);
   }
 
-  if (nextPosition[0] != nextPosition[0]) {
+  if (currentPosition[0] != nextPosition[0]) {
+    rotateCcwTo(nextPosition[0]);
     rotateCcwTo(nextPosition[0]);
   }
 
-  rotateCwTo(0);
   rotateCwTo(10);
 
   currentPosition[0] = nextPosition[0];
@@ -96,11 +100,27 @@ void setPosition() {
 }
 
 void rotateCwTo(int pos) {
-  stepper.move(1);
-  stepper.runToNewPosition(2 * pos * microStepsFactor);
+  Serial.print("CW to ");
+  Serial.println(pos);
+  if (stepperPosition == pos) {
+    // make sure we actually move one whole turn
+    stepper.move(-200 * microStepsFactor);
+    stepper.runToPosition();
+  } else {
+    stepper.runToNewPosition(-2 * pos * microStepsFactor);
+    stepperPosition = pos;
+  }
 }
 
 void rotateCcwTo(int pos) {
-  stepper.move(-1);
-  stepper.runToNewPosition(-2 * pos * microStepsFactor);
+  Serial.print("CCW to ");
+  Serial.println(pos);
+  if (stepperPosition == pos) {
+    // make sure we actually move one whole turn
+    stepper.move(200 * microStepsFactor);
+    stepper.runToPosition();
+  } else {
+    stepper.runToNewPosition(2 * pos * microStepsFactor);
+    stepperPosition = pos;
+  }
 }