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.
@@ -2016,6 +2019,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

@@ -361,6 +361,9 @@ void gc_init (bool stop)
 #if NGC_PARAMETERS_ENABLE
     ngc_modal_state_invalidate();
 #endif
+#if ENABLE_ACCELERATION_PROFILES
+    gc_state.modal.acceleration_factor = 1.0f; // Initialize machine with 100% Profile
+#endif
 
 //    if(settings.flags.lathe_mode)
 //        gc_state.modal.plane_select = PlaneSelect_ZX;
@@ -1194,6 +1197,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
 
@@ -2358,7 +2370,19 @@ 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);
+
+            if(gc_block.words.p && (gc_block.values.p < 1.0f || gc_block.values.p > 5.0f))
+                FAIL(Status_GcodeValueOutOfRange);
 
+            gc_state.modal.acceleration_factor = lookupfactor(gc_block.words.p ? (uint8_t)gc_block.values.p - 1 : 0);
+            gc_block.words.p = Off;
+            break;
+#endif
         default:
 
             // At this point, the rest of the explicit axis commands treat the axis values as the traditional
@@ -3030,6 +3054,9 @@ status_code_t gc_execute_block (char *block)
     memset(&plan_data, 0, sizeof(plan_line_data_t)); // Zero plan_data struct
     plan_data.offset_id = gc_state.offset_id;
     plan_data.condition.target_validated = plan_data.condition.target_valid = sys.soft_limits.mask == 0;
+#if ENABLE_ACCELERATION_PROFILES
+    plan_data.acceleration_factor = gc_state.modal.acceleration_factor;
+#endif
 
     // Intercept jog commands and complete error checking for valid jog commands and execute.
     // NOTE: G-code parser state is not updated, except the position to ensure sequential jog
@@ -3819,6 +3846,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.acceleration_factor = 1.0f;
+            #endif
 
 #if N_SYS_SPINDLE > 1
 

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;
 
 
@@ -541,6 +546,9 @@ typedef struct {
     bool auto_restore;
     float feed_rate;                     //!< {F} NOTE: only set when saving modal state
 #endif
+#if ENABLE_ACCELERATION_PROFILES
+    float acceleration_factor;          //!< {G187} currently active factor of acceleration profile
+#endif
 } gc_modal_t;
 
 //! Data for canned cycles.

planner.c

@@ -348,9 +348,22 @@ 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);
+    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);
     return limit_value;
 }
+#endif
 
 static inline float limit_max_rate_by_axis_maximum (float *unit_vec)
 {
@@ -502,10 +515,24 @@ 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;
+#endif
+#if ENABLE_ACCELERATION_PROFILES                                 // recalculate the acceleration limits when enabled.
+    block->acceleration_factor = pl_data->acceleration_factor;
+#if ENABLE_JERK_ACCELERATION
+    block->max_acceleration *= block->acceleration_factor;
+    block->jerk *= block->acceleration_factor;
+#else
+    block->acceleration *= block->acceleration_factor;
+#endif
 #endif
 
     // Store programmed rate.
@@ -516,6 +543,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)) {
@@ -704,4 +738,7 @@ void plan_data_init (plan_line_data_t *plan_data)
 #ifdef KINEMATICS_API
     plan_data->rate_multiplier = 1.0f;
 #endif
+#ifdef ENABLE_ACCELERATION_PROFILES
+    plan_data->acceleration_factor = 1.0f; 
+#endif
 }

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.
 
@@ -73,6 +77,9 @@ typedef struct plan_block {
     float programmed_rate;          // Programmed rate of this block (mm/min).
 #ifdef KINEMATICS_API
     float rate_multiplier;          // Rate multiplier of this block.
+#endif
+#ifdef ENABLE_ACCELERATION_PROFILES
+    float acceleration_factor;        // Stores the currently used acceleration factor.
 #endif
     // Stored spindle speed data used by spindle overrides and resuming methods.
     spindle_t spindle;              // Block spindle parameters. Copied from pl_line_data.
@@ -89,6 +96,9 @@ typedef struct {
 #ifdef KINEMATICS_API
     float rate_multiplier;          // Feed rate multiplier.
 #endif
+#ifdef ENABLE_ACCELERATION_PROFILES
+    float acceleration_factor;        // Stores the currently used acceleration factor.
+#endif
 #if ENABLE_PATH_BLENDING
     float path_tolerance;           //!< Path blending tolerance.
     float cam_tolerance;            //!< Naive CAM tolerance.