From eea3ba5588580b4460b3f27da549bcf52db5aad6 Mon Sep 17 00:00:00 2001
From: Cameron Lai <cameron.lai@makible.com>
Date: Thu, 24 Apr 2014 12:56:06 +0800
Subject: [PATCH] Fix for stepper microstepping

---
 Marlin/stepper.cpp | 55 ++++++++++++++++++++++++++++++++--------------
 1 file changed, 39 insertions(+), 16 deletions(-)

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index eaba4362d..8f8be8c82 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -1228,34 +1228,55 @@ void digipot_current(uint8_t driver, int current)
 
 void microstep_init()
 {
+  #if defined(Y_MS1_PIN) && Y_MS1_PIN > -1
+  SET_OUTPUT(Y_MS1_PIN);
+  SET_OUTPUT(Y_MS2_PIN);
+  #endif
+  #if defined(Z_MS1_PIN) && Z_MS1_PIN > -1
+  SET_OUTPUT(Z_MS1_PIN);
+  SET_OUTPUT(Z_MS2_PIN);
+  #endif
+
+  #if defined(E0_MS1_PIN) && E0_MS1_PIN > -1
+  SET_OUTPUT(E0_MS1_PIN);
+  SET_OUTPUT(E0_MS2_PIN);
+  #endif
+  
+  #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
+  SET_OUTPUT(E1_MS1_PIN);
+  SET_OUTPUT(E1_MS2_PIN);
+  #endif
+  
   #if defined(X_MS1_PIN) && X_MS1_PIN > -1
+  SET_OUTPUT(X_MS1_PIN);
+  SET_OUTPUT(X_MS2_PIN);
+  
   const uint8_t microstep_modes[] = MICROSTEP_MODES;
-  pinMode(X_MS2_PIN,OUTPUT);
-  pinMode(Y_MS2_PIN,OUTPUT);
-  pinMode(Z_MS2_PIN,OUTPUT);
-  pinMode(E0_MS2_PIN,OUTPUT);
-  pinMode(E1_MS2_PIN,OUTPUT);
   for(int i=0;i<=4;i++) microstep_mode(i,microstep_modes[i]);
-  #endif
+  #endif  
 }
 
 void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
 {
   if(ms1 > -1) switch(driver)
   {
-    case 0: digitalWrite( X_MS1_PIN,ms1); break;
-    case 1: digitalWrite( Y_MS1_PIN,ms1); break;
-    case 2: digitalWrite( Z_MS1_PIN,ms1); break;
-    case 3: digitalWrite(E0_MS1_PIN,ms1); break;
-    case 4: digitalWrite(E1_MS1_PIN,ms1); break;
+    case 0: WRITE( X_MS1_PIN,ms1); break;
+    case 1: WRITE( Y_MS1_PIN,ms1); break;
+    case 2: WRITE( Z_MS1_PIN,ms1); break;
+    case 3: WRITE(E0_MS1_PIN,ms1); break;
+    #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
+    case 4: WRITE(E1_MS1_PIN,ms1); break;
+    #endif
   }
   if(ms2 > -1) switch(driver)
   {
-    case 0: digitalWrite( X_MS2_PIN,ms2); break;
-    case 1: digitalWrite( Y_MS2_PIN,ms2); break;
-    case 2: digitalWrite( Z_MS2_PIN,ms2); break;
-    case 3: digitalWrite(E0_MS2_PIN,ms2); break;
-    case 4: digitalWrite(E1_MS2_PIN,ms2); break;
+    case 0: WRITE( X_MS2_PIN,ms2); break;
+    case 1: WRITE( Y_MS2_PIN,ms2); break;
+    case 2: WRITE( Z_MS2_PIN,ms2); break;
+    case 3: WRITE(E0_MS2_PIN,ms2); break;
+    #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
+    case 4: WRITE(E1_MS2_PIN,ms2); break;
+    #endif
   }
 }
 
@@ -1286,8 +1307,10 @@ void microstep_readings()
       SERIAL_PROTOCOLPGM("E0: ");
       SERIAL_PROTOCOL(   digitalRead(E0_MS1_PIN));
       SERIAL_PROTOCOLLN( digitalRead(E0_MS2_PIN));
+      #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
       SERIAL_PROTOCOLPGM("E1: ");
       SERIAL_PROTOCOL(   digitalRead(E1_MS1_PIN));
       SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
+      #endif
 }