Merge branch 'Merge_cleanup' into Development

Marlin/Conditionals.h

@@ -4,6 +4,10 @@
  */
 #ifndef CONDITIONALS_H
 
+#ifndef M_PI
+  #define M_PI 3.1415926536
+#endif
+
 #ifndef CONFIGURATION_LCD // Get the LCD defines which are needed first
 
   #define CONFIGURATION_LCD
@@ -255,7 +259,7 @@
    * Advance calculated values
    */
   #ifdef ADVANCE
-    #define EXTRUSION_AREA (0.25 * D_FILAMENT * D_FILAMENT * 3.14159)
+    #define EXTRUSION_AREA (0.25 * D_FILAMENT * D_FILAMENT * M_PI)
     #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS] / EXTRUSION_AREA)
   #endif
 

Marlin/Marlin.h

@@ -29,6 +29,8 @@
 
 #define BIT(b) (1<<(b))
 #define TEST(n,b) (((n)&BIT(b))!=0)
+#define RADIANS(d) ((d)*M_PI/180.0)
+#define DEGREES(r) ((d)*180.0/M_PI)
 
 // Arduino < 1.0.0 does not define this, so we need to do it ourselves
 #ifndef analogInputToDigitalPin

Marlin/Marlin_main.cpp

@@ -1034,6 +1034,12 @@ inline void line_to_destination() {
 inline void sync_plan_position() {
   plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
+#if defined(DELTA) || defined(SCARA)
+  inline void sync_plan_position_delta() {
+    calculate_delta(current_position);
+    plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
+  }
+#endif
 
 #ifdef ENABLE_AUTO_BED_LEVELING
 
@@ -1103,14 +1109,13 @@ inline void sync_plan_position() {
       destination[Z_AXIS] = -10;
       prepare_move_raw();
       st_synchronize();
-      endstops_hit_on_purpose();
+      endstops_hit_on_purpose(); // clear endstop hit flags
       
       // we have to let the planner know where we are right now as it is not where we said to go.
       long stop_steps = st_get_position(Z_AXIS);
       float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
       current_position[Z_AXIS] = mm;
-      calculate_delta(current_position);
+      sync_plan_position_delta();
-      plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
       
     #else // !DELTA
 
@@ -1130,7 +1135,7 @@ inline void sync_plan_position() {
       zPosition += home_retract_mm(Z_AXIS);
       line_to_z(zPosition);
       st_synchronize();
-      endstops_hit_on_purpose();
+      endstops_hit_on_purpose(); // clear endstop hit flags
 
       // move back down slowly to find bed
       if (homing_bump_divisor[Z_AXIS] >= 1)
@@ -1143,7 +1148,7 @@ inline void sync_plan_position() {
       zPosition -= home_retract_mm(Z_AXIS) * 2;
       line_to_z(zPosition);
       st_synchronize();
-      endstops_hit_on_purpose();
+      endstops_hit_on_purpose(); // clear endstop hit flags
 
       current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
       // make sure the planner knows where we are as it may be a bit different than we last said to move to
@@ -1267,7 +1272,7 @@ inline void sync_plan_position() {
       if (servo_endstops[Z_AXIS] >= 0) {
 
         #if Z_RAISE_AFTER_PROBING > 0
-          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
+          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
           st_synchronize();
         #endif
 
@@ -1355,7 +1360,7 @@ inline void sync_plan_position() {
 
     #if Z_RAISE_BETWEEN_PROBINGS > 0
       if (retract_action == ProbeStay) {
-        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BETWEEN_PROBINGS);
+        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
         st_synchronize();
       }
     #endif
@@ -1440,13 +1445,17 @@ inline void sync_plan_position() {
 
 #endif // ENABLE_AUTO_BED_LEVELING
 
+/**
+ * Home an individual axis
+ */
+
+#define HOMEAXIS(LETTER) homeaxis(LETTER##_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) :
+  if (axis == X_AXIS ? HOMEAXIS_DO(X) : axis == Y_AXIS ? HOMEAXIS_DO(Y) : axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
-      axis == Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
 
     int axis_home_dir;
 
@@ -1456,74 +1465,81 @@ static void homeaxis(int axis) {
       axis_home_dir = home_dir(axis);
     #endif
 
+    // Set the axis position as setup for the move
     current_position[axis] = 0;
     sync_plan_position();
 
-    #ifndef Z_PROBE_SLED
     // Engage Servo endstop if enabled
-      #ifdef SERVO_ENDSTOPS
+    #ifdef SERVO_ENDSTOPS && !defined(Z_PROBE_SLED)
-        #if SERVO_LEVELING
-          if (axis == Z_AXIS) {
-            engage_z_probe();
-          }
-          else
-        #endif // SERVO_LEVELING
 
+      #if SERVO_LEVELING
+        if (axis == Z_AXIS) engage_z_probe(); else
+      #endif
+        {
           if (servo_endstops[axis] > -1)
             servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
+        }
 
-      #endif // SERVO_ENDSTOPS
+    #endif // SERVO_ENDSTOPS && !Z_PROBE_SLED
-
-    #endif // Z_PROBE_SLED
 
     #ifdef Z_DUAL_ENDSTOPS
       if (axis == Z_AXIS) In_Homing_Process(true);
     #endif
 
+    // Move towards the endstop until an endstop is triggered
     destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
     feedrate = homing_feedrate[axis];
     line_to_destination();
     st_synchronize();
 
+    // Set the axis position as setup for the move
     current_position[axis] = 0;
     sync_plan_position();
+
+    // Move away from the endstop by the axis HOME_RETRACT_MM
     destination[axis] = -home_retract_mm(axis) * axis_home_dir;
     line_to_destination();
     st_synchronize();
 
-    destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
+    // Slow down the feedrate for the next move
-
     if (homing_bump_divisor[axis] >= 1)
       feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
     else {
       feedrate = homing_feedrate[axis] / 10;
-      SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
+      SERIAL_ECHOLNPGM("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
     }
 
+    // Move slowly towards the endstop until triggered
+    destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
     line_to_destination();
     st_synchronize();
+
     #ifdef Z_DUAL_ENDSTOPS
-      if (axis==Z_AXIS)
+      if (axis == Z_AXIS) {
-      {
+        float adj = fabs(z_endstop_adj);
-        feedrate = homing_feedrate[axis];
+        bool lockZ1;
-        sync_plan_position();
+        if (axis_home_dir > 0) {
-        if (axis_home_dir > 0)
+          adj = -adj;
-        {
+          lockZ1 = (z_endstop_adj > 0);
-          destination[axis] = (-1) * fabs(z_endstop_adj);
-          if (z_endstop_adj > 0) Lock_z_motor(true); else Lock_z2_motor(true);
-        } else {
-          destination[axis] = fabs(z_endstop_adj);
-          if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);        
         }
+        else
+          lockZ1 = (z_endstop_adj < 0);
+
+        if (lockZ1) Lock_z_motor(true); else Lock_z2_motor(true);
+        sync_plan_position();
+
+        // Move to the adjusted endstop height
+        feedrate = homing_feedrate[axis];
+        destination[Z_AXIS] = adj;
         line_to_destination();
         st_synchronize();
-        Lock_z_motor(false);
+
-        Lock_z2_motor(false);
+        if (lockZ1) Lock_z_motor(false); else Lock_z2_motor(false);
         In_Homing_Process(false);
-      }
+      } // Z_AXIS
     #endif
 
-#ifdef DELTA
+    #ifdef DELTA
       // retrace by the amount specified in endstop_adj
       if (endstop_adj[axis] * axis_home_dir < 0) {
         sync_plan_position();
@@ -1531,91 +1547,83 @@ static void homeaxis(int axis) {
         line_to_destination();
         st_synchronize();
       }
-#endif
+    #endif
+
+    // Set the axis position to its home position (plus home offsets)
     axis_is_at_home(axis);
+
     destination[axis] = current_position[axis];
     feedrate = 0.0;
-    endstops_hit_on_purpose();
+    endstops_hit_on_purpose(); // clear endstop hit flags
     axis_known_position[axis] = true;
 
     // Retract Servo endstop if enabled
     #ifdef SERVO_ENDSTOPS
-      if (servo_endstops[axis] > -1) {
+      if (servo_endstops[axis] > -1)
         servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
-      }
     #endif
-#if SERVO_LEVELING
+
-  #ifndef Z_PROBE_SLED
+    #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
-    if (axis==Z_AXIS) retract_z_probe();
+      if (axis == Z_AXIS) retract_z_probe();
     #endif
-#endif
 
   }
 }
-#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
 
-void refresh_cmd_timeout(void)
+void refresh_cmd_timeout(void) { previous_millis_cmd = millis(); }
-{
-  previous_millis_cmd = millis();
-}
 
 #ifdef FWRETRACT
+
   void retract(bool retracting, bool swapretract = false) {
-    if(retracting && !retracted[active_extruder]) {
+
-      destination[X_AXIS]=current_position[X_AXIS];
+    if (retracting == retracted[active_extruder]) return;
-      destination[Y_AXIS]=current_position[Y_AXIS];
+
-      destination[Z_AXIS]=current_position[Z_AXIS];
-      destination[E_AXIS]=current_position[E_AXIS];
-      if (swapretract) {
-        current_position[E_AXIS]+=retract_length_swap/volumetric_multiplier[active_extruder];
-      } else {
-        current_position[E_AXIS]+=retract_length/volumetric_multiplier[active_extruder];
-      }
-      plan_set_e_position(current_position[E_AXIS]);
     float oldFeedrate = feedrate;
+
+    for (int i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i];
+
+    if (retracting) {
+
       feedrate = retract_feedrate * 60;
-      retracted[active_extruder]=true;
+      current_position[E_AXIS] += (swapretract ? retract_length_swap : retract_length) / volumetric_multiplier[active_extruder];
+      plan_set_e_position(current_position[E_AXIS]);
       prepare_move();
-      if(retract_zlift > 0.01) {
+
-         current_position[Z_AXIS]-=retract_zlift;
+      if (retract_zlift > 0.01) {
-#ifdef DELTA
+        current_position[Z_AXIS] -= retract_zlift;
-         calculate_delta(current_position); // change cartesian kinematic to  delta kinematic;
+        #ifdef DELTA
-         plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
+          sync_plan_position_delta();
-#else
+        #else
           sync_plan_position();
-#endif
+        #endif
         prepare_move();
       }
-      feedrate = oldFeedrate;
+    }
-    } else if(!retracting && retracted[active_extruder]) {
+    else {
-      destination[X_AXIS]=current_position[X_AXIS];
+
-      destination[Y_AXIS]=current_position[Y_AXIS];
+      if (retract_zlift > 0.01) {
-      destination[Z_AXIS]=current_position[Z_AXIS];
+        current_position[Z_AXIS] + =retract_zlift;
-      destination[E_AXIS]=current_position[E_AXIS];
+        #ifdef DELTA
-      if(retract_zlift > 0.01) {
+          sync_plan_position_delta();
-         current_position[Z_AXIS]+=retract_zlift;
+        #else
-#ifdef DELTA
-         calculate_delta(current_position); // change cartesian kinematic  to  delta kinematic;
-         plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
-#else
           sync_plan_position();
-#endif
+        #endif
         //prepare_move();
       }
-      if (swapretract) {
+
-        current_position[E_AXIS]-=(retract_length_swap+retract_recover_length_swap)/volumetric_multiplier[active_extruder]; 
-      } else {
-        current_position[E_AXIS]-=(retract_length+retract_recover_length)/volumetric_multiplier[active_extruder]; 
-      }
-      plan_set_e_position(current_position[E_AXIS]);
-      float oldFeedrate = feedrate;
       feedrate = retract_recover_feedrate * 60;
-      retracted[active_extruder] = false;
+      float move_e = swapretract ? retract_length_swap + retract_recover_length_swap : retract_length + retract_recover_length;
+      current_position[E_AXIS] -= move_e / volumetric_multiplier[active_extruder];
+      plan_set_e_position(current_position[E_AXIS]);
       prepare_move();
-      feedrate = oldFeedrate;
     }
-  } //retract
+
-#endif //FWRETRACT
+    feedrate = oldFeedrate;
+    retracted[active_extruder] = retract;
+
+  } // retract()
+
+#endif // FWRETRACT
 
 #ifdef Z_PROBE_SLED
 
@@ -1623,16 +1631,14 @@ void refresh_cmd_timeout(void)
     #define SLED_DOCKING_OFFSET 0
   #endif
 
-//
+  //
-// Method to dock/undock a sled designed by Charles Bell.
+  // Method to dock/undock a sled designed by Charles Bell.
-//
+  //
-// dock[in]     If true, move to MAX_X and engage the electromagnet
+  // dock[in]     If true, move to MAX_X and engage the electromagnet
-// offset[in]   The additional distance to move to adjust docking location
+  // offset[in]   The additional distance to move to adjust docking location
-//
+  //
-static void dock_sled(bool dock, int offset=0) {
+  static void dock_sled(bool dock, int offset=0) {
- int z_loc;
+    if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
- 
- if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
       LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
       SERIAL_ECHO_START;
       SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
@@ -1640,23 +1646,17 @@ static void dock_sled(bool dock, int offset=0) {
     }
 
     if (dock) {
-   do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset,
+      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], current_position[Z_AXIS]);
-                       current_position[Y_AXIS],
+      digitalWrite(SERVO0_PIN, LOW); // turn off magnet
-                       current_position[Z_AXIS]);
-   // turn off magnet
-   digitalWrite(SERVO0_PIN, LOW);
     } else {
-   if (current_position[Z_AXIS] < (Z_RAISE_BEFORE_PROBING + 5))
+      float z_loc = current_position[Z_AXIS];
-     z_loc = Z_RAISE_BEFORE_PROBING;
+      if (z_loc < Z_RAISE_BEFORE_PROBING + 5) z_loc = Z_RAISE_BEFORE_PROBING;
-   else
+      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
-     z_loc = current_position[Z_AXIS];
+      digitalWrite(SERVO0_PIN, HIGH); // turn on magnet
-   do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset,
-                       Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
-   // turn on magnet
-   digitalWrite(SERVO0_PIN, HIGH);
     }
-}
+  }
-#endif
+
+#endif // Z_PROBE_SLED
 
 /**
  *
@@ -1798,7 +1798,7 @@ inline void gcode_G28() {
     feedrate = 1.732 * homing_feedrate[X_AXIS];
     line_to_destination();
     st_synchronize();
-    endstops_hit_on_purpose();
+    endstops_hit_on_purpose(); // clear endstop hit flags
 
     // Destination reached
     for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = destination[i];
@@ -1808,8 +1808,7 @@ inline void gcode_G28() {
     HOMEAXIS(Y);
     HOMEAXIS(Z);
 
-    calculate_delta(current_position);
+    sync_plan_position_delta();
-    plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
 
   #else // NOT DELTA
 
@@ -1817,7 +1816,7 @@ inline void gcode_G28() {
           homeY = code_seen(axis_codes[Y_AXIS]),
           homeZ = code_seen(axis_codes[Z_AXIS]);
 
-    home_all_axis = !homeX && !homeY && !homeZ; // No parameters means home all axes
+    home_all_axis = !(homeX || homeY || homeZ) || (homeX && homeY && homeZ);
 
     #if Z_HOME_DIR > 0                      // If homing away from BED do Z first
 
@@ -1836,7 +1835,9 @@ inline void gcode_G28() {
     #endif
 
     #ifdef QUICK_HOME
-      if (home_all_axis || (homeX && homeY)) {  //first diagonal move
+
+      if (home_all_axis || (homeX && homeY)) {  // First diagonal move
+
         current_position[X_AXIS] = current_position[Y_AXIS] = 0;
 
         #ifdef DUAL_X_CARRIAGE
@@ -1847,27 +1848,26 @@ inline void gcode_G28() {
         #endif
 
         sync_plan_position();
-        destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;
+
-        destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS);
+        float mlx = max_length(X_AXIS), mly = max_length(Y_AXIS),
-        feedrate = homing_feedrate[X_AXIS];
+              mlratio = mlx>mly ? mly/mlx : mlx/mly;
-        if (homing_feedrate[Y_AXIS] < feedrate) feedrate = homing_feedrate[Y_AXIS];
+
-        if (max_length(X_AXIS) > max_length(Y_AXIS)) {
+        destination[X_AXIS] = 1.5 * mlx * x_axis_home_dir;
-          feedrate *= sqrt(pow(max_length(Y_AXIS) / max_length(X_AXIS), 2) + 1);
+        destination[Y_AXIS] = 1.5 * mly * home_dir(Y_AXIS);
-        } else {
+        feedrate = min(homing_feedrate[X_AXIS], homing_feedrate[Y_AXIS]) * sqrt(mlratio * mlratio + 1);
-          feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
-        }
         line_to_destination();
         st_synchronize();
 
         axis_is_at_home(X_AXIS);
         axis_is_at_home(Y_AXIS);
         sync_plan_position();
+
         destination[X_AXIS] = current_position[X_AXIS];
         destination[Y_AXIS] = current_position[Y_AXIS];
         line_to_destination();
         feedrate = 0.0;
         st_synchronize();
-        endstops_hit_on_purpose();
+        endstops_hit_on_purpose(); // clear endstop hit flags
 
         current_position[X_AXIS] = destination[X_AXIS];
         current_position[Y_AXIS] = destination[Y_AXIS];
@@ -1875,7 +1875,8 @@ inline void gcode_G28() {
           current_position[Z_AXIS] = destination[Z_AXIS];
         #endif
       }
-    #endif //QUICK_HOME
+
+    #endif // QUICK_HOME
 
     // Home X
     if (home_all_axis || homeX) {
@@ -1957,7 +1958,7 @@ inline void gcode_G28() {
                 && cpy >= Y_MIN_POS - Y_PROBE_OFFSET_FROM_EXTRUDER
                 && cpy <= Y_MAX_POS - Y_PROBE_OFFSET_FROM_EXTRUDER) {
               current_position[Z_AXIS] = 0;
-              plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]);
+              plan_set_position(cpx, cpy, 0, current_position[E_AXIS]);
               destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS);    // Set destination away from bed
               feedrate = max_feedrate[Z_AXIS];
               line_to_destination();
@@ -1996,8 +1997,7 @@ inline void gcode_G28() {
   #endif // else DELTA
 
   #ifdef SCARA
-    calculate_delta(current_position);
+    sync_plan_position_delta();
-    plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
   #endif
 
   #ifdef ENDSTOPS_ONLY_FOR_HOMING
@@ -2024,7 +2024,7 @@ inline void gcode_G28() {
   feedrate = saved_feedrate;
   feedmultiply = saved_feedmultiply;
   previous_millis_cmd = millis();
-  endstops_hit_on_purpose();
+  endstops_hit_on_purpose(); // clear endstop hit flags
 }
 
 #if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING)
@@ -2196,8 +2196,7 @@ inline void gcode_G28() {
         bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t');
       #endif
 
-      if (verbose_level > 0)
+      if (verbose_level > 0) {
-      {
         SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n");
         if (dryrun) SERIAL_ECHOLN("Running in DRY-RUN mode");
       }
@@ -2272,7 +2271,6 @@ inline void gcode_G28() {
         current_position[Y_AXIS] = uncorrected_position.y;
         current_position[Z_AXIS] = uncorrected_position.z;
         sync_plan_position();
-
       #endif // !DELTA
     }
 
@@ -2283,8 +2281,8 @@ inline void gcode_G28() {
     #ifdef AUTO_BED_LEVELING_GRID
 
       // probe at the points of a lattice grid
-      const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1);
+      const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1),
-      const int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1);
+                yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
 
       #ifdef DELTA
         delta_grid_spacing[0] = xGridSpacing;
@@ -2842,9 +2840,7 @@ inline void gcode_M42() {
   inline void gcode_M48() {
 
     double sum = 0.0, mean = 0.0, sigma = 0.0, sample_set[50];
-    int verbose_level = 1, n = 0, j, n_samples = 10, n_legs = 0, engage_probe_for_each_reading = 0;
+    int verbose_level = 1, n_samples = 10, n_legs = 0;
-    double X_current, Y_current, Z_current;
-    double X_probe_location, Y_probe_location, Z_start_location, ext_position;
     
     if (code_seen('V') || code_seen('v')) {
       verbose_level = code_value();
@@ -2865,14 +2861,14 @@ inline void gcode_M42() {
       }
     }
 
-    X_current = X_probe_location = st_get_position_mm(X_AXIS);
+    double X_probe_location, Y_probe_location,
-    Y_current = Y_probe_location = st_get_position_mm(Y_AXIS);
+           X_current = X_probe_location = st_get_position_mm(X_AXIS),
-    Z_current = st_get_position_mm(Z_AXIS);
+           Y_current = Y_probe_location = st_get_position_mm(Y_AXIS),
-    Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
+           Z_current = st_get_position_mm(Z_AXIS),
+           Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING,
            ext_position = st_get_position_mm(E_AXIS);
 
-    if (code_seen('E') || code_seen('e'))
+    bool engage_probe_for_each_reading = code_seen('E') || code_seen('e');
-      engage_probe_for_each_reading++;
 
     if (code_seen('X') || code_seen('x')) {
       X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER;
@@ -2952,33 +2948,29 @@ inline void gcode_M42() {
 
     if (engage_probe_for_each_reading) retract_z_probe();
 
-    for (n=0; n < n_samples; n++) {
+    for (uint16_t n=0; n < n_samples; n++) {
 
-      do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
+      do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
 
       if (n_legs) {
-        double radius=0.0, theta=0.0;
+        unsigned long ms = millis();
-        int l;
+        double radius = ms % (X_MAX_LENGTH / 4),       // limit how far out to go
-        int rotational_direction = (unsigned long) millis() & 0x0001;     // clockwise or counter clockwise
+               theta = RADIANS(ms % 360L);
-        radius = (unsigned long)millis() % (long)(X_MAX_LENGTH / 4);      // limit how far out to go
+        float dir = (ms & 0x0001) ? 1 : -1;            // clockwise or counter clockwise
-        theta = (float)((unsigned long)millis() % 360L) / (360. / (2 * 3.1415926)); // turn into radians
 
         //SERIAL_ECHOPAIR("starting radius: ",radius);
         //SERIAL_ECHOPAIR("   theta: ",theta);
-        //SERIAL_ECHOPAIR("   direction: ",rotational_direction);
+        //SERIAL_ECHOPAIR("   direction: ",dir);
         //SERIAL_EOL;
 
-        float dir = rotational_direction ? 1 : -1;
+        for (int l = 0; l < n_legs - 1; l++) {
-        for (l = 0; l < n_legs - 1; l++) {
+          ms = millis();
-          theta += dir * (float)((unsigned long)millis() % 20L) / (360.0/(2*3.1415926)); // turn into radians
+          theta += RADIANS(dir * (ms % 20L));
-
+          radius += (ms % 10L) - 5L;
-          radius += (float)(((long)((unsigned long) millis() % 10L)) - 5L);
           if (radius < 0.0) radius = -radius;
 
           X_current = X_probe_location + cos(theta) * radius;
           Y_current = Y_probe_location + sin(theta) * radius;
-
-          // Make sure our X & Y are sane
           X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
           Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
 
@@ -2988,10 +2980,13 @@ inline void gcode_M42() {
             SERIAL_EOL;
           }
 
-          do_blocking_move_to( X_current, Y_current, Z_current );
+          do_blocking_move_to(X_current, Y_current, Z_current);
-        }
+
-        do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
+        } // n_legs loop
-      }
+
+        do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
+
+      } // n_legs
 
       if (engage_probe_for_each_reading)  {
         engage_z_probe(); 
@@ -3007,36 +3002,37 @@ inline void gcode_M42() {
       // Get the current mean for the data points we have so far
       //
       sum = 0.0;
-      for (j=0; j<=n; j++) sum += sample_set[j];
+      for (int j = 0; j <= n; j++) sum += sample_set[j];
-      mean = sum / (double (n+1));
+      mean = sum / (n + 1);
 
       //
       // Now, use that mean to calculate the standard deviation for the
       // data points we have so far
       //
       sum = 0.0;
-      for (j=0; j<=n; j++) sum += (sample_set[j]-mean) * (sample_set[j]-mean);
+      for (int j = 0; j <= n; j++) {
-      sigma = sqrt( sum / (double (n+1)) );
+        float ss = sample_set[j] - mean;
+        sum += ss * ss;
+      }
+      sigma = sqrt(sum / (n + 1));
 
       if (verbose_level > 1) {
         SERIAL_PROTOCOL(n+1);
-        SERIAL_PROTOCOL(" of ");
+        SERIAL_PROTOCOLPGM(" of ");
         SERIAL_PROTOCOL(n_samples);
         SERIAL_PROTOCOLPGM("   z: ");
         SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6);
-      }
-
         if (verbose_level > 2) {
-        SERIAL_PROTOCOL(" mean: ");
+          SERIAL_PROTOCOLPGM(" mean: ");
           SERIAL_PROTOCOL_F(mean,6);
-        SERIAL_PROTOCOL("   sigma: ");
+          SERIAL_PROTOCOLPGM("   sigma: ");
           SERIAL_PROTOCOL_F(sigma,6);
         }
+      }
 
       if (verbose_level > 0) SERIAL_EOL;
 
-      plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location,
+      plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
-          current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
       st_synchronize();
 
       if (engage_probe_for_each_reading) {
@@ -3045,8 +3041,10 @@ inline void gcode_M42() {
       }
     }
 
+    if (!engage_probe_for_each_reading) {
       retract_z_probe();
       delay(1000);
+    }
 
     clean_up_after_endstop_move();
 
@@ -4688,9 +4686,7 @@ inline void gcode_T() {
           active_extruder = tmp_extruder;
         #endif // !DUAL_X_CARRIAGE
         #ifdef DELTA
-          calculate_delta(current_position); // change cartesian kinematic  to  delta kinematic;
+          sync_plan_position_delta();
-          //sent position to plan_set_position();
-          plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],current_position[E_AXIS]);
         #else
           sync_plan_position();
         #endif
@@ -5279,19 +5275,18 @@ void clamp_to_software_endstops(float target[3])
 }
 
 #ifdef DELTA
-void recalc_delta_settings(float radius, float diagonal_rod)
-{
-   delta_tower1_x= -SIN_60*radius; // front left tower
-   delta_tower1_y= -COS_60*radius;     
-   delta_tower2_x=  SIN_60*radius; // front right tower
-   delta_tower2_y= -COS_60*radius;     
-   delta_tower3_x= 0.0;                  // back middle tower
-   delta_tower3_y= radius;
-   delta_diagonal_rod_2= sq(diagonal_rod);
-}
 
-void calculate_delta(float cartesian[3])
+  void recalc_delta_settings(float radius, float diagonal_rod) {
-{
+    delta_tower1_x = -SIN_60 * radius;  // front left tower
+    delta_tower1_y = -COS_60 * radius;
+    delta_tower2_x =  SIN_60 * radius;  // front right tower
+    delta_tower2_y = -COS_60 * radius;
+    delta_tower3_x = 0.0;               // back middle tower
+    delta_tower3_y = radius;
+    delta_diagonal_rod_2 = sq(diagonal_rod);
+  }
+
+  void calculate_delta(float cartesian[3]) {
     delta[X_AXIS] = sqrt(delta_diagonal_rod_2
                          - sq(delta_tower1_x-cartesian[X_AXIS])
                          - sq(delta_tower1_y-cartesian[Y_AXIS])
@@ -5313,30 +5308,28 @@ void calculate_delta(float cartesian[3])
     SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
     SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
     */
-}
+  }
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+  #ifdef ENABLE_AUTO_BED_LEVELING
-// Adjust print surface height by linear interpolation over the bed_level array.
+
-int delta_grid_spacing[2] = { 0, 0 };
+    // Adjust print surface height by linear interpolation over the bed_level array.
-void adjust_delta(float cartesian[3])
+    int delta_grid_spacing[2] = { 0, 0 };
-{
+    void adjust_delta(float cartesian[3]) {
-  if (delta_grid_spacing[0] == 0 || delta_grid_spacing[1] == 0)
+      if (delta_grid_spacing[0] == 0 || delta_grid_spacing[1] == 0) return; // G29 not done!
-    return; // G29 not done
 
       int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
-  float grid_x = max(0.001-half, min(half-0.001, cartesian[X_AXIS] / delta_grid_spacing[0]));
+      float h1 = 0.001 - half, h2 = half - 0.001,
-  float grid_y = max(0.001-half, min(half-0.001, cartesian[Y_AXIS] / delta_grid_spacing[1]));
+            grid_x = max(h1, min(h2, cartesian[X_AXIS] / delta_grid_spacing[0])),
-  int floor_x = floor(grid_x);
+            grid_y = max(h1, min(h2, cartesian[Y_AXIS] / delta_grid_spacing[1]));
-  int floor_y = floor(grid_y);
+      int floor_x = floor(grid_x), floor_y = floor(grid_y);
-  float ratio_x = grid_x - floor_x;
+      float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
-  float ratio_y = grid_y - floor_y;
+            z1 = bed_level[floor_x + half][floor_y + half],
-  float z1 = bed_level[floor_x+half][floor_y+half];
+            z2 = bed_level[floor_x + half][floor_y + half + 1],
-  float z2 = bed_level[floor_x+half][floor_y+half+1];
+            z3 = bed_level[floor_x + half + 1][floor_y + half],
-  float z3 = bed_level[floor_x+half+1][floor_y+half];
+            z4 = bed_level[floor_x + half + 1][floor_y + half + 1],
-  float z4 = bed_level[floor_x+half+1][floor_y+half+1];
+            left = (1 - ratio_y) * z1 + ratio_y * z2,
-  float left = (1-ratio_y)*z1 + ratio_y*z2;
+            right = (1 - ratio_y) * z3 + ratio_y * z4,
-  float right = (1-ratio_y)*z3 + ratio_y*z4;
+            offset = (1 - ratio_x) * left + ratio_x * right;
-  float offset = (1-ratio_x)*left + ratio_x*right;
 
       delta[X_AXIS] += offset;
       delta[Y_AXIS] += offset;
@@ -5357,26 +5350,24 @@ void adjust_delta(float cartesian[3])
       SERIAL_ECHOPGM(" right="); SERIAL_ECHO(right);
       SERIAL_ECHOPGM(" offset="); SERIAL_ECHOLN(offset);
       */
-}
+    }
-#endif //ENABLE_AUTO_BED_LEVELING
+  #endif // ENABLE_AUTO_BED_LEVELING
 
-void prepare_move_raw()
+  void prepare_move_raw() {
-{
     previous_millis_cmd = millis();
     calculate_delta(destination);
-  plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+    plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
-                   destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+    for (int i = 0; i < NUM_AXIS; i++) current_position[i] = destination[i];
-                   active_extruder);
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
   }
-}
+
-#endif //DELTA
+#endif // DELTA
 
 #if defined(MESH_BED_LEVELING)
-#if !defined(MIN)
+
-#define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
+  #if !defined(MIN)
-#endif  // ! MIN
+    #define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
+  #endif  // ! MIN
+
 // This function is used to split lines on mesh borders so each segment is only part of one mesh area
 void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t &extruder, uint8_t x_splits=0xff, uint8_t y_splits=0xff)
 {
@@ -5448,8 +5439,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
 }
 #endif  // MESH_BED_LEVELING
 
-void prepare_move()
+void prepare_move() {
-{
   clamp_to_software_endstops(destination);
   previous_millis_cmd = millis();
   
@@ -5563,7 +5553,7 @@ void prepare_move()
   }
 #endif //DUAL_X_CARRIAGE
 
-#if ! (defined DELTA || defined SCARA)
+#if !defined(DELTA) && !defined(SCARA)
   // 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])) {
     line_to_destination();

Marlin/Sd2PinMap.h

@@ -312,6 +312,75 @@ static const pin_map_t digitalPinMap[] = {
   {&DDRC, &PINC, &PORTC, 4},  // C4 18
   {&DDRC, &PINC, &PORTC, 5}   // C5 19
 };
+#elif defined(__AVR_ATmega1281__)
+// Waspmote
+ 
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 41;
+uint8_t const SCL_PIN = 40;
+ 
+ 
+#undef MOSI_PIN
+#undef MISO_PIN
+// SPI port
+uint8_t const SS_PIN = 16;    // B0
+uint8_t const MOSI_PIN = 11;  // B2
+uint8_t const MISO_PIN = 12;  // B3
+uint8_t const SCK_PIN = 10;   // B1
+ 
+static const pin_map_t digitalPinMap[] = {
+    {&DDRE, &PINE, &PORTE, 0}, // E0 0
+    {&DDRE, &PINE, &PORTE, 1}, // E1 1
+    {&DDRE, &PINE, &PORTE, 3}, // E3 2
+    {&DDRE, &PINE, &PORTE, 4}, // E4 3
+    {&DDRC, &PINC, &PORTC, 4}, // C4 4
+    {&DDRC, &PINC, &PORTC, 5}, // C5 5
+    {&DDRC, &PINC, &PORTC, 6}, // C6 6
+    {&DDRC, &PINC, &PORTC, 7}, // C7 7
+    {&DDRA, &PINA, &PORTA, 2}, // A2 8
+    {&DDRA, &PINA, &PORTA, 3}, // A3 9
+    {&DDRA, &PINA, &PORTA, 4}, // A4 10
+    {&DDRD, &PIND, &PORTD, 5}, // D5 11
+    {&DDRD, &PIND, &PORTD, 6}, // D6 12
+    {&DDRC, &PINC, &PORTC, 1}, // C1 13
+    {&DDRF, &PINF, &PORTF, 1}, // F1 14
+    {&DDRF, &PINF, &PORTF, 2}, // F2 15
+    {&DDRF, &PINF, &PORTF, 3}, // F3 16
+    {&DDRF, &PINF, &PORTF, 4}, // F4 17
+    {&DDRF, &PINF, &PORTF, 5}, // F5 18
+    {&DDRF, &PINF, &PORTF, 6}, // F6 19
+    {&DDRF, &PINF, &PORTF, 7}, // F7 20
+    {&DDRF, &PINF, &PORTF, 0}, // F0 21
+    {&DDRA, &PINA, &PORTA, 1}, // A1 22
+    {&DDRD, &PIND, &PORTD, 7}, // D7 23
+    {&DDRE, &PINE, &PORTE, 5}, // E5 24
+    {&DDRA, &PINA, &PORTA, 6}, // A6 25
+    {&DDRE, &PINE, &PORTE, 2}, // E2 26
+    {&DDRA, &PINA, &PORTA, 5}, // A5 27
+    {&DDRC, &PINC, &PORTC, 0}, // C0 28
+    {&DDRB, &PINB, &PORTB, 0}, // B0 29
+    {&DDRB, &PINB, &PORTB, 1}, // B1 30
+    {&DDRB, &PINB, &PORTB, 2}, // B2 31
+    {&DDRB, &PINB, &PORTB, 3}, // B3 32
+    {&DDRB, &PINB, &PORTB, 4}, // B4 33
+    {&DDRB, &PINB, &PORTB, 5}, // B5 34
+    {&DDRA, &PINA, &PORTA, 0}, // A0 35
+    {&DDRB, &PINB, &PORTB, 6}, // B6 36
+    {&DDRB, &PINB, &PORTB, 7}, // B7 37
+    {&DDRE, &PINE, &PORTE, 6}, // E6 38
+    {&DDRE, &PINE, &PORTE, 7}, // E7 39
+    {&DDRD, &PIND, &PORTD, 0}, // D0 40
+    {&DDRD, &PIND, &PORTD, 1}, // D1 41
+    {&DDRC, &PINC, &PORTC, 3}, // C3 42
+    {&DDRD, &PIND, &PORTD, 2}, // D2 43
+    {&DDRD, &PIND, &PORTD, 3}, // D3 44
+    {&DDRA, &PINA, &PORTA, 7}, // A7 45
+    {&DDRC, &PINC, &PORTC, 2}, // C2 46
+    {&DDRD, &PIND, &PORTD, 4}, // D4 47
+    {&DDRG, &PING, &PORTG, 2}, // G2 48
+    {&DDRG, &PING, &PORTG, 1}, // G1 49
+    {&DDRG, &PING, &PORTG, 0}, // G0 50
+};
 #else  // defined(__AVR_ATmega1280__)
 #error unknown chip
 #endif  // defined(__AVR_ATmega1280__)

Marlin/ultralcd.h

@@ -99,7 +99,8 @@
 #else //no LCD
   FORCE_INLINE void lcd_update() {}
   FORCE_INLINE void lcd_init() {}
-  FORCE_INLINE void lcd_setstatus(const char* message) {}
+  FORCE_INLINE void lcd_setstatus(const char* message, const bool persist=false) {}
+  FORCE_INLINE void lcd_setstatuspgm(const char* message, const uint8_t level=0) {}
   FORCE_INLINE void lcd_buttons_update() {}
   FORCE_INLINE void lcd_reset_alert_level() {}
   FORCE_INLINE void lcd_buzz(long duration,uint16_t freq) {}