Jerk Acceleration Settings and HAAS G187 Acceleration Profiles

config.h

@@ -189,7 +189,10 @@ or EMI triggering the related interrupt falsely or too many times.
 // ---------------------------------------------------------------------------------------
 // ADVANCED CONFIGURATION OPTIONS:
 
+// EXPERIMENTAL OPTIONS
 #define ENABLE_PATH_BLENDING Off // Do NOT enable unless working on adding this feature!
+#define ENABLE_ACCELERATION_PROFILES Off // Enable to allow G-Code changeable acceleration profiles.
+#define ENABLE_JERK_ACCELERATION Off // Enable to use 3rd order Acceleration calculations. May need more processing power, tiny chips beware. 
 
 // Enables code for debugging purposes. Not for general use and always in constant flux.
 //#define DEBUG // Uncomment to enable. Default disabled.
@@ -1922,6 +1925,37 @@ G90
 #endif
 ///@}
 
+/*! @name 22x - Setting_AxisJerk
+*/
+///@{
+#if ENABLE_JERK_ACCELERATION
+#if !defined DEFAULT_X_JERK|| defined __DOXYGEN__
+#define DEFAULT_X_JERK 100.0f // mm/sec^3
+#endif
+#if !defined DEFAULT_Y_JERK|| defined __DOXYGEN__
+#define DEFAULT_Y_JERK 100.0f // mm/sec^3
+#endif
+#if !defined DEFAULT_Z_JERK || defined __DOXYGEN__
+#define DEFAULT_Z_JERK 100.0f // mm/sec^3
+#endif
+#if (defined A_AXIS && !defined DEFAULT_A_JERK) || defined __DOXYGEN__
+#define DEFAULT_A_JERK 100.0f // mm/sec^3
+#endif
+#if (defined B_AXIS && !defined DEFAULT_B_JERK) || defined __DOXYGEN__
+#define DEFAULT_B_JERK 100.0f // mm/sec^3
+#endif
+#if (defined C_AXIS && !defined DEFAULT_C_JERK) || defined __DOXYGEN__
+#define DEFAULT_C_JERK 100.0f // mm/sec^3
+#endif
+#if (defined U_AXIS && !defined DEFAULT_U_JERK) || defined __DOXYGEN__
+#define DEFAULT_U_JERK 100.0f // mm/sec^3
+#endif
+#if (defined V_AXIS && !defined DEFAULT_V_JERK) || defined __DOXYGEN__
+#define DEFAULT_V_JERK 100.0f // mm/sec^3
+#endif
+#endif
+///@}
+
 /*! @name 13x - Setting_AxisMaxTravel
 __NOTE:__ Must be a positive values.
 */

gcode.c

@@ -325,6 +325,9 @@ void gc_init (void)
 #if NGC_PARAMETERS_ENABLE
     ngc_modal_state_invalidate();
 #endif
+#if ENABLE_ACCELERATION_PROFILES
+    gc_state.modal.activeaccelprofile = 1.0f; // Initialize machine with 100% Profile
+#endif
 
 //    if(settings.flags.lathe_mode)
 //        gc_state.modal.plane_select = PlaneSelect_ZX;
@@ -1291,6 +1294,15 @@ status_code_t gc_execute_block (char *block)
                         gc_block.modal.scaling_active = int_value == 51;
                         break;
 
+#if ENABLE_ACCELERATION_PROFILES
+                    case 187:
+                        word_bit.modal_group.G0 = On;
+                        gc_block.non_modal_command = (non_modal_t)int_value;
+                        if(mantissa != 0)
+                            FAIL(Status_GcodeUnsupportedCommand);
+                        break;
+#endif
+
                     default: FAIL(Status_GcodeUnsupportedCommand); // [Unsupported G command]
                 } // end G-value switch
 
@@ -2436,7 +2448,21 @@ status_code_t gc_execute_block (char *block)
                     gc_block.values.xyz[idx] = gc_state.g92_coord_offset[idx];
             } while(idx);
             break;
-
+
+#if ENABLE_ACCELERATION_PROFILES
+        case NonModal_SetAccelerationProfile:
+            if (gc_block.words.e){
+                FAIL(Status_GcodeUnsupportedCommand);
+                break;}
+            else if (!gc_block.word.p){
+                gc_block.values.p = 1.0f;}
+            else if (gc_block.values.p < 1.0f){
+                FAIL(Status_NegativeValue);}
+            else if (gc_block.values.p > 5.0f){
+                FAIL(Status_GcodeValueOutOfRange);}
+            gc_state.modal.activeaccelprofile = gc_block.values.p;
+            break;
+#endif
         default:
 
             // At this point, the rest of the explicit axis commands treat the axis values as the traditional
@@ -3854,6 +3880,9 @@ status_code_t gc_execute_block (char *block)
             gc_state.modal.coolant = (coolant_state_t){0};
             gc_state.modal.override_ctrl.feed_rate_disable = Off;
             gc_state.modal.override_ctrl.spindle_rpm_disable = Off;
+            #if ENABLE_ACCELERATION_PROFILES
+            gc_state.modal.activeaccelprofile = 1.0f;
+            #endif
 
             idx = N_SYS_SPINDLE;
             spindle_ptrs_t *spindle;

gcode.h

@@ -59,7 +59,12 @@ typedef enum {
     NonModal_SetCoordinateOffset = 92,      //!< 92 - G92
     NonModal_ResetCoordinateOffset = 102,   //!< 102 - G92.1
     NonModal_ClearCoordinateOffset = 112,   //!< 112 - G92.2
-    NonModal_RestoreCoordinateOffset = 122  //!< 122 - G92.3
+ #if ENABLE_ACCELERATION_PROFILES
+    NonModal_RestoreCoordinateOffset = 122, //!< 122 - G92.3
+    NonModal_SetAccelerationProfile = 187   //!< 187 - G187 
+ #else
+    NonModal_RestoreCoordinateOffset = 122 //!< 122 - G92.3
+ #endif
 } non_modal_t;
 
 

planner.c

@@ -348,9 +348,28 @@ static inline float limit_acceleration_by_axis_maximum (float *unit_vec)
         if (unit_vec[--idx] != 0.0f)  // Avoid divide by zero.
             limit_value = min(limit_value, fabsf(settings.axis[idx].acceleration / unit_vec[idx]));
     } while(idx);
+#if ENABLE_ACCELERATION_PROFILES
+    limit_value *= lookupprofile(gc_state.modal.activeaccelprofile);
+#endif
+    return limit_value;
+}
+
+#if ENABLE_JERK_ACCELERATION
+static inline float limit_jerk_by_axis_maximum (float *unit_vec)
+{
+    uint_fast8_t idx = N_AXIS;
+    float limit_value = SOME_LARGE_VALUE;
 
+    do {
+        if (unit_vec[--idx] != 0.0f)  // Avoid divide by zero.
+            limit_value = min(limit_value, fabsf(settings.axis[idx].jerk / unit_vec[idx]));
+    } while(idx);
+#if ENABLE_ACCELERATION_PROFILES
+    limit_value *= lookupprofile(gc_state.modal.activeaccelprofile);
+#endif
     return limit_value;
 }
+#endif
 
 static inline float limit_max_rate_by_axis_maximum (float *unit_vec)
 {
@@ -502,7 +521,12 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data)
 #endif
 
     block->millimeters = convert_delta_vector_to_unit_vector(unit_vec);
+#if ENABLE_JERK_ACCELERATION
+    block->max_acceleration = limit_acceleration_by_axis_maximum(unit_vec);
+    block->jerk = limit_jerk_by_axis_maximum(unit_vec);
+#else
     block->acceleration = limit_acceleration_by_axis_maximum(unit_vec);
+#endif
     block->rapid_rate = limit_max_rate_by_axis_maximum(unit_vec);
 #ifdef KINEMATICS_API
     block->rate_multiplier = pl_data->rate_multiplier;
@@ -516,6 +540,13 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data)
         if (block->condition.inverse_time)
             block->programmed_rate *= block->millimeters;
     }
+#if ENABLE_JERK_ACCELERATION
+    // Calculate effective acceleration over block. Since jerk acceleration takes longer to execute due to ramp up and 
+    // ramp down of the acceleration at the start and end of a ramp we need to adjust the acceleration value the planner 
+    // uses so it still calculates reasonable entry and exit speeds. We do this by adding 2x the time it takes to reach 
+    // full acceleration to the trapezoidal acceleration time and dividing the programmed rate by the value obtained.
+    block->acceleration = block->programmed_rate / ((block->programmed_rate / block->max_acceleration) + 2.0f * (block->max_acceleration / block->jerk));
+#endif
 
     // TODO: Need to check this method handling zero junction speeds when starting from rest.
     if ((block_buffer_head == block_buffer_tail) || (block->condition.system_motion)) {

planner.h

@@ -63,7 +63,11 @@ typedef struct plan_block {
     float entry_speed_sqr;          // The current planned entry speed at block junction in (mm/min)^2
     float max_entry_speed_sqr;      // Maximum allowable entry speed based on the minimum of junction limit and
                                     // neighboring nominal speeds with overrides in (mm/min)^2
-    float acceleration;             // Axis-limit adjusted line acceleration in (mm/min^2). Does not change.
+    float acceleration;             // Effective acceleration over plannerblock calculated from trapezoidal movement plan. Does not change in trapezoidal mode.                     
+#if ENABLE_JERK_ACCELERATION   
+    float max_acceleration;         // Axis-limit adjusted line acceleration in (mm/min^2). Does not change.
+    float jerk;                     // Axis-limit adjusted jerk value in (mm/min^3). Does not change.
+#endif
     float millimeters;              // The remaining distance for this block to be executed in (mm).
                                     // NOTE: This value may be altered by stepper algorithm during execution.
 

settings.c

@@ -208,6 +208,9 @@ PROGMEM const settings_t defaults = {
     .axis[X_AXIS].steps_per_mm = DEFAULT_X_STEPS_PER_MM,
     .axis[X_AXIS].max_rate = DEFAULT_X_MAX_RATE,
     .axis[X_AXIS].acceleration = (DEFAULT_X_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION    
+    .axis[X_AXIS].jerk = (DEFAULT_X_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[X_AXIS].max_travel = (-DEFAULT_X_MAX_TRAVEL),
     .axis[X_AXIS].dual_axis_offset = 0.0f,
 #if ENABLE_BACKLASH_COMPENSATION
@@ -218,6 +221,9 @@ PROGMEM const settings_t defaults = {
     .axis[Y_AXIS].max_rate = DEFAULT_Y_MAX_RATE,
     .axis[Y_AXIS].max_travel = (-DEFAULT_Y_MAX_TRAVEL),
     .axis[Y_AXIS].acceleration = (DEFAULT_Y_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[Y_AXIS].jerk = (DEFAULT_Y_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[Y_AXIS].dual_axis_offset = 0.0f,
 #if ENABLE_BACKLASH_COMPENSATION
     .axis[Y_AXIS].backlash = 0.0f,
@@ -226,6 +232,9 @@ PROGMEM const settings_t defaults = {
     .axis[Z_AXIS].steps_per_mm = DEFAULT_Z_STEPS_PER_MM,
     .axis[Z_AXIS].max_rate = DEFAULT_Z_MAX_RATE,
     .axis[Z_AXIS].acceleration = (DEFAULT_Z_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[Z_AXIS].jerk = (DEFAULT_Z_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[Z_AXIS].max_travel = (-DEFAULT_Z_MAX_TRAVEL),
     .axis[Z_AXIS].dual_axis_offset = 0.0f,
 #if ENABLE_BACKLASH_COMPENSATION
@@ -236,6 +245,9 @@ PROGMEM const settings_t defaults = {
     .axis[A_AXIS].steps_per_mm = DEFAULT_A_STEPS_PER_MM,
     .axis[A_AXIS].max_rate = DEFAULT_A_MAX_RATE,
     .axis[A_AXIS].acceleration =(DEFAULT_A_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[A_AXIS].jerk = (DEFAULT_A_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[A_AXIS].max_travel = (-DEFAULT_A_MAX_TRAVEL),
     .axis[A_AXIS].dual_axis_offset = 0.0f,
 #if ENABLE_BACKLASH_COMPENSATION
@@ -248,6 +260,9 @@ PROGMEM const settings_t defaults = {
     .axis[B_AXIS].steps_per_mm = DEFAULT_B_STEPS_PER_MM,
     .axis[B_AXIS].max_rate = DEFAULT_B_MAX_RATE,
     .axis[B_AXIS].acceleration = (DEFAULT_B_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[B_AXIS].jerk = (DEFAULT_B_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[B_AXIS].max_travel = (-DEFAULT_B_MAX_TRAVEL),
     .axis[B_AXIS].dual_axis_offset = 0.0f,
 #if ENABLE_BACKLASH_COMPENSATION
@@ -259,6 +274,9 @@ PROGMEM const settings_t defaults = {
 #ifdef C_AXIS
     .axis[C_AXIS].steps_per_mm = DEFAULT_C_STEPS_PER_MM,
     .axis[C_AXIS].acceleration = (DEFAULT_C_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[C_AXIS].jerk = (DEFAULT_C_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[C_AXIS].max_rate = DEFAULT_C_MAX_RATE,
     .axis[C_AXIS].max_travel = (-DEFAULT_C_MAX_TRAVEL),
     .axis[C_AXIS].dual_axis_offset = 0.0f,
@@ -271,6 +289,9 @@ PROGMEM const settings_t defaults = {
 #ifdef U_AXIS
     .axis[U_AXIS].steps_per_mm = DEFAULT_U_STEPS_PER_MM,
     .axis[U_AXIS].acceleration = (DEFAULT_U_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[U_AXIS].jerk = (DEFAULT_U_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[U_AXIS].max_rate = DEFAULT_U_MAX_RATE,
     .axis[U_AXIS].max_travel = (-DEFAULT_U_MAX_TRAVEL),
     .axis[U_AXIS].dual_axis_offset = 0.0f,
@@ -282,6 +303,9 @@ PROGMEM const settings_t defaults = {
 #ifdef V_AXIS
     .axis[V_AXIS].steps_per_mm = DEFAULT_V_STEPS_PER_MM,
     .axis[V_AXIS].acceleration = (DEFAULT_V_ACCELERATION * 60.0f * 60.0f),
+#if ENABLE_JERK_ACCELERATION   
+    .axis[V_AXIS].jerk = (DEFAULT_V_JERK * 60.0f * 60.0f * 60.0f),
+#endif
     .axis[V_AXIS].max_rate = DEFAULT_V_MAX_RATE,
     .axis[V_AXIS].max_travel = (-DEFAULT_V_MAX_TRAVEL),
     .axis[V_AXIS].dual_axis_offset = 0.0f,
@@ -467,6 +491,9 @@ static char spindle_types[100] = "";
 static char axis_dist[4] = "mm";
 static char axis_rate[8] = "mm/min";
 static char axis_accel[10] = "mm/sec^2";
+#if ENABLE_JERK_ACCELERATION   
+static char axis_jerk[15] = "mm/sec^3";
+#endif
 #if DELTA_ROBOT
 static char axis_steps[9] = "step/rev";
 #else
@@ -595,6 +622,9 @@ PROGMEM static const setting_detail_t setting_detail[] = {
      { Setting_AxisStepsPerMM, Group_Axis0, "-axis travel resolution", axis_steps, Format_Decimal, "#####0.000##", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL, AXIS_OPTS },
      { Setting_AxisMaxRate, Group_Axis0, "-axis maximum rate", axis_rate, Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL, AXIS_OPTS },
      { Setting_AxisAcceleration, Group_Axis0, "-axis acceleration", axis_accel, Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL, AXIS_OPTS },
+#if ENABLE_JERK_ACCELERATION   
+     { Setting_AxisJerk, Group_Axis0, "-axis jerk", axis_jerk, Format_Decimal, "#####0.000", NULL, NULL, Setting_IsExtendedFn, set_axis_setting, get_float, NULL, AXIS_OPTS },
+#endif
      { Setting_AxisMaxTravel, Group_Axis0, "-axis maximum travel", axis_dist, Format_Decimal, "#####0.000", NULL, NULL, Setting_IsLegacyFn, set_axis_setting, get_float, NULL, AXIS_OPTS },
 #if ENABLE_BACKLASH_COMPENSATION
      { Setting_AxisBacklash, Group_Axis0, "-axis backlash compensation", axis_dist, Format_Decimal, "#####0.000##", NULL, NULL, Setting_IsExtendedFn, set_axis_setting, get_float, NULL, AXIS_OPTS },
@@ -772,6 +802,13 @@ PROGMEM static const setting_descr_t setting_descr[] = {
     { (setting_id_t)(Setting_AxisStepsPerMM + 1), "Travel resolution in steps per degree." }, // "Hack" to get correct description for rotary axes
     { Setting_AxisMaxRate, "Maximum rate. Used as G0 rapid rate." },
     { Setting_AxisAcceleration, "Acceleration. Used for motion planning to not exceed motor torque and lose steps." },
+#if ENABLE_JERK_ACCELERATION   
+    { Setting_AxisJerk, "Maximum rate of acceleration change - smoothes out acceleration profile up to max axis acceleration.\\n\\n"
+                        "Minimum value of x10 Acceleration setting to ensure decent acceleration times.\\n"
+                        "Maximum is calculated by current acceleration and stepper segment time.\\n"
+                        "At Maximum value motion is effectively trapezoidal instead of constant jerk.\\n\\n"
+                        "Can be increased by adjusting ACCELERATION_TICKS_PER_SECOND to a larger value before compiling."},
+#endif
     { Setting_AxisMaxTravel, "Maximum axis travel distance from homing switch. Determines valid machine space for soft-limits and homing search distances." },
 #if ENABLE_BACKLASH_COMPENSATION
     { Setting_AxisBacklash, "Backlash distance to compensate for." },
@@ -889,12 +926,25 @@ bool settings_override_acceleration (uint8_t axis, float acceleration)
     } else {
         if(!override_backup.valid)
             save_override_backup();
-        settings.axis[axis].acceleration = acceleration * 60.0f * 60.0f; // Limit max to setting value?
+        settings.axis[axis].acceleration = (override_backup.acceleration[axis] >= (acceleration * 60.0f * 60.0f)) ? (acceleration * 60.0f * 60.0f) : override_backup.acceleration[axis]; // Limited to max setting value
     }
-
     return true;
 }
 
+#if ENABLE_ACCELERATION_PROFILES
+//Acceleration Profiles for G187 P[x] in percent of maximum machine acceleration.
+float lookupprofile(uint8_t profile) {
+    static const float lookup[5] = { 
+        1.0f,   // 100% - Roughing - Max Acceleration Default
+        0.8f,   // 80% - Semi Roughing
+        0.6f,   // 60% - Semi Finish
+        0.4f,   // 40% - Finish
+        0.2f,   // 20% - Slow AF Mode
+    };
+    return lookup[profile];
+}
+#endif
+
 // ---
 
 static setting_details_t *settingsd = &setting_details;
@@ -1402,6 +1452,12 @@ static const char *set_axis_setting_unit (setting_id_t setting_id, uint_fast8_t
             unit = is_rotary ? "deg/sec^2" : "mm/sec^2";
             break;
 
+#if ENABLE_JERK_ACCELERATION   
+        case Setting_AxisJerk:
+            unit = is_rotary ? "deg/sec^3" : "mm/sec^3";
+            break;
+#endif
+
         case Setting_AxisMaxTravel:
         case Setting_AxisBacklash:
             unit = is_rotary ? "deg" : "mm";
@@ -1517,6 +1573,19 @@ static status_code_t set_axis_setting (setting_id_t setting, float value)
             settings.axis[idx].acceleration = override_backup.acceleration[idx] = value * 60.0f * 60.0f; // Convert to mm/min^2 for grbl internal use.
             break;
 
+#if ENABLE_JERK_ACCELERATION      
+        case Setting_AxisJerk:
+            if ((value * 60.0f * 60.0f) < (settings.axis[idx].acceleration * 10.0f)) { //ensuring that the acceleration ramp time is limited to at maximum 100ms (or 10 stepper segments).
+                settings.axis[idx].jerk = settings.axis[idx].acceleration * 10.0f * 60.0f; // mm/min^2 -> mm/min^3
+            }
+            else if ((settings.axis[idx].acceleration / (value * 60.0f * 60.0f * 60.0f)) < (1.0f / ACCELERATION_TICKS_PER_SECOND)) { // Limit Jerk if value is so large that it reverts back to trapezoidal.
+                settings.axis[idx].jerk = settings.axis[idx].acceleration * ACCELERATION_TICKS_PER_SECOND * 60.0f; // mm/min^2 -> mm/min^3
+            }
+            else
+                settings.axis[idx].jerk = value * 60.0f * 60.0f * 60.0f; // Convert to mm/min^3 for grbl internal use.
+            break;
+#endif            
+
         case Setting_AxisMaxTravel:
             if(settings.axis[idx].max_travel != -value) {
                 bit_false(sys.homed.mask, bit(idx));
@@ -1562,7 +1631,7 @@ static float get_float (setting_id_t setting)
 {
     float value = 0.0f;
 
-    if (setting >= Setting_AxisSettingsBase && setting <= Setting_AxisSettingsMax) {
+    if (setting >= Setting_AxisSettingsBase && setting <= Setting_AxisSettingsMax2) {
 
         uint_fast8_t idx;
 
@@ -1580,6 +1649,12 @@ static float get_float (setting_id_t setting)
                 value = settings.axis[idx].acceleration / (60.0f * 60.0f); // Convert from mm/min^2 to mm/sec^2.
                 break;
 
+#if ENABLE_JERK_ACCELERATION          
+            case Setting_AxisJerk:
+                value = settings.axis[idx].jerk / (60.0f * 60.0f * 60.0f); // Convert from mm/min^3 to mm/sec^3.
+                break;
+#endif
+
             case Setting_AxisMaxTravel:
                 value = -settings.axis[idx].max_travel; // Store as negative for grbl internal use.
                 break;