Browse Source

Adapt speed/jerk code based on Prusa MK2 branch

Scott Lahteine 8 years ago
parent
commit
1092319b19
1 changed files with 142 additions and 59 deletions
  1. 142
    59
      Marlin/planner.cpp

+ 142
- 59
Marlin/planner.cpp View File

@@ -85,8 +85,8 @@ float Planner::max_feedrate_mm_s[NUM_AXIS], // Max speeds in mm per second
85 85
       Planner::axis_steps_per_mm[NUM_AXIS],
86 86
       Planner::steps_to_mm[NUM_AXIS];
87 87
 
88
-unsigned long Planner::max_acceleration_steps_per_s2[NUM_AXIS],
89
-              Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
88
+uint32_t Planner::max_acceleration_steps_per_s2[NUM_AXIS],
89
+         Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
90 90
 
91 91
 millis_t Planner::min_segment_time;
92 92
 float Planner::min_feedrate_mm_s,
@@ -236,6 +236,7 @@ void Planner::reverse_pass() {
236 236
 
237 237
     uint8_t b = BLOCK_MOD(block_buffer_head - 3);
238 238
     while (b != tail) {
239
+      if (block[0] && (block[0]->flag & BLOCK_FLAG_START_FROM_FULL_HALT)) break;
239 240
       b = prev_block_index(b);
240 241
       block[2] = block[1];
241 242
       block[1] = block[0];
@@ -696,6 +697,9 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
696 697
   // Bail if this is a zero-length block
697 698
   if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
698 699
 
700
+  // Clear the block flags
701
+  block->flag = 0;
702
+
699 703
   // For a mixing extruder, get a magnified step_event_count for each
700 704
   #if ENABLED(MIXING_EXTRUDER)
701 705
     for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
@@ -1011,90 +1015,170 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
1011 1015
 
1012 1016
   // Compute and limit the acceleration rate for the trapezoid generator.
1013 1017
   float steps_per_mm = block->step_event_count / block->millimeters;
1018
+  uint32_t accel;
1014 1019
   if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
1015
-    block->acceleration_steps_per_s2 = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
1020
+    // convert to: acceleration steps/sec^2
1021
+    accel = ceil(retract_acceleration * steps_per_mm);
1016 1022
   }
1017 1023
   else {
1024
+    #define LIMIT_ACCEL(AXIS) do{ \
1025
+      const uint32_t comp = max_acceleration_steps_per_s2[AXIS] * block->step_event_count; \
1026
+      if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
1027
+    }while(0)
1028
+
1029
+    // Start with print or travel acceleration
1030
+    accel = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
1031
+
1018 1032
     // Limit acceleration per axis
1019
-    block->acceleration_steps_per_s2 = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
1020
-    if (max_acceleration_steps_per_s2[X_AXIS] < (block->acceleration_steps_per_s2 * block->steps[X_AXIS]) / block->step_event_count)
1021
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[X_AXIS] * block->step_event_count) / block->steps[X_AXIS];
1022
-    if (max_acceleration_steps_per_s2[Y_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Y_AXIS]) / block->step_event_count)
1023
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Y_AXIS] * block->step_event_count) / block->steps[Y_AXIS];
1024
-    if (max_acceleration_steps_per_s2[Z_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Z_AXIS]) / block->step_event_count)
1025
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Z_AXIS] * block->step_event_count) / block->steps[Z_AXIS];
1026
-    if (max_acceleration_steps_per_s2[E_AXIS] < (block->acceleration_steps_per_s2 * block->steps[E_AXIS]) / block->step_event_count)
1027
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[E_AXIS] * block->step_event_count) / block->steps[E_AXIS];
1033
+    LIMIT_ACCEL(X_AXIS);
1034
+    LIMIT_ACCEL(Y_AXIS);
1035
+    LIMIT_ACCEL(Z_AXIS);
1036
+    LIMIT_ACCEL(E_AXIS);
1028 1037
   }
1029
-  block->acceleration = block->acceleration_steps_per_s2 / steps_per_mm;
1030
-  block->acceleration_rate = (long)(block->acceleration_steps_per_s2 * 16777216.0 / ((F_CPU) * 0.125));
1038
+  block->acceleration_steps_per_s2 = accel;
1039
+  block->acceleration = accel / steps_per_mm;
1040
+  block->acceleration_rate = (long)(accel * 16777216.0 / ((F_CPU) * 0.125)); // * 8.388608
1041
+
1042
+  // Initial limit on the segment entry velocity
1043
+  float vmax_junction;
1031 1044
 
1032 1045
   #if 0  // Use old jerk for now
1033 1046
 
1034 1047
     float junction_deviation = 0.1;
1035 1048
 
1036 1049
     // Compute path unit vector
1037
-    double unit_vec[XYZ];
1038
-
1039
-    unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters;
1040
-    unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters;
1041
-    unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters;
1042
-
1043
-    // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
1044
-    // Let a circle be tangent to both previous and current path line segments, where the junction
1045
-    // deviation is defined as the distance from the junction to the closest edge of the circle,
1046
-    // collinear with the circle center. The circular segment joining the two paths represents the
1047
-    // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
1048
-    // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
1049
-    // path width or max_jerk in the previous grbl version. This approach does not actually deviate
1050
-    // from path, but used as a robust way to compute cornering speeds, as it takes into account the
1051
-    // nonlinearities of both the junction angle and junction velocity.
1052
-    double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
1050
+    double unit_vec[XYZ] = {
1051
+      delta_mm[X_AXIS] * inverse_millimeters,
1052
+      delta_mm[Y_AXIS] * inverse_millimeters,
1053
+      delta_mm[Z_AXIS] * inverse_millimeters
1054
+    };
1055
+
1056
+    /*
1057
+       Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
1058
+
1059
+       Let a circle be tangent to both previous and current path line segments, where the junction
1060
+       deviation is defined as the distance from the junction to the closest edge of the circle,
1061
+       collinear with the circle center.
1062
+
1063
+       The circular segment joining the two paths represents the path of centripetal acceleration.
1064
+       Solve for max velocity based on max acceleration about the radius of the circle, defined
1065
+       indirectly by junction deviation.
1066
+
1067
+       This may be also viewed as path width or max_jerk in the previous grbl version. This approach
1068
+       does not actually deviate from path, but used as a robust way to compute cornering speeds, as
1069
+       it takes into account the nonlinearities of both the junction angle and junction velocity.
1070
+     */
1071
+
1072
+    vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
1053 1073
 
1054 1074
     // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
1055
-    if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
1075
+    if (block_buffer_head != block_buffer_tail && previous_nominal_speed > 0.0) {
1056 1076
       // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
1057 1077
       // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
1058
-      double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
1059
-                         - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
1060
-                         - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
1078
+      float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
1079
+                        - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
1080
+                        - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
1061 1081
       // Skip and use default max junction speed for 0 degree acute junction.
1062 1082
       if (cos_theta < 0.95) {
1063 1083
         vmax_junction = min(previous_nominal_speed, block->nominal_speed);
1064 1084
         // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
1065 1085
         if (cos_theta > -0.95) {
1066 1086
           // Compute maximum junction velocity based on maximum acceleration and junction deviation
1067
-          double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
1087
+          float sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
1068 1088
           NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
1069 1089
         }
1070 1090
       }
1071 1091
     }
1072 1092
   #endif
1073 1093
 
1074
-  // Start with a safe speed
1075
-  float vmax_junction = max_jerk[X_AXIS] * 0.5, vmax_junction_factor = 1.0;
1076
-  if (max_jerk[Y_AXIS] * 0.5 < fabs(current_speed[Y_AXIS])) NOMORE(vmax_junction, max_jerk[Y_AXIS] * 0.5);
1077
-  if (max_jerk[Z_AXIS] * 0.5 < fabs(current_speed[Z_AXIS])) NOMORE(vmax_junction, max_jerk[Z_AXIS] * 0.5);
1078
-  if (max_jerk[E_AXIS] * 0.5 < fabs(current_speed[E_AXIS])) NOMORE(vmax_junction, max_jerk[E_AXIS] * 0.5);
1079
-  NOMORE(vmax_junction, block->nominal_speed);
1080
-  float safe_speed = vmax_junction;
1094
+  /**
1095
+   * Adapted from Prusa MKS firmware
1096
+   *
1097
+   * Start with a safe speed (from which the machine may halt to stop immediately).
1098
+   */
1099
+
1100
+  // Exit speed limited by a jerk to full halt of a previous last segment
1101
+  static float previous_safe_speed;
1102
+
1103
+  float safe_speed = block->nominal_speed;
1104
+  bool limited = false;
1105
+  LOOP_XYZE(i) {
1106
+    float jerk = fabs(current_speed[i]);
1107
+    if (jerk > max_jerk[i]) {
1108
+      // The actual jerk is lower if it has been limited by the XY jerk.
1109
+      if (limited) {
1110
+        // Spare one division by a following gymnastics:
1111
+        // Instead of jerk *= safe_speed / block->nominal_speed,
1112
+        // multiply max_jerk[i] by the divisor.
1113
+        jerk *= safe_speed;
1114
+        float mjerk = max_jerk[i] * block->nominal_speed;
1115
+        if (jerk > mjerk) safe_speed *= mjerk / jerk;
1116
+      }
1117
+      else {
1118
+        safe_speed = max_jerk[i];
1119
+        limited = true;
1120
+      }
1121
+    }
1122
+  }
1081 1123
 
1082 1124
   if (moves_queued > 1 && previous_nominal_speed > 0.0001) {
1083
-    //if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
1084
-        vmax_junction = block->nominal_speed;
1085
-    //}
1086
-
1087
-    float dsx = fabs(current_speed[X_AXIS] - previous_speed[X_AXIS]),
1088
-          dsy = fabs(current_speed[Y_AXIS] - previous_speed[Y_AXIS]),
1089
-          dsz = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]),
1090
-          dse = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]);
1091
-    if (dsx > max_jerk[X_AXIS]) NOMORE(vmax_junction_factor, max_jerk[X_AXIS] / dsx);
1092
-    if (dsy > max_jerk[Y_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Y_AXIS] / dsy);
1093
-    if (dsz > max_jerk[Z_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Z_AXIS] / dsz);
1094
-    if (dse > max_jerk[E_AXIS]) NOMORE(vmax_junction_factor, max_jerk[E_AXIS] / dse);
1095
-
1096
-    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
1125
+    // Estimate a maximum velocity allowed at a joint of two successive segments.
1126
+    // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
1127
+    // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
1128
+
1129
+    // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
1130
+    bool prev_speed_larger = previous_nominal_speed > block->nominal_speed;
1131
+    float smaller_speed_factor = prev_speed_larger ? (block->nominal_speed / previous_nominal_speed) : (previous_nominal_speed / block->nominal_speed);
1132
+    // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
1133
+    vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
1134
+    // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
1135
+    float v_factor = 1.f;
1136
+    limited = false;
1137
+    // Now limit the jerk in all axes.
1138
+    LOOP_XYZE(axis) {
1139
+      // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
1140
+      float v_exit = previous_speed[axis], v_entry = current_speed[axis];
1141
+      if (prev_speed_larger) v_exit *= smaller_speed_factor;
1142
+      if (limited) {
1143
+        v_exit *= v_factor;
1144
+        v_entry *= v_factor;
1145
+      }
1146
+      // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
1147
+      float jerk = 
1148
+        (v_exit > v_entry) ?
1149
+          ((v_entry > 0.f || v_exit < 0.f) ?
1150
+            // coasting
1151
+            (v_exit - v_entry) : 
1152
+            // axis reversal
1153
+            max(v_exit, -v_entry)) :
1154
+          // v_exit <= v_entry
1155
+          ((v_entry < 0.f || v_exit > 0.f) ?
1156
+            // coasting
1157
+            (v_entry - v_exit) :
1158
+            // axis reversal
1159
+            max(-v_exit, v_entry));
1160
+      if (jerk > max_jerk[axis]) {
1161
+        v_factor *= max_jerk[axis] / jerk;
1162
+        limited = true;
1163
+      }
1164
+    }
1165
+    if (limited) vmax_junction *= v_factor;
1166
+    // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
1167
+    // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
1168
+    float vmax_junction_threshold = vmax_junction * 0.99f;
1169
+    if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
1170
+      // Not coasting. The machine will stop and start the movements anyway,
1171
+      // better to start the segment from start.
1172
+      block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
1173
+      vmax_junction = safe_speed;
1174
+    }
1097 1175
   }
1176
+  else {
1177
+    block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
1178
+    vmax_junction = safe_speed;
1179
+  }
1180
+
1181
+  // Max entry speed of this block equals the max exit speed of the previous block.
1098 1182
   block->max_entry_speed = vmax_junction;
1099 1183
 
1100 1184
   // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
@@ -1109,13 +1193,12 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
1109 1193
   // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
1110 1194
   // the reverse and forward planners, the corresponding block junction speed will always be at the
1111 1195
   // the maximum junction speed and may always be ignored for any speed reduction checks.
1112
-  block->flag &= ~BLOCK_FLAG_NOMINAL_LENGTH;
1113
-  if (block->nominal_speed <= v_allowable) block->flag |= BLOCK_FLAG_NOMINAL_LENGTH;
1114
-  block->flag |= BLOCK_FLAG_RECALCULATE; // Always calculate trapezoid for new block
1196
+  block->flag |= BLOCK_FLAG_RECALCULATE | (block->nominal_speed <= v_allowable ? BLOCK_FLAG_NOMINAL_LENGTH : 0);
1115 1197
 
1116 1198
   // Update previous path unit_vector and nominal speed
1117 1199
   memcpy(previous_speed, current_speed, sizeof(previous_speed));
1118 1200
   previous_nominal_speed = block->nominal_speed;
1201
+  previous_safe_speed = safe_speed;
1119 1202
 
1120 1203
   #if ENABLED(LIN_ADVANCE)
1121 1204
 

Loading…
Cancel
Save