diff --git a/main.py b/main.py
index 7eff2f1..912ba65 100644
--- a/main.py
+++ b/main.py
@@ -18,6 +18,7 @@
  
  Shaft diameter is 5mm with a 3mm inner key thing. Mounting holes are 35mm apart.  """
 
+import machine
 from machine import Pin
 from machine import Timer
 import math
@@ -26,7 +27,7 @@ import time
 NUM_PINS = 4
 NUM_STEPS = 8
 RADS_PER_SEC = 7.292115e-05
-LENGTH_CM = 28.884 # fill in with precise measured value
+LENGTH_CM = 28.884  # fill in with precise measured value
 # For theta zero, I used relative measurement between two boards w/ level.
 # Got 0.72 degrees, which is 0.012566 radians
 THETAO = 0.012566
@@ -40,29 +41,29 @@ REWINDING = 1
 STOPPED = 2
 
 motorPins = [
-    Pin(16, Pin.out), # D0
-    Pin(5, Pin.out), # D1
-    Pin(4, Pin.out), #D2
-    Pin(14, Pin.out) #D5
+    Pin(16, Pin.out),  # D0
+    Pin(5, Pin.out),  # D1
+    Pin(4, Pin.out),  # D2
+    Pin(14, Pin.out)  # D5
 ]
 
-MODE_PIN = Pin(13, Pin.out) #D7
+MODE_PIN = Pin(13, Pin.out)  # D7
 
 STEPPER_SEQUENCE = [
-    [1,0,0,1],
-    [1,0,0,0],
-    [1,1,0,0],
-    [0,1,0,0],
-    [0,1,1,0],
-    [0,0,1,0],
-    [0,0,1,1],
-    [0,0,0,1]
+    [1, 0, 0, 1],
+    [1, 0, 0, 0],
+    [1, 1, 0, 0],
+    [0, 1, 0, 0],
+    [0, 1, 1, 0],
+    [0, 0, 1, 0],
+    [0, 0, 1, 1],
+    [0, 0, 0, 1]
 ]
 
 step_delta = 0
 step_num = 0
 total_seconds = 0.0
-totalsteps  = 0
+totalsteps = 0
 step_interval_s = 0.001
 current_step = 0
 next = 0
@@ -75,7 +76,8 @@ autostop = True
 def ypt(ts):
     # Bolt insertion rate in cm/s: y'(t)
     # Note, if you run this for ~359 minutes, it goes to infinity!!
-    return LENGTH_CM * RADS_PER_SEC/math.pow(math.cos(THETAO + RADS_PER_SEC * ts),2)
+    return LENGTH_CM * RADS_PER_SEC/math.pow(math.cos(THETAO + RADS_PER_SEC * ts), 2)
+
 
 def step_motor():
     """
@@ -86,27 +88,73 @@ def step_motor():
     """
     pass
 
+
 def do_step(current_step):
     # apply a single step of the stepper motor on its pins.
-    pass
+    for i in NUM_PINS:
+        if current_step[i] is 1:
+            Pin(motorPins[i], Pin.HIGH)
+        else
+            Pin(motorPins[i], Pin.LOW )
+
+
 def setup():
-    pass
+    setup_gpio()
+    setup_timer()
+    
+    buttonUp = Pin(MODE_PIN)
+    if not buttonUp:
+        print("Manual REWIND")
+        autostop = False
+        current_mode = REWINDING
+
+
 def setup_timer():
-    pass
+    machine.disable_irq()
+    timer = Timer(2)
+    timer.init(freq=1000)
+    timer.callback(lambda t:step_motor())
+    machine.enable_irq()
+
+
 def setup_gpio():
     for i in NUM_PINS:
         Pin(motorPins[i], Pin.OUT)
     all_pins_off()
     button = Pin(MODE_PIN, Pin.IN)
     button.irq(trigger=Pin.IRQ_FALLING, handler=toggle_mode)
-    pass
+
+
 def all_pins_off():
     for i in NUM_PINS:
         Pin(motorPins[i], Pin.low())
 
+
 def toggle_mode():
     # We have several modes that we can toggle between with a button,
     # NORMAL, REWIND, and STOPPED.
+    # Need to find replacement for ESP.getCycleCount();
+    #if ESP.getCycleCount() - last_toggle < 0.2*CYCLES_PER_SECOND:
+    #    return 
+    if current_mode is REWINDING:
+        print("STOPPING")
+        current_mode = STOPPED
+        all_pins_off()
+        if not autostop:
+            step_num = 0
+            total_seconds = 0
+            totalsteps = 0
+            autostop = True
+    elif current_mode is NORMAL:
+        print("REWINDING")
+        current_mode = REWINDING
+    else:
+        print("RESTARTING")
+        current_mode = NORMAL
+    last_toggle = 0# FIND REPLACEMENT FOR ESP.getCycleCount();
+        
     pass
+
+
 def loop():
-    pass
\ No newline at end of file
+    pass