Some fixes in planner

Marlin/Configuration.h

 
   #ifdef PID_PID
     //PID according to Ziegler-Nichols method
-    #define  DEFAULT_Kp  (0.6*PID_CRITIAL_GAIN)
-    #define  DEFAULT_Ki (2*Kp/PID_SWING_AT_CRITIAL*PID_dT)  
-    #define  DEFAULT_Kd (PID_SWING_AT_CRITIAL/8./PID_dT)  
+//    #define  DEFAULT_Kp  (0.6*PID_CRITIAL_GAIN)
+//    #define  DEFAULT_Ki (2*Kp/PID_SWING_AT_CRITIAL*PID_dT)  
+//    #define  DEFAULT_Kd (PID_SWING_AT_CRITIAL/8./PID_dT)  
+
+    #define  DEFAULT_Kp  22.2
+    #define  DEFAULT_Ki (1.25*PID_dT)  
+    #define  DEFAULT_Kd (99/PID_dT)  
   #endif
- 
+   
   #ifdef PID_PI
     //PI according to Ziegler-Nichols method
     #define  DEFAULT_Kp (PID_CRITIAL_GAIN/2.2) 
 
 #define AXIS_RELATIVE_MODES {false, false, false, false}
 
-#define MAX_STEP_FREQUENCY 40000 // Max step frequency for Ultimaker (5000 pps / half step)
+#define MAX_STEP_FREQUENCY 40000L // Max step frequency for Ultimaker (5000 pps / half step)
 
 // default settings 
 
-#define DEFAULT_AXIS_STEPS_PER_UNIT   {79.87220447,79.87220447,200*8/3,14}                    // default steps per unit for ultimaker 
+#define DEFAULT_AXIS_STEPS_PER_UNIT   {79.87220447,79.87220447,200*8/3,760*1.1}                    // default steps per unit for ultimaker 
 //#define DEFAULT_AXIS_STEPS_PER_UNIT   {40, 40, 3333.92, 67} 
-#define DEFAULT_MAX_FEEDRATE          {500, 500, 10, 500000}    // (mm/min)    
+#define DEFAULT_MAX_FEEDRATE          {500, 500, 5, 200000}    // (mm/sec)    
 #define DEFAULT_MAX_ACCELERATION      {9000,9000,100,10000}    // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for skeinforge 40+, for older versions raise them a lot.
 
 #define DEFAULT_ACCELERATION          3000    // X, Y, Z and E max acceleration in mm/s^2 for printing moves 
 #define DEFAULT_RETRACT_ACCELERATION  7000   // X, Y, Z and E max acceleration in mm/s^2 for r retracts
 
-#define DEFAULT_MINIMUMFEEDRATE       0     // minimum feedrate
-#define DEFAULT_MINTRAVELFEEDRATE     0
+#define DEFAULT_MINIMUMFEEDRATE       0.0     // minimum feedrate
+#define DEFAULT_MINTRAVELFEEDRATE     0.0
 
 // minimum time in microseconds that a movement needs to take if the buffer is emptied.   Increase this number if you see blobs while printing high speed & high detail.  It will slowdown on the detailed stuff.
 #define DEFAULT_MINSEGMENTTIME        20000   // Obsolete delete this

Marlin/planner.cpp

   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];
   block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) +
-                            square(delta_mm[Z_AXIS]));
+                            square(delta_mm[Z_AXIS]) + square(delta_mm[E_AXIS]));
   float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides 
   
   // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
   //  segment time im micro seconds
   long segment_time = lround(1000000.0/inverse_second);
  
- 
+
   if (block->steps_e == 0) {
         if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
   }
 #endif
 
 /*
-
-  
   if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) {
     if (segment_time<minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
         segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/blockcount);
   //  END OF SLOW DOWN SECTION    
 */
 
+
  // Calculate speed in mm/sec for each axis
   float current_speed[4];
   for(int i=0; i < 4; i++) {
   }
 
 // Max segement time in us.
-
 #ifdef XY_FREQUENCY_LIMIT
 #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
 
   if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
 #endif
 
-
   // Correct the speed  
   if( speed_factor < 1.0) {
 //    Serial.print("speed factor : "); Serial.println(speed_factor);

Marlin/stepper.cpp

   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
   
   if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
-    step_rate = step_rate >> 2;
+    step_rate = (step_rate >> 2)&0x3fff;
     step_loops = 4;
   }
   else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
-    step_rate = step_rate >> 1;
+    step_rate = (step_rate >> 1)&0x7fff;
     step_loops = 2;
   }
   else {
     timer = (unsigned short)pgm_read_word_near(table_address);
     timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
   }
-  //if(timer < 100) timer = 100;
+  if(timer < 100) timer = 100; //(20kHz this should never happen)
   return timer;
 }
 
       #endif
       #if X_MIN_PIN > -1
             if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) {
-              endstops_triggered(step_events_completed);
+ //             endstops_triggered(step_events_completed);
               step_events_completed = current_block->step_event_count;
             }
       #endif
       #endif
       #if X_MAX_PIN > -1
         if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING)  && (current_block->steps_x >0)){
-          endstops_triggered(step_events_completed);
+ //         endstops_triggered(step_events_completed);
           step_events_completed = current_block->step_event_count;
         }
         #endif
       #endif
       #if Y_MIN_PIN > -1
         if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) {
-          endstops_triggered(step_events_completed);
+ //         endstops_triggered(step_events_completed);
           step_events_completed = current_block->step_event_count;
         }
       #endif
       #endif
       #if Y_MAX_PIN > -1
       if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){
-          endstops_triggered(step_events_completed);
+ //         endstops_triggered(step_events_completed);
           step_events_completed = current_block->step_event_count;
         }
       #endif
       #endif
       #if Z_MAX_PIN > -1
         if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING)  && (current_block->steps_z >0)){
-          endstops_triggered(step_events_completed);
+ //         endstops_triggered(step_events_completed);
           step_events_completed = current_block->step_event_count;
         }
       #endif