diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 3d5cf2ef6b3fe53fa73223af388b41bf5105ccdd..6f71b855d95894142d06644184ecc6bd13239ea2 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -48,6 +48,7 @@
 // 90 = Alpha OMCA board
 // 91 = Final OMCA board
 // 301 = Rambo
+// 21 = Elefu Ra Board (v3)
 
 #ifndef MOTHERBOARD
 #define MOTHERBOARD 7
@@ -65,6 +66,43 @@
 
 #define POWER_SUPPLY 1
 
+
+//===========================================================================
+//============================== Delta Settings =============================
+//===========================================================================
+// Enable DELTA kinematics
+#define DELTA
+
+// Make delta curves from many straight lines (linear interpolation).
+// This is a trade-off between visible corners (not enough segments)
+// and processor overload (too many expensive sqrt calls).
+#define DELTA_SEGMENTS_PER_SECOND 200
+
+// Center-to-center distance of the holes in the diagonal push rods.
+#define DELTA_DIAGONAL_ROD 250.0 // mm
+
+// Horizontal offset from middle of printer to smooth rod center.
+#define DELTA_SMOOTH_ROD_OFFSET 175.0 // mm
+
+// Horizontal offset of the universal joints on the end effector.
+#define DELTA_EFFECTOR_OFFSET 33.0 // mm
+
+// Horizontal offset of the universal joints on the carriages.
+#define DELTA_CARRIAGE_OFFSET 18.0 // mm
+
+// Effective horizontal distance bridged by diagonal push rods.
+#define DELTA_RADIUS (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET)
+
+// Effective X/Y positions of the three vertical towers.
+#define SIN_60 0.8660254037844386
+#define COS_60 0.5
+#define DELTA_TOWER1_X -SIN_60*DELTA_RADIUS // front left tower
+#define DELTA_TOWER1_Y -COS_60*DELTA_RADIUS
+#define DELTA_TOWER2_X SIN_60*DELTA_RADIUS // front right tower
+#define DELTA_TOWER2_Y -COS_60*DELTA_RADIUS
+#define DELTA_TOWER3_X 0.0 // back middle tower
+#define DELTA_TOWER3_Y DELTA_RADIUS
+
 //===========================================================================
 //=============================Thermal Settings  ============================
 //===========================================================================
@@ -130,8 +168,8 @@
 // PID settings:
 // Comment the following line to disable PID and enable bang-bang.
 #define PIDTEMP
-#define BANG_MAX 256 // limits current to nozzle while in bang-bang mode; 256=full current
-#define PID_MAX 256 // limits current to nozzle while PID is active (see PID_FUNCTIONAL_RANGE below); 256=full current
+#define BANG_MAX 255 // limits current to nozzle while in bang-bang mode; 255=full current
+#define PID_MAX 255 // limits current to nozzle while PID is active (see PID_FUNCTIONAL_RANGE below); 255=full current
 #ifdef PIDTEMP
   //#define PID_DEBUG // Sends debug data to the serial port.
   //#define PID_OPENLOOP 1 // Puts PID in open loop. M104/M140 sets the output power from 0 to PID_MAX
@@ -174,9 +212,9 @@
 
 // This sets the max power delivered to the bed, and replaces the HEATER_BED_DUTY_CYCLE_DIVIDER option.
 // all forms of bed control obey this (PID, bang-bang, bang-bang with hysteresis)
-// setting this to anything other than 256 enables a form of PWM to the bed just like HEATER_BED_DUTY_CYCLE_DIVIDER did,
+// setting this to anything other than 255 enables a form of PWM to the bed just like HEATER_BED_DUTY_CYCLE_DIVIDER did,
 // so you shouldn't use it unless you are OK with PWM on your bed.  (see the comment on enabling PIDTEMPBED)
-#define MAX_BED_POWER 256 // limits duty cycle to bed; 256=full current
+#define MAX_BED_POWER 255 // limits duty cycle to bed; 255=full current
 
 #ifdef PIDTEMPBED
 //120v 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
@@ -240,6 +278,11 @@ const bool Y_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of the endstops.
 //#define DISABLE_MAX_ENDSTOPS
 
+// Disable max endstops for compatibility with endstop checking routine
+#if defined(COREXY) && !defined(DISABLE_MAX_ENDSTOPS)
+  #define DISABLE_MAX_ENDSTOPS
+#endif
+
 // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1
 #define X_ENABLE_ON 0
 #define Y_ENABLE_ON 0
@@ -284,9 +327,11 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 //#define BED_CENTER_AT_0_0  // If defined, the center of the bed is at (X=0, Y=0)
 
 //Manual homing switch locations:
+// For deltabots this means top and center of the cartesian print volume.
 #define MANUAL_X_HOME_POS 0
 #define MANUAL_Y_HOME_POS 0
 #define MANUAL_Z_HOME_POS 0
+//#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing.
 
 //// MOVEMENT SETTINGS
 #define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
@@ -364,6 +409,11 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 //#define REPRAPWORLD_KEYPAD
 //#define REPRAPWORLD_KEYPAD_MOVE_STEP 10.0 // how much should be moved when a key is pressed, eg 10.0 means 10mm per click
 
+// The Elefu RA Board Control Panel
+// http://www.elefu.com/index.php?route=product/product&product_id=53
+// REMEMBER TO INSTALL LiquidCrystal_I2C.h in your ARUDINO library folder: https://github.com/kiyoshigawa/LiquidCrystal_I2C
+//#define RA_CONTROL_PANEL
+
 //automatic expansion
 #if defined (REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER)
  #define DOGLCD
@@ -380,6 +430,12 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
   #define NEWPANEL
   #define ULTIPANEL
 #endif
+#if defined(RA_CONTROL_PANEL)
+ #define ULTIPANEL
+ #define NEWPANEL
+ #define LCD_I2C_TYPE_PCA8574
+ #define LCD_I2C_ADDRESS 0x27   // I2C Address of the port expander
+#endif
 
 //I2C PANELS
 
@@ -448,6 +504,17 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 // Increase the FAN pwm frequency. Removes the PWM noise but increases heating in the FET/Arduino
 //#define FAST_PWM_FAN
 
+// Use software PWM to drive the fan, as for the heaters. This uses a very low frequency
+// which is not ass annoying as with the hardware PWM. On the other hand, if this frequency
+// is too low, you should also increment SOFT_PWM_SCALE.
+//#define FAN_SOFT_PWM
+
+// Incrementing this by 1 will double the software PWM frequency,
+// affecting heaters, and the fan if FAN_SOFT_PWM is enabled.
+// However, control resolution will be halved for each increment;
+// at zero value, there are 128 effective control positions.
+#define SOFT_PWM_SCALE 0
+
 // M240  Triggers a camera by emulating a Canon RC-1 Remote
 // Data from: http://www.doc-diy.net/photo/rc-1_hacked/
 // #define PHOTOGRAPH_PIN     23
@@ -473,7 +540,15 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 // leaving it undefined or defining as 0 will disable the servo subsystem
 // If unsure, leave commented / disabled
 //
-// #define NUM_SERVOS 3
+//#define NUM_SERVOS 3 // Servo index starts with 0
+
+// Servo Endstops
+// 
+// This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
+// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// 
+//#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
+//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
 
 #include "Configuration_adv.h"
 #include "thermistortables.h"
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 2e87d7b2ea2714ea240da9317db91575cdeeb8f0..c9759eb219dcaa14b316fe8d356f100c497dd9bc 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -157,6 +157,9 @@ void FlushSerialRequestResend();
 void ClearToSend();
 
 void get_coordinates();
+#ifdef DELTA
+void calculate_delta(float cartesian[3]);
+#endif
 void prepare_move();
 void kill();
 void Stop();
@@ -191,6 +194,10 @@ extern int ValvePressure;
 extern int EtoPPressure;
 #endif
 
+#ifdef FAN_SOFT_PWM
+extern unsigned char fanSpeedSoftPwm;
+#endif
+
 #ifdef FWRETRACT
 extern bool autoretract_enabled;
 extern bool retracted;
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 39a7315ee44a27e1a8b4e97f23f7109b63250106..5c9137ebc45f535d74934ac1a4f4cc492bc8af74 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -177,6 +177,10 @@ float extruder_offset[2][EXTRUDERS] = {
 #endif
 uint8_t active_extruder = 0;
 int fanSpeed=0;
+#ifdef SERVO_ENDSTOPS
+  int servo_endstops[] = SERVO_ENDSTOPS;
+  int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES;
+#endif
 #ifdef BARICUDA
 int ValvePressure=0;
 int EtoPPressure=0;
@@ -194,6 +198,9 @@ int EtoPPressure=0;
 //===========================================================================
 const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
 static float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
+#ifdef DELTA
+static float delta[3] = {0.0, 0.0, 0.0};
+#endif
 static float offset[3] = {0.0, 0.0, 0.0};
 static bool home_all_axis = true;
 static float feedrate = 1500.0, next_feedrate, saved_feedrate;
@@ -351,6 +358,16 @@ void servo_init()
   #if (NUM_SERVOS >= 5)
     #error "TODO: enter initalisation code for more servos"
   #endif
+
+  // Set position of Servo Endstops that are defined
+  #ifdef SERVO_ENDSTOPS
+  for(int8_t i = 0; i < 3; i++)
+  {
+    if(servo_endstops[i] > -1) {
+      servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
+    }
+  }
+  #endif
 }
 
 void setup()
@@ -404,10 +421,10 @@ void setup()
   servo_init();
 
   lcd_init();
-
+  
   #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
     SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
-  #endif
+  #endif 
 }
 
 
@@ -664,11 +681,16 @@ static void axis_is_at_home(int axis) {
 static void homeaxis(int axis) {
 #define HOMEAXIS_DO(LETTER) \
   ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
-
   if (axis==X_AXIS ? HOMEAXIS_DO(X) :
       axis==Y_AXIS ? HOMEAXIS_DO(Y) :
       axis==Z_AXIS ? HOMEAXIS_DO(Z) :
       0) {
+
+    // Engage Servo endstop if enabled
+    #ifdef SERVO_ENDSTOPS[axis] > -1
+      servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
+    #endif
+
     current_position[axis] = 0;
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
     destination[axis] = 1.5 * max_length(axis) * home_dir(axis);
@@ -691,6 +713,11 @@ static void homeaxis(int axis) {
     destination[axis] = current_position[axis];
     feedrate = 0.0;
     endstops_hit_on_purpose();
+
+    // Retract Servo endstop if enabled
+    #ifdef SERVO_ENDSTOPS[axis] > -1
+      servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
+    #endif
   }
 }
 #define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
@@ -782,8 +809,8 @@ void process_commands()
         destination[i] = current_position[i];
       }
       feedrate = 0.0;
-      home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
-
+      home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])))
+                    || ((code_seen(axis_codes[0])) && (code_seen(axis_codes[1])) && (code_seen(axis_codes[2])));
       #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
       if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
         HOMEAXIS(Z);
@@ -812,6 +839,10 @@ void process_commands()
         feedrate = 0.0;
         st_synchronize();
         endstops_hit_on_purpose();
+
+        current_position[X_AXIS] = destination[X_AXIS];
+        current_position[Y_AXIS] = destination[Y_AXIS];
+        current_position[Z_AXIS] = destination[Z_AXIS];
       }
       #endif
 
@@ -848,8 +879,12 @@ void process_commands()
           current_position[Z_AXIS]=code_value()+add_homeing[2];
         }
       }
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-
+      #ifdef DELTA
+        calculate_delta(current_position);
+        plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
+      #else
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+      #endif
       #ifdef ENDSTOPS_ONLY_FOR_HOMING
         enable_endstops(false);
       #endif
@@ -2032,11 +2067,64 @@ void clamp_to_software_endstops(float target[3])
   }
 }
 
+#ifdef DELTA
+void calculate_delta(float cartesian[3])
+{
+  delta[X_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
+                       - sq(DELTA_TOWER1_X-cartesian[X_AXIS])
+                       - sq(DELTA_TOWER1_Y-cartesian[Y_AXIS])
+                       ) + cartesian[Z_AXIS];
+  delta[Y_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
+                       - sq(DELTA_TOWER2_X-cartesian[X_AXIS])
+                       - sq(DELTA_TOWER2_Y-cartesian[Y_AXIS])
+                       ) + cartesian[Z_AXIS];
+  delta[Z_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
+                       - sq(DELTA_TOWER3_X-cartesian[X_AXIS])
+                       - sq(DELTA_TOWER3_Y-cartesian[Y_AXIS])
+                       ) + cartesian[Z_AXIS];
+  /*
+  SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
+  SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
+  SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
+
+  SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]);
+  SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
+  SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
+  */
+}
+#endif
+
 void prepare_move()
 {
   clamp_to_software_endstops(destination);
 
   previous_millis_cmd = millis();
+#ifdef DELTA
+  float difference[NUM_AXIS];
+  for (int8_t i=0; i < NUM_AXIS; i++) {
+    difference[i] = destination[i] - current_position[i];
+  }
+  float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
+                            sq(difference[Y_AXIS]) +
+                            sq(difference[Z_AXIS]));
+  if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
+  if (cartesian_mm < 0.000001) { return; }
+  float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+  int steps = max(1, int(DELTA_SEGMENTS_PER_SECOND * seconds));
+  // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
+  // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
+  // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+  for (int s = 1; s <= steps; s++) {
+    float fraction = float(s) / float(steps);
+    for(int8_t i=0; i < NUM_AXIS; i++) {
+      destination[i] = current_position[i] + difference[i] * fraction;
+    }
+    calculate_delta(destination);
+    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+                     destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+                     active_extruder);
+  }
+#else
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
       plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
@@ -2044,6 +2132,7 @@ void prepare_move()
   else {
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
   }
+#endif
   for(int8_t i=0; i < NUM_AXIS; i++) {
     current_position[i] = destination[i];
   }
diff --git a/Marlin/Servo.cpp b/Marlin/Servo.cpp
index 1b42ce0b049cd8b42865f1520beb1a8a966b8ee7..47c16aa7194e45724a4706b19b308b00debc8957 100644
--- a/Marlin/Servo.cpp
+++ b/Marlin/Servo.cpp
@@ -41,6 +41,8 @@
  detach()    - Stops an attached servos from pulsing its i/o pin.
 
 */
+#include "Configuration.h" 
+
 #ifdef NUM_SERVOS
 #include <avr/interrupt.h>
 #include <Arduino.h>
diff --git a/Marlin/language.h b/Marlin/language.h
index ea43c1bbc3c79a3b201c01c5f784dddc209f7311..dcc4112fa8d4a8a7510db69ae9288934193c7ced 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -9,7 +9,7 @@
 // Languages
 // 1  English
 // 2  Polish
-// 3  French	(awaiting translation!)
+// 3  French
 // 4  German
 // 5  Spanish
 // 6  Russian
@@ -79,9 +79,9 @@
 	#define MSG_PID_D "PID-D"
 	#define MSG_PID_C "PID-C"
 	#define MSG_ACC  "Accel"
-	#define MSG_VXY_JERK "Vxy-jerk"
-	#define MSG_VZ_JERK "Vz-jerk"
-	#define MSG_VE_JERK "Ve-jerk"
+	#define MSG_VXY_JERK "Vxy-jerk"
+	#define MSG_VZ_JERK "Vz-jerk"
+	#define MSG_VE_JERK "Ve-jerk"
 	#define MSG_VMAX "Vmax "
 	#define MSG_X "x"
 	#define MSG_Y "y"
@@ -239,8 +239,8 @@
 	#define MSG_PID_C "PID-C"
 	#define MSG_ACC  "Acc"
 	#define MSG_VXY_JERK "Zryw Vxy"
-	#define MSG_VZ_JERK "Zryw Vz"
-	#define MSG_VE_JERK "Zryw Ve"
+	#define MSG_VZ_JERK "Zryw Vz"
+	#define MSG_VE_JERK "Zryw Ve"
 	#define MSG_VMAX "Vmax"
 	#define MSG_X "x"
 	#define MSG_Y "y"
@@ -400,8 +400,8 @@
 #define MSG_PID_C " PID-C: "
 #define MSG_ACC " Acc:"
 #define MSG_VXY_JERK "Vxy-jerk"
-#define MSG_VZ_JERK "Vz-jerk"
-#define MSG_VE_JERK "Ve-jerk"
+#define MSG_VZ_JERK "Vz-jerk"
+#define MSG_VE_JERK "Ve-jerk"
 #define MSG_VMAX " Vmax "
 #define MSG_X "x:"
 #define MSG_Y "y:"
@@ -565,8 +565,8 @@
 	#define MSG_PID_C            "PID-C"
 	#define MSG_ACC              "Acc"
 	#define MSG_VXY_JERK         "Vxy-jerk"
-	#define MSG_VZ_JERK          "Vz-jerk"
-	#define MSG_VE_JERK          "Ve-jerk"
+	#define MSG_VZ_JERK          "Vz-jerk"
+	#define MSG_VE_JERK          "Ve-jerk"
 	#define MSG_VMAX             "Vmax "
 	#define MSG_X                "x"
 	#define MSG_Y                "y"
@@ -706,9 +706,9 @@
 #define MSG_PREHEAT_ABS_SETTINGS " Ajustar temp. ABS"
 #define MSG_MOVE_AXIS " Mover Ejes      \x7E"
 #define MSG_SPEED " Velocidad:"
-#define MSG_NOZZLE " \002Nozzle:"
-#define MSG_NOZZLE1 " \002Nozzle2:"
-#define MSG_NOZZLE2 " \002Nozzle3:"
+#define MSG_NOZZLE " \002Fusor:"
+#define MSG_NOZZLE1 " \002Fusor2:"
+#define MSG_NOZZLE2 " \002Fusor3:"
 #define MSG_BED " \002Base:"
 #define MSG_FAN_SPEED " Ventilador:"
 #define MSG_FLOW " Flujo:"
@@ -724,9 +724,9 @@
 #define MSG_PID_D " PID-D: "
 #define MSG_PID_C " PID-C: "
 #define MSG_ACC  " Acc:"
-#define MSG_VXY_JERK " Vxy-jerk: "
-#define MSG_VZ_JERK "Vz-jerk"
-#define MSG_VE_JERK "Ve-jerk"
+#define MSG_VXY_JERK " Vxy-agit: "
+#define MSG_VZ_JERK "Vz-agit"
+#define MSG_VE_JERK "Ve-agit"
 #define MSG_VMAX " Vmax "
 #define MSG_X "x:"
 #define MSG_Y "y:"
@@ -821,7 +821,7 @@
 #define MSG_M119_REPORT "Comprobando fines de carrera."
 #define MSG_ENDSTOP_HIT "PULSADO"
 #define MSG_ENDSTOP_OPEN "abierto"
-#define MSG_HOTEND_OFFSET "Hotend offsets:"
+#define MSG_HOTEND_OFFSET "Despl. Hotend:"
         
 #define MSG_SD_CANT_OPEN_SUBDIR "No se pudo abrir la subcarpeta."
 #define MSG_SD_INIT_FAIL "Fallo al iniciar la SD"
@@ -885,8 +885,8 @@
 #define MSG_PID_C							" PID-C: "
 #define MSG_ACC								" Acc:"
 #define MSG_VXY_JERK						" Vxy-jerk: "
-#define MSG_VZ_JERK                         "Vz-jerk"
-#define MSG_VE_JERK                         "Ve-jerk"
+#define MSG_VZ_JERK                         "Vz-jerk"
+#define MSG_VE_JERK                         "Ve-jerk"
 #define MSG_VMAX							" Vmax "
 #define MSG_X								"x:"
 #define MSG_Y								"y:"
@@ -1040,8 +1040,8 @@
 	#define MSG_PID_C                "PID-C"
 	#define MSG_ACC                  "Accel"
 	#define MSG_VXY_JERK             "Vxy-jerk"
-	#define MSG_VZ_JERK              "Vz-jerk"
-	#define MSG_VE_JERK              "Ve-jerk"
+	#define MSG_VZ_JERK              "Vz-jerk"
+	#define MSG_VE_JERK              "Ve-jerk"
 	#define MSG_VMAX                 "Vmax"
 	#define MSG_X                    "x"
 	#define MSG_Y                    "y"
@@ -1202,8 +1202,8 @@
 	#define MSG_PID_C " PID-C: "
 	#define MSG_ACC  " Acc:"
 	#define MSG_VXY_JERK " Vxy-jerk: "
-	#define MSG_VZ_JERK "Vz-jerk"
-	#define MSG_VE_JERK "Ve-jerk"
+	#define MSG_VZ_JERK "Vz-jerk"
+	#define MSG_VE_JERK "Ve-jerk"
 	#define MSG_VMAX " Vmax "
 	#define MSG_X "x:"
 	#define MSG_Y "y:"
@@ -1370,8 +1370,8 @@
 	#define MSG_PID_C "PID-C"
 	#define MSG_ACC  "Kiihtyv"
 	#define MSG_VXY_JERK "Vxy-jerk"
-	#define MSG_VZ_JERK "Vz-jerk"
-	#define MSG_VE_JERK "Ve-jerk"
+	#define MSG_VZ_JERK "Vz-jerk"
+	#define MSG_VE_JERK "Ve-jerk"
 	#define MSG_VMAX "Vmax "
 	#define MSG_X "x"
 	#define MSG_Y "y"
diff --git a/Marlin/pins.h b/Marlin/pins.h
index 140525f685fbb0959f09ceaa9b3cd28b22ddcc47..58684c4413072ba1803f6a9f7ac1ada27f76acb5 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -53,6 +53,7 @@
 
 #endif /* 99 */
 
+
 /****************************************************************************************
 * Gen7 v1.1, v1.2, v1.3 pin assignment
 *
@@ -391,7 +392,7 @@
       #define SERVO2_PIN         5
     #endif
 
-    #if NUM_SERVOS > 2
+    #if NUM_SERVOS > 3
       #define SERVO3_PIN         4
     #endif
   #endif
@@ -571,6 +572,131 @@
 
 #endif
 
+/****************************************************************************************
+* Elefu RA Board Pin Assignments
+*
+****************************************************************************************/
+#if MOTHERBOARD == 21
+#define	KNOWN_BOARD 1
+
+#ifndef __AVR_ATmega2560__
+ #error Oops!  Make sure you have 'Arduino Mega' selected from the 'Tools -> Boards' menu.
+#endif
+
+
+#define X_STEP_PIN		     49
+#define X_DIR_PIN			     13
+#define X_ENABLE_PIN		   48
+#define X_MIN_PIN			     35
+#define X_MAX_PIN			     -1 //34
+
+#define Y_STEP_PIN         11
+#define Y_DIR_PIN          9
+#define Y_ENABLE_PIN       12
+#define Y_MIN_PIN          33
+#define Y_MAX_PIN          -1 //32
+
+#define Z_STEP_PIN         7
+#define Z_DIR_PIN          6
+#define Z_ENABLE_PIN       8
+#define Z_MIN_PIN          31
+#define Z_MAX_PIN          -1 //30
+
+#define E2_STEP_PIN        43
+#define E2_DIR_PIN         47
+#define E2_ENABLE_PIN      42
+
+#define E1_STEP_PIN        18
+#define E1_DIR_PIN         19
+#define E1_ENABLE_PIN      38
+
+#define E0_STEP_PIN        40
+#define E0_DIR_PIN         41
+#define E0_ENABLE_PIN      37
+
+#define SDPOWER            -1
+#define LED_PIN            -1 //Use +12V Aux port for LED Ring
+
+#define FAN_PIN            16 //5V PWM
+
+#define PS_ON_PIN          10 //Set to -1 if using a manual switch on the PWRSW Connector
+#define SLEEP_WAKE_PIN		 26 //This feature still needs work
+
+#define HEATER_0_PIN       45	//12V PWM1
+#define HEATER_1_PIN       46	//12V PWM2
+#define HEATER_2_PIN       17	//12V PWM3
+#define HEATER_BED_PIN     44	//DOUBLE 12V PWM
+#define TEMP_0_PIN         3	//ANALOG NUMBERING
+#define TEMP_1_PIN         2 	//ANALOG NUMBERING
+#define TEMP_2_PIN         1 	//ANALOG NUMBERING
+#define TEMP_BED_PIN       0	//ANALOG NUMBERING
+
+#define BEEPER 		         36
+
+#define KILL_PIN           -1
+
+// M240  Triggers a camera by emulating a Canon RC-1 Remote
+// Data from: http://www.doc-diy.net/photo/rc-1_hacked/
+#define PHOTOGRAPH_PIN     29
+
+#ifdef RA_CONTROL_PANEL
+
+  #define SDSS             53
+  #define SDCARDDETECT     28
+
+  #define BTN_EN1          14
+  #define BTN_EN2          39
+  #define BTN_ENC          15  //the click
+    
+  #define BLEN_C           2
+  #define BLEN_B           1
+  #define BLEN_A           0
+    
+  //encoder rotation values
+  #define encrot0          0
+  #define encrot1          2
+  #define encrot2          3
+  #define encrot3          1
+  
+#endif //RA_CONTROL_PANEL
+
+#ifdef RA_DISCO
+  //variables for which pins the TLC5947 is using
+  #define TLC_CLOCK_PIN    25
+  #define TLC_BLANK_PIN    23
+  #define TLC_XLAT_PIN     22
+  #define TLC_DATA_PIN     24
+
+  //We also need to define pin to port number mapping for the 2560 to match the pins listed above. If you change the TLC pins, update this as well per the 2560 datasheet!
+  //This currently only works with the RA Board.
+  #define TLC_CLOCK_BIT 3 //bit 3 on port A
+  #define TLC_CLOCK_PORT &PORTA //bit 3 on port A
+  
+  #define TLC_BLANK_BIT 1 //bit 1 on port A
+  #define TLC_BLANK_PORT &PORTA //bit 1 on port A
+
+  #define TLC_DATA_BIT 2 //bit 2 on port A
+  #define TLC_DATA_PORT &PORTA //bit 2 on port A
+
+  #define TLC_XLAT_BIT 0 //bit 0 on port A
+  #define TLC_XLAT_PORT &PORTA //bit 0 on port A
+
+  //change this to match your situation. Lots of TLCs takes up the arduino SRAM very quickly, so be careful 
+  //Leave it at at least 1 if you have enabled RA_LIGHTING
+  //The number of TLC5947 boards chained together for use with the animation, additional ones will repeat the animation on them, but are not individually addressable and mimic those before them. You can leave the default at 2 even if you only have 1 TLC5947 module.
+  #define NUM_TLCS 2 
+
+  //These TRANS_ARRAY values let you change the order the LEDs on the lighting modules will animate for chase functions. 
+  //Modify them according to your specific situation.
+  //NOTE: the array should be 8 long for every TLC you have. These defaults assume (2) TLCs.
+  #define TRANS_ARRAY {0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8} //forwards
+  //#define TRANS_ARRAY {7, 6, 5, 4, 3, 2, 1, 0, 8, 9, 10, 11, 12, 13, 14, 15} //backwards
+#endif //RA_LIGHTING
+
+
+#endif /* Ra Board */
+
+
 /****************************************************************************************
 * Gen6 pin assignment
 *
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index fcebab938aeb443c01df106139d5ef88d9bc2f73..8eff191751a078be015e12744dfc71a9ccc8fb84 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -473,22 +473,24 @@ void check_axes_activity()
     disable_e2(); 
   }
 #if defined(FAN_PIN) && FAN_PIN > -1
-  #ifndef FAN_SOFT_PWM
-    #ifdef FAN_KICKSTART_TIME
-      static unsigned long fan_kick_end;
-      if (tail_fan_speed) {
-        if (fan_kick_end == 0) {
-          // Just starting up fan - run at full power.
-          fan_kick_end = millis() + FAN_KICKSTART_TIME;
-          tail_fan_speed = 255;
-        } else if (fan_kick_end > millis())
-          // Fan still spinning up.
-          tail_fan_speed = 255;
-      } else {
-        fan_kick_end = 0;
-      }
-    #endif//FAN_KICKSTART_TIME
-    analogWrite(FAN_PIN,tail_fan_speed);
+  #ifdef FAN_KICKSTART_TIME
+    static unsigned long fan_kick_end;
+    if (tail_fan_speed) {
+      if (fan_kick_end == 0) {
+        // Just starting up fan - run at full power.
+        fan_kick_end = millis() + FAN_KICKSTART_TIME;
+        tail_fan_speed = 255;
+      } else if (fan_kick_end > millis())
+        // Fan still spinning up.
+        tail_fan_speed = 255;
+    } else {
+      fan_kick_end = 0;
+    }
+  #endif//FAN_KICKSTART_TIME
+  #ifdef FAN_SOFT_PWM
+  fanSpeedSoftPwm = tail_fan_speed;
+  #else
+  analogWrite(FAN_PIN,tail_fan_speed);
   #endif//!FAN_SOFT_PWM
 #endif//FAN_PIN > -1
 #ifdef AUTOTEMP
@@ -562,8 +564,16 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   block->busy = false;
 
   // Number of steps for each axis
-  block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
-  block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
+#ifndef COREXY
+// default non-h-bot planning
+block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
+block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
+#else
+// corexy planning
+// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
+block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
+block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
+#endif
   block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
   block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
   block->steps_e *= extrudemultiply;
@@ -584,6 +594,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
 
   // Compute direction bits for this block 
   block->direction_bits = 0;
+#ifndef COREXY
   if (target[X_AXIS] < position[X_AXIS])
   {
     block->direction_bits |= (1<<X_AXIS); 
@@ -592,6 +603,16 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   {
     block->direction_bits |= (1<<Y_AXIS); 
   }
+#else
+  if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
+  {
+    block->direction_bits |= (1<<X_AXIS); 
+  }
+  if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
+  {
+    block->direction_bits |= (1<<Y_AXIS); 
+  }
+#endif
   if (target[Z_AXIS] < position[Z_AXIS])
   {
     block->direction_bits |= (1<<Z_AXIS); 
@@ -636,8 +657,13 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   } 
 
   float delta_mm[4];
-  delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
-  delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
+  #ifndef COREXY
+    delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
+    delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
+  #else
+    delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
+    delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
+  #endif
   delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
   delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
   if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
@@ -757,7 +783,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
       block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
   }
   block->acceleration = block->acceleration_st / steps_per_mm;
-  block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
+  block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
 
 #if 0  // Use old jerk for now
   // Compute path unit vector
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index e25ba6453d601d1c4d5733a599f49eb15b394576..a7991501e940fef5dbb9b9c115d685f1a7e8d26c 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -345,12 +345,31 @@ ISR(TIMER1_COMPA_vect)
     // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
     out_bits = current_block->direction_bits;
 
+
+    // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
+    if((out_bits & (1<<X_AXIS))!=0){
+      WRITE(X_DIR_PIN, INVERT_X_DIR);
+      count_direction[X_AXIS]=-1;
+    }
+    else{
+      WRITE(X_DIR_PIN, !INVERT_X_DIR);
+      count_direction[X_AXIS]=1;
+    }
+    if((out_bits & (1<<Y_AXIS))!=0){
+      WRITE(Y_DIR_PIN, INVERT_Y_DIR);
+      count_direction[Y_AXIS]=-1;
+    }
+    else{
+      WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
+      count_direction[Y_AXIS]=1;
+    }
+    
     // Set direction en check limit switches
+    #ifndef COREXY
     if ((out_bits & (1<<X_AXIS)) != 0) {   // stepping along -X axis
-      #if !defined COREXY  //NOT COREXY
-        WRITE(X_DIR_PIN, INVERT_X_DIR);
-      #endif
-      count_direction[X_AXIS]=-1;
+    #else
+    if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) {   //-X occurs for -A and -B
+    #endif
       CHECK_ENDSTOPS
       {
         #if defined(X_MIN_PIN) && X_MIN_PIN > -1
@@ -365,11 +384,6 @@ ISR(TIMER1_COMPA_vect)
       }
     }
     else { // +direction
-      #if !defined COREXY  //NOT COREXY
-        WRITE(X_DIR_PIN,!INVERT_X_DIR);
-      #endif
-      
-      count_direction[X_AXIS]=1;
       CHECK_ENDSTOPS 
       {
         #if defined(X_MAX_PIN) && X_MAX_PIN > -1
@@ -384,11 +398,11 @@ ISR(TIMER1_COMPA_vect)
       }
     }
 
+    #ifndef COREXY
     if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction
-      #if !defined COREXY  //NOT COREXY
-        WRITE(Y_DIR_PIN,INVERT_Y_DIR);
-      #endif
-      count_direction[Y_AXIS]=-1;
+    #else
+    if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) {   // -Y occurs for -A and +B
+    #endif
       CHECK_ENDSTOPS
       {
         #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
@@ -403,10 +417,6 @@ ISR(TIMER1_COMPA_vect)
       }
     }
     else { // +direction
-      #if !defined COREXY  //NOT COREXY
-        WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
-      #endif
-      count_direction[Y_AXIS]=1;
       CHECK_ENDSTOPS
       {
         #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
@@ -420,28 +430,7 @@ ISR(TIMER1_COMPA_vect)
         #endif
       }
     }
-    
-    
-    #ifdef COREXY  //coreXY kinematics defined
-      if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) == 0)){  //+X is major axis
-        WRITE(X_DIR_PIN, !INVERT_X_DIR);
-        WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
-      }
-      if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) != 0)){  //-X is major axis
-        WRITE(X_DIR_PIN, INVERT_X_DIR);
-        WRITE(Y_DIR_PIN, INVERT_Y_DIR);
-      }      
-      if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) == 0)){  //+Y is major axis
-        WRITE(X_DIR_PIN, !INVERT_X_DIR);
-        WRITE(Y_DIR_PIN, INVERT_Y_DIR);
-      }        
-      if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) != 0)){  //-Y is major axis
-        WRITE(X_DIR_PIN, INVERT_X_DIR);
-        WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
-      }  
-    #endif //coreXY
-    
-    
+
     if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
       WRITE(Z_DIR_PIN,INVERT_Z_DIR);
       
@@ -516,7 +505,6 @@ ISR(TIMER1_COMPA_vect)
       }    
       #endif //ADVANCE
 
-      #if !defined COREXY      
         counter_x += current_block->steps_x;
         if (counter_x > 0) {
           WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
@@ -532,56 +520,7 @@ ISR(TIMER1_COMPA_vect)
           count_position[Y_AXIS]+=count_direction[Y_AXIS]; 
           WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
         }
-      #endif
   
-      #ifdef COREXY
-        counter_x += current_block->steps_x;        
-        counter_y += current_block->steps_y;
-        
-        if ((counter_x > 0)&&!(counter_y>0)){  //X step only
-          WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
-          WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
-          counter_x -= current_block->step_event_count; 
-          count_position[X_AXIS]+=count_direction[X_AXIS];         
-          WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
-          WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
-        }
-        
-        if (!(counter_x > 0)&&(counter_y>0)){  //Y step only
-          WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
-          WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
-          counter_y -= current_block->step_event_count; 
-          count_position[Y_AXIS]+=count_direction[Y_AXIS];
-          WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
-          WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
-        }        
-        
-        if ((counter_x > 0)&&(counter_y>0)){  //step in both axes
-          if (((out_bits & (1<<X_AXIS)) == 0)^((out_bits & (1<<Y_AXIS)) == 0)){  //X and Y in different directions
-            WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
-            counter_x -= current_block->step_event_count;             
-            WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
-            step_wait();
-            count_position[X_AXIS]+=count_direction[X_AXIS];
-            count_position[Y_AXIS]+=count_direction[Y_AXIS];
-            WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
-            counter_y -= current_block->step_event_count;
-            WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
-          }
-          else{  //X and Y in same direction
-            WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
-            counter_x -= current_block->step_event_count;             
-            WRITE(X_STEP_PIN, INVERT_X_STEP_PIN) ;
-            step_wait();
-            count_position[X_AXIS]+=count_direction[X_AXIS];
-            count_position[Y_AXIS]+=count_direction[Y_AXIS];
-            WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); 
-            counter_y -= current_block->step_event_count;    
-            WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);        
-          }
-        }
-      #endif //corexy
-      
       counter_z += current_block->steps_z;
       if (counter_z > 0) {
         WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index ed0a47be2d4c5525eb1f5e3720ef07a412888a1b..f5df7a1e72211392fd2ff2822eab8d83d0e28dfa 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -62,6 +62,9 @@ float current_temperature_bed = 0.0;
   float bedKd=(DEFAULT_bedKd/PID_dT);
 #endif //PIDTEMPBED
   
+#ifdef FAN_SOFT_PWM
+  unsigned char fanSpeedSoftPwm;
+#endif
   
 //===========================================================================
 //=============================private variables============================
@@ -145,6 +148,10 @@ int watch_start_temp[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
 unsigned long watchmillis[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0,0,0);
 #endif //WATCH_TEMP_PERIOD
 
+#ifndef SOFT_PWM_SCALE
+#define SOFT_PWM_SCALE 0
+#endif
+
 //===========================================================================
 //=============================   functions      ============================
 //===========================================================================
@@ -720,8 +727,8 @@ void tp_init()
     setPwmFrequency(FAN_PIN, 1); // No prescaling. Pwm frequency = F_CPU/256/8
     #endif
     #ifdef FAN_SOFT_PWM
-	soft_pwm_fan=(unsigned char)fanSpeed;
-	#endif
+    soft_pwm_fan = fanSpeedSoftPwm / 2;
+    #endif
   #endif  
 
   #ifdef HEATER_0_USES_MAX6675
@@ -1028,7 +1035,7 @@ ISR(TIMER0_COMPB_vect)
   static unsigned long raw_temp_2_value = 0;
   static unsigned long raw_temp_bed_value = 0;
   static unsigned char temp_state = 0;
-  static unsigned char pwm_count = 1;
+  static unsigned char pwm_count = (1 << SOFT_PWM_SCALE);
   static unsigned char soft_pwm_0;
   #if EXTRUDERS > 1
   static unsigned char soft_pwm_1;
@@ -1056,7 +1063,7 @@ ISR(TIMER0_COMPB_vect)
     if(soft_pwm_b > 0) WRITE(HEATER_BED_PIN,1);
     #endif
     #ifdef FAN_SOFT_PWM
-    soft_pwm_fan =(unsigned char) fanSpeed;
+    soft_pwm_fan = fanSpeedSoftPwm / 2;
     if(soft_pwm_fan > 0) WRITE(FAN_PIN,1);
     #endif
   }
@@ -1074,7 +1081,7 @@ ISR(TIMER0_COMPB_vect)
   if(soft_pwm_fan <= pwm_count) WRITE(FAN_PIN,0);
   #endif
   
-  pwm_count++;
+  pwm_count += (1 << SOFT_PWM_SCALE);
   pwm_count &= 0x7f;
   
   switch(temp_state) {
diff --git a/Marlin/ultralcd_implementation_hitachi_HD44780.h b/Marlin/ultralcd_implementation_hitachi_HD44780.h
index fa20a03fa77a187159ccb8bf13e9a3c4c5553680..9be57a905c2794716476551766f55b7e0cbaa906 100644
--- a/Marlin/ultralcd_implementation_hitachi_HD44780.h
+++ b/Marlin/ultralcd_implementation_hitachi_HD44780.h
@@ -13,7 +13,7 @@ extern volatile uint16_t buttons;  //an extended version of the last checked but
 #endif
 
 ////////////////////////////////////
-// Setup button and encode mappings for each panel (into 'buttons' variable)
+// Setup button and encode mappings for each panel (into 'buttons' variable
 //
 // This is just to map common functions (across different panels) onto the same 
 // macro name. The mapping is independent of whether the button is directly connected or 
@@ -180,6 +180,11 @@ extern volatile uint16_t buttons;  //an extended version of the last checked but
   #include <LiquidTWI2.h>
   #define LCD_CLASS LiquidTWI2
   LCD_CLASS lcd(LCD_I2C_ADDRESS);  
+
+#elif defined(LCD_I2C_TYPE_PCA8574)
+    #include <LiquidCrystal_I2C.h>
+    #define LCD_CLASS LiquidCrystal_I2C
+    LCD_CLASS lcd(LCD_I2C_ADDRESS, LCD_WIDTH, LCD_HEIGHT);
   
 #else
   // Standard directly connected LCD implementations
@@ -305,6 +310,10 @@ static void lcd_implementation_init()
 #elif defined(LCD_I2C_TYPE_MCP23008)
     lcd.setMCPType(LTI_TYPE_MCP23008);
     lcd.begin(LCD_WIDTH, LCD_HEIGHT);
+
+#elif defined(LCD_I2C_TYPE_PCA8574)
+      lcd.init();
+      lcd.backlight();
     
 #else
     lcd.begin(LCD_WIDTH, LCD_HEIGHT);
@@ -706,9 +715,9 @@ static void lcd_implementation_quick_feedback()
     for(int8_t i=0;i<10;i++)
     {
       WRITE(BEEPER,HIGH);
-      delay(3);
+      delayMicroseconds(100);
       WRITE(BEEPER,LOW);
-      delay(3);
+      delayMicroseconds(100);
     }
 #endif
 }