diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 85fd38968..6b5e19a3d 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -28,7 +28,8 @@
 // Gen6 = 5,
 // Sanguinololu 1.2 and above = 62
 // Ultimaker = 7,
-// Teensylu = 8
+// Teensylu = 8,
+// Gen3+ =9
 #define MOTHERBOARD 7
 
 //===========================================================================
@@ -201,6 +202,7 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 #define DISABLE_Y false
 #define DISABLE_Z false
 #define DISABLE_E false // For all extruders
+//#define Z_LATE_ENABLE // Enable Z the last moment. Needed if your Z driver overheats.
 
 // Inverting axis direction
 //#define INVERT_X_DIR false    // for Mendel set to false, for Orca set to true
diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde
index bbef8170d..57eb61b8e 100644
--- a/Marlin/Marlin.pde
+++ b/Marlin/Marlin.pde
@@ -327,7 +327,7 @@ void loop()
 }
 
 
-FORCE_INLINE void get_command() 
+void get_command() 
 { 
   while( MSerial.available() > 0  && buflen < BUFSIZE) {
     serial_char = MSerial.read();
@@ -474,20 +474,20 @@ FORCE_INLINE void get_command()
 }
 
 
-FORCE_INLINE float code_value() 
+float code_value() 
 { 
   return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); 
 }
-FORCE_INLINE long code_value_long() 
+long code_value_long() 
 { 
   return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); 
 }
-FORCE_INLINE bool code_seen(char code_string[]) //Return True if the string was found
+bool code_seen(char code_string[]) //Return True if the string was found
 { 
   return (strstr(cmdbuffer[bufindr], code_string) != NULL); 
 }  
 
-FORCE_INLINE bool code_seen(char code)
+bool code_seen(char code)
 {
   strchr_pointer = strchr(cmdbuffer[bufindr], code);
   return (strchr_pointer != NULL);  //Return True if a character was found
@@ -519,7 +519,7 @@ FORCE_INLINE bool code_seen(char code)
     endstops_hit_on_purpose();\
   }
 
-FORCE_INLINE void process_commands()
+void process_commands()
 {
   unsigned long codenum; //throw away variable
   char *starpos = NULL;
@@ -1235,7 +1235,7 @@ void ClearToSend()
   SERIAL_PROTOCOLLNPGM("ok"); 
 }
 
-FORCE_INLINE void get_coordinates()
+void get_coordinates()
 {
   for(int8_t i=0; i < NUM_AXIS; i++) {
     if(code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
@@ -1247,7 +1247,7 @@ FORCE_INLINE void get_coordinates()
   }
 }
 
-FORCE_INLINE void get_arc_coordinates()
+void get_arc_coordinates()
 {
    get_coordinates();
    if(code_seen('I')) offset[0] = code_value();
diff --git a/Marlin/pins.h b/Marlin/pins.h
index 28e349161..02f26150a 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -470,7 +470,7 @@
 #define X_STEP_PIN         15
 #define X_DIR_PIN          21
 #define X_MIN_PIN          18
-#define X_MAX_PIN           -2
+#define X_MAX_PIN           -1
 
 #define Y_STEP_PIN         22
 #define Y_DIR_PIN          23
@@ -653,6 +653,73 @@
 
 #endif
 
+#if MOTHERBOARD == 71
+#define KNOWN_BOARD
+/*****************************************************************
+* Ultimaker pin assignment (Old electronics)
+******************************************************************/
+
+#ifndef __AVR_ATmega1280__
+ #ifndef __AVR_ATmega2560__
+ #error Oops!  Make sure you have 'Arduino Mega' selected from the 'Tools -> Boards' menu.
+ #endif
+#endif
+
+#define X_STEP_PIN 25
+#define X_DIR_PIN 23
+#define X_MIN_PIN 15
+#define X_MAX_PIN 14
+#define X_ENABLE_PIN 27
+
+#define Y_STEP_PIN 31
+#define Y_DIR_PIN 33
+#define Y_MIN_PIN 17
+#define Y_MAX_PIN 16
+#define Y_ENABLE_PIN 29
+
+#define Z_STEP_PIN 37 
+#define Z_DIR_PIN 39
+#define Z_MIN_PIN 19
+#define Z_MAX_PIN 18
+#define Z_ENABLE_PIN 35
+
+#define HEATER_BED_PIN -1 
+#define TEMP_BED_PIN -1  
+
+#define HEATER_0_PIN  2
+#define TEMP_0_PIN 8   
+
+#define HEATER_1_PIN 1
+#define TEMP_1_PIN 1
+
+#define HEATER_2_PIN -1
+#define TEMP_2_PIN -1
+
+#define E0_STEP_PIN         43
+#define E0_DIR_PIN          45
+#define E0_ENABLE_PIN       41
+
+#define E1_STEP_PIN         -1
+#define E1_DIR_PIN          -1
+#define E1_ENABLE_PIN       -1
+
+#define SDPOWER            -1
+#define SDSS               -1
+#define LED_PIN            -1
+#define FAN_PIN            -1
+#define PS_ON_PIN          -1
+#define KILL_PIN           -1
+#define SUICIDE_PIN        -1  //PIN that has to be turned on right after start, to keep power flowing.
+
+#define LCD_PINS_RS 24 
+#define LCD_PINS_ENABLE 22
+#define LCD_PINS_D4 36
+#define LCD_PINS_D5 34 
+#define LCD_PINS_D6 32
+#define LCD_PINS_D7 30
+
+#endif
+
 /****************************************************************************************
 * Teensylu 0.7 pin assingments (ATMEGA90USB)
 * Requires the Teensyduino software with Teensy2.0++ selected in arduino IDE!
@@ -712,6 +779,62 @@
 #endif
 #endif
 
+/****************************************************************************************
+* Gen3+ pin assignment
+*
+****************************************************************************************/
+#if MOTHERBOARD == 9
+#define MOTHERBOARD 6
+#define KNOWN_BOARD 1
+#ifndef __AVR_ATmega644P__
+#error Oops!  Make sure you have 'Sanguino' selected from the 'Tools -> Boards' menu.
+#endif
+
+#define X_STEP_PIN         15
+#define X_DIR_PIN          18
+#define X_MIN_PIN          20
+#define X_MAX_PIN           -1
+
+#define Y_STEP_PIN         23
+#define Y_DIR_PIN          22
+#define Y_MIN_PIN          25
+#define Y_MAX_PIN          -1
+
+#define Z_STEP_PIN         27
+#define Z_DIR_PIN          28
+#define Z_MIN_PIN          30
+#define Z_MAX_PIN          -1
+
+#define E_STEP_PIN         17
+#define E_DIR_PIN          21
+
+#define LED_PIN            -1
+
+#define FAN_PIN            -1 
+
+#define PS_ON_PIN         14
+#define KILL_PIN           -1
+
+#define HEATER_0_PIN       12 // (extruder)
+
+#define HEATER_1_PIN       16 // (bed)
+#define X_ENABLE_PIN       19
+#define Y_ENABLE_PIN       24
+#define Z_ENABLE_PIN       29
+#define E_ENABLE_PIN       13
+
+#define TEMP_0_PIN          0   // MUST USE ANALOG INPUT NUMBERING NOT DIGITAL OUTPUT NUMBERING!!!!!!!!! (pin 33 extruder)
+#define TEMP_1_PIN          5   // MUST USE ANALOG INPUT NUMBERING NOT DIGITAL OUTPUT NUMBERING!!!!!!!!! (pin 34 bed)
+#define TEMP_2_PIN         -1
+#define SDPOWER            -1
+#define SDSS               4
+#define HEATER_2_PIN       -1
+
+#endif
+
+
+
+
 #ifndef KNOWN_BOARD
 #error Unknown MOTHERBOARD value in configuration.h
 #endif
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index f3007af53..cfa478cdd 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -430,7 +430,9 @@ void check_axes_activity() {
   }
   if((DISABLE_X) && (x_active == 0)) disable_x();
   if((DISABLE_Y) && (y_active == 0)) disable_y();
-  if((DISABLE_Z) && (z_active == 0)) disable_z();
+  #ifndef Z_LATE_ENABLE
+    if((DISABLE_Z) && (z_active == 0)) disable_z();
+  #endif
   if((DISABLE_E) && (e_active == 0)) { disable_e0();disable_e1();disable_e2(); }
 }
 
@@ -505,7 +507,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   //enable active axes
   if(block->steps_x != 0) enable_x();
   if(block->steps_y != 0) enable_y();
-  if(block->steps_z != 0) enable_z();
+//  if(block->steps_z != 0) enable_z();
 
   // Enable all
   if(block->steps_e != 0) { enable_e0();enable_e1();enable_e2(); }
@@ -535,10 +537,10 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     	if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
   } 
 
-#ifdef SLOWDOWN
+
   // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
   int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
-  
+#ifdef SLOWDOWN
   if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); 
 #endif
 
@@ -686,7 +688,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     vmax_junction = max_z_jerk/2;
   vmax_junction = min(vmax_junction, block->nominal_speed);
 
-  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+  if ((moves_queued > 1) && (previous_nominal_speed > 0.0)) {
     float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
     if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) {
       vmax_junction = block->nominal_speed;
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 076fbeea1..69d433545 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -274,6 +274,9 @@ FORCE_INLINE void trapezoid_generator_reset() {
   acceleration_time = calc_timer(acc_step_rate);
   OCR1A = acceleration_time;
   OCR1A_nominal = calc_timer(current_block->nominal_rate);
+  #ifdef Z_LATE_ENABLE
+    if(current_block->steps_z > 0) enable_z();
+  #endif
   
 //    SERIAL_ECHO_START;
 //    SERIAL_ECHOPGM("advance :");
diff --git a/Marlin/ultralcd.pde b/Marlin/ultralcd.pde
index 69be85959..68a595475 100644
--- a/Marlin/ultralcd.pde
+++ b/Marlin/ultralcd.pde
@@ -59,7 +59,7 @@ void lcdProgMemprint(const char *str)
 //=============================functions         ============================
 //===========================================================================
 
-FORCE_INLINE int intround(const float &x){return int(0.5+x);}
+int intround(const float &x){return int(0.5+x);}
 
 void lcd_status(const char* message)
 {