diff --git a/StepperTest/StepperTest.ino b/StepperTest/StepperTest.ino
index 355e5e8..6aabf70 100644
--- a/StepperTest/StepperTest.ino
+++ b/StepperTest/StepperTest.ino
@@ -1,36 +1,106 @@
-/*Example sketch to control a stepper motor with DRV8825 stepper motor driver, AccelStepper library and Arduino: number of steps or revolutions. More info: https://www.makerguides.com */
-
-// Include the AccelStepper library:
 #include <AccelStepper.h>
 
-// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver:
+// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver
+#define motorInterfaceType 1
 #define dirPin 2
 #define stepPin 3
-#define motorInterfaceType 1
+
+// Stepper motor speed configuration
 #define microStepsFactor 4
+#define maxStepperSpeed 1000 * microStepsFactor
+#define inversionPause 20
 
 // Create a new instance of the AccelStepper class:
 AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
 
-int dir = 1;
+// Runtime data
+int currentPosition[] = {-1, -1, -1};
+int nextPosition[] = {0, 0, 0};
 
 void setup() {
-  // Set the maximum speed in steps per second:
-  stepper.setMaxSpeed(1000*microStepsFactor);
+  Serial.begin(115200);
   
+  // Wait for zeroing
+  Serial.println("Please rotate to zero position and press Enter");
+  while (Serial.available() == 0) {}
+  Serial.flush();
+  
+  // Set the current position to 0:
+  stepper.setCurrentPosition(0);
+  // Set the maximum speed in steps per second:
+  stepper.setMaxSpeed(maxStepperSpeed);
+
+  // Rotate to 0,0,0
+  setPosition();
 }
 
 void loop() { 
-  // Set the current position to 0:
-  stepper.setCurrentPosition(0);
 
-  int revs = random(1,5);
-  dir = -dir;
-  // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
-  while(stepper.currentPosition() != 200 * microStepsFactor * revs * dir) {
-    stepper.setSpeed(1000 * microStepsFactor * dir);
-    stepper.runSpeed();
+  incrementCombination();
+  setPosition();
+
+  delay(inversionPause);
+}
+
+void incrementCombination() {
+  if (currentPosition[0] < 99) {
+    nextPosition[0] = currentPosition[0] + 1;
+    nextPosition[1] = currentPosition[1];
+    nextPosition[2] = currentPosition[2];
+  } else {
+    nextPosition[0] = 0;
+    if (currentPosition[1] < 99) {
+      nextPosition[1] = currentPosition[1] + 1;
+      nextPosition[2] = currentPosition[2];
+    } else {
+      nextPosition[1] = 0;
+      if (currentPosition[2] < 99) {
+        nextPosition[2] = currentPosition[2] + 1;
+      } else {
+        nextPosition[2] = 0;        
+      }
+    }
+  }
+}
+
+void setPosition() {
+
+  Serial.print("Testing ");
+  Serial.print(currentPosition[0]);
+  Serial.print(",");
+  Serial.print(currentPosition[1]);
+  Serial.print(",");
+  Serial.println(currentPosition[2]);
+
+  if (nextPosition[2] != nextPosition[2]) {
+    rotateCcwTo(nextPosition[2]);
+    rotateCcwTo(nextPosition[2]);
+    rotateCcwTo(nextPosition[2]);
   }
 
-  delay(200);
+  if (nextPosition[1] != nextPosition[1]) {
+    rotateCwTo(nextPosition[1]);
+    rotateCwTo(nextPosition[1]);
+  }
+
+  if (nextPosition[0] != nextPosition[0]) {
+    rotateCcwTo(nextPosition[0]);
+  }
+
+  rotateCwTo(0);
+  rotateCwTo(10);
+
+  currentPosition[0] = nextPosition[0];
+  currentPosition[1] = nextPosition[1];
+  currentPosition[2] = nextPosition[2];
+}
+
+void rotateCwTo(int pos) {
+  stepper.move(1);
+  stepper.runToNewPosition(2 * pos * microStepsFactor);
+}
+
+void rotateCcwTo(int pos) {
+  stepper.move(-1);
+  stepper.runToNewPosition(-2 * pos * microStepsFactor);
 }