diff --git a/code/cane_control/cane_control.ino b/code/cane_control/cane_control.ino
index a4f312bc8117ce8655b33d183549198576c18577..dd0ebe35e23f3cd2c6de26a1393f667393123957 100644
--- a/code/cane_control/cane_control.ino
+++ b/code/cane_control/cane_control.ino
@@ -1,7 +1,7 @@
 #include <TMC2209.h>
 #include <AccelStepper.h>
 #include <Adafruit_SSD1306.h>
- 
+
 #define PIN_STEP1 28
 #define PIN_DIR1 29
 #define PIN_STEP2 26
@@ -22,13 +22,23 @@
 
 #define SERIAL_BAUD_RATE 115200
 #define RUN_CURRENT_PERCENT 80
-#define STALL_GUARD_THRESHOLD 80
+#define STALL_GUARD_THRESHOLD 70
 #define HOMING_VELOCITY 400
 
 #define MOVE_SPEED 800
 #define MOVE_ACC 1200
 
 #define MICROSTEPS 4
+#define MICROSTEPS_PER_TURN (200L*MICROSTEPS)
+
+#define ANGLE_INIT 16
+#define ANGLE_READY 340
+
+#define STATE_MENU 0
+#define STATE_COCKING 1
+#define STATE_FEEDING 2
+
+int machine_state = STATE_MENU;
 
 HardwareSerial & serial_stream = Serial1;
 
@@ -48,14 +58,23 @@ int lookup_move[] = {0,-1,1,-2,1,0,2,-1,-1,2,0,1,-2,1,-1,0};
 float length = 5.00;
 
 int counter = 0;
-int active = false;
+int cutting = false;
 bool update_screen = true;
 
 int k = 0;
 
+long int length_to_step(float d) {
+  // 1 inch per revolution
+  return MICROSTEPS_PER_TURN * d / 3.14159;
+}
+
+long int angle_to_step(int angle) {
+  return 2 * MICROSTEPS_PER_TURN * angle / 360;
+}
+
 inline void update_knob() {
   int key = (state_now << 2) | state_prev;
-  if (!active) {
+  if (machine_state == STATE_MENU) {
     k += lookup_move[key];
     if (k >= 4) {
       length += 0.05;
@@ -89,14 +108,21 @@ void homing() {
   while (!done) {
     uint16_t stall_guard_result = stepper_driver2.getStallGuardResult();
     done = stall_guard_result < STALL_GUARD_THRESHOLD;
+    Serial.println(stall_guard_result);
   }
   stepper_driver2.moveAtVelocity(0);
   stepper_driver2.enable();
+
+  stepper2.moveTo(angle_to_step(ANGLE_INIT));
+  stepper2.runToPosition();
+  stepper2.setCurrentPosition(0);
 }
 
 void setup() {
   Serial.begin(0);
 
+  delay(2000);
+
   pinMode(PIN_ENC_A, INPUT);
   pinMode(PIN_ENC_B, INPUT);
   pinMode(PIN_BTN, INPUT_PULLUP);
@@ -112,7 +138,7 @@ void setup() {
   stepper_driver1.setup(serial_stream, SERIAL_BAUD_RATE, TMC2209::SERIAL_ADDRESS_1);
   stepper_driver1.setRunCurrent(RUN_CURRENT_PERCENT);
   stepper_driver1.setMicrostepsPerStep(MICROSTEPS);
-  
+
   delay(50);
 
   stepper_driver2.setup(serial_stream, SERIAL_BAUD_RATE, TMC2209::SERIAL_ADDRESS_0);
@@ -127,14 +153,15 @@ void setup() {
   display.print("Homing...");
   display.display();
 
-  homing();
-
   stepper1.setMaxSpeed(MOVE_SPEED);
   stepper1.setAcceleration(MOVE_ACC);
+  stepper1.setPinsInverted(true);
 
   stepper2.setMaxSpeed(MOVE_SPEED);
   stepper2.setAcceleration(MOVE_ACC);
 
+  homing();
+
   state_now = READ_FAST(PIN_ENC_A) | (READ_FAST(PIN_ENC_B) << 1);
   state_prev = state_now;
 
@@ -144,25 +171,51 @@ void setup() {
 
 void loop() {
   static int btn_pressed = false;
-  
-  int btn_pressed_now = !digitalRead(PIN_BTN);
-
-  if (btn_pressed_now && !btn_pressed) {
-    active = !active;
-    update_screen = true;
-    if (active) {
-      stepper_driver1.enable();
-    } else {
-      stepper_driver1.disable();
-    }
-  }
 
-  if (active) {
-    stepper1.moveTo(800);
-    stepper1.runToPosition();
-    stepper1.setCurrentPosition(0);
-    active = false;
-    update_screen = true;
+  int btn_pressed_now = !digitalRead(PIN_BTN);
+  bool just_pressed = btn_pressed_now && !btn_pressed;
+
+  stepper1.run();
+  stepper2.run();
+
+  switch machine_state {
+    case STATE_MENU:
+      if (just_pressed) {
+        // prepare
+        machine_state = STATE_COCKING;
+        stepper_driver1.enable();
+        stepper2.moveTo(angle_to_step(ANGLE_READY));
+        update_screen = true;
+      }
+      break;
+    case STATE_COCKING:
+      if (just_pressed) {
+        machine_state = STATE_MENU;
+        stepper_driver1.disable();
+        update_screen = true;
+      } else if (!stepper2.isRunning()) {
+        machine_state = STATE_FEEDING;
+        stepper1.moveTo(lengt_to_step(length));
+      }
+      break;
+    case STATE_FEEDING:
+      if (just_pressed) {
+        machine_state = STATE_MENU;
+        stepper_driver1.disable();
+        update_screen = true;
+      } else if (!stepper1.isRunning()) {
+        // chop!
+        stepper2.moveTo(angle_to_step(360));
+        stepper2.runToPosition();
+        stepper2.setCurrentPosition(0);
+        counter++;
+        update_screen = true;
+
+        // prepare
+        machine_state = STATE_COCKING;
+        stepper2.moveTo(angle_to_step(ANGLE_READY));
+      }
+      break;
   }
 
   if (update_screen) {
@@ -175,12 +228,11 @@ void loop() {
     display.printf("Counter: %7d cuts\n", counter);
 
     display.setCursor(0, 40);
-    display.println(active ? "RUNNING..." : "STOPPED");
+    display.println(cutting ? "RUNNING..." : "STOPPED");
     display.setCursor(0, 56);
-    display.println(active ? "[ press to stop ]" : "[ press to start ]");
+    display.println(cutting ? "[ press to stop ]" : "[ press to start ]");
     display.display();
   }
 
-  delay(1);
   btn_pressed = btn_pressed_now;
 }