     step_count += 1;
     step_t = tu;
   }
+#endif
+
+
+
+#if 1
+  //A4983
+  //One revolution, then wait one cycle
+  if (tu - step_t > step_wait) {
+    //One fullstep lap (two toggles per step)
+    if (step_count > mode_div*400) {
+      digitalWrite(A4983_ENABLE, !digitalRead(A4983_ENABLE));
+      step_count = 0;
+
+      if (digitalRead(A4983_ENABLE) == LOW) {
+        //Change step mode
+        if (mode == 4) mode = 0;
+        else mode += 1;
+  
+        if (mode == 0) {
+          digitalWrite(A4983_MODE0, LOW);
+          digitalWrite(A4983_MODE1, LOW);
+          digitalWrite(A4983_MODE2, LOW);
+          mode_div = 1;
+          step_wait = 1000;
+        } else if (mode == 1) {
+          digitalWrite(A4983_MODE0, HIGH);
+          digitalWrite(A4983_MODE1, LOW);
+          digitalWrite(A4983_MODE2, LOW);
+          mode_div = 2;
+          step_wait = 500;
+        } else if (mode == 2) {
+          digitalWrite(A4983_MODE0, LOW);
+          digitalWrite(A4983_MODE1, HIGH);
+          digitalWrite(A4983_MODE2, LOW);
+          mode_div = 4;
+          step_wait = 250;
+        } else if (mode == 3) {
+          digitalWrite(A4983_MODE0, HIGH);
+          digitalWrite(A4983_MODE1, HIGH);
+          digitalWrite(A4983_MODE2, LOW);
+          mode_div = 8;
+          step_wait = 125;
+        } else {
+          digitalWrite(A4983_MODE0, HIGH);
+          digitalWrite(A4983_MODE1, HIGH);
+          digitalWrite(A4983_MODE2, HIGH);
+          mode_div = 16;
+          step_wait = 62;
+        }
+      }
+    }
+    
+    if (digitalRead(A4983_ENABLE) == LOW) {
+      digitalWrite(A4983_STEP, !digitalRead(A4983_STEP));
+    }
+    step_count += 1;
+    step_t = tu;
+  }
+#endif
 
   //Blink every second
   if (tm - blink_t > 1000) {

#define DRV8825_MODE2 5
#define DRV8825_ENABLE 6


void setup() {
  Serial.begin(9600);

  //DRV8825 pin config
  pinMode(13, OUTPUT);
  pinMode(DRV8825_STEP, OUTPUT);
  pinMode(DRV8825_MODE0, OUTPUT);
  pinMode(DRV8825_MODE1, OUTPUT);
  pinMode(DRV8825_MODE2, OUTPUT);
  pinMode(DRV8825_ENABLE, OUTPUT);

  digitalWrite(DRV8825_STEP, LOW);
  digitalWrite(DRV8825_MODE0, LOW);
  digitalWrite(DRV8825_MODE1, LOW);
  digitalWrite(DRV8825_MODE2, LOW);
  digitalWrite(DRV8825_ENABLE, LOW);

  //Timer variables for steps
  blink_t = millis();

    Serial.write(Serial.read());
  }

#if 0
  //DRV8825
  //One revolution, then wait one cycle
  if (tu - step_t > step_wait) {
    //One fullstep lap (two toggles per step)
