Implement motion profiles, use them in homing and probing

g2core/canonical_machine.cpp

@@ -1214,9 +1214,14 @@ stat_t cm_resume_g92_offsets()
  * cm_straight_traverse() - G0 linear rapid
  */
 
-stat_t cm_straight_traverse(const float *target, const bool *flags, const uint8_t motion_profile)
+stat_t cm_straight_traverse(const float *target, const bool *flags, const cmMotionProfile motion_profile)
 {
     cm->gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE;
+#ifdef TRAVERSE_AT_HIGH_JERK
+    cm->gm.motion_profile = PROFILE_FAST; // override to make all traverses use high jerk
+#else
+    cm->gm.motion_profile = motion_profile;
+#endif
 
     // it's legal for a G0 to have no axis words but we don't want to process it
     if (!(flags[AXIS_X] | flags[AXIS_Y] | flags[AXIS_Z] |
@@ -1371,7 +1376,7 @@ stat_t cm_dwell(const float seconds)
  * cm_straight_feed() - G1
  */
 
-stat_t cm_straight_feed(const float *target, const bool *flags, const uint8_t motion_profile)
+stat_t cm_straight_feed(const float *target, const bool *flags, const cmMotionProfile motion_profile)
 {
     // trap zero feed rate condition
     if (fp_ZERO(cm->gm.feed_rate)) {

g2core/canonical_machine.h

@@ -147,11 +147,6 @@ typedef enum {                      // feedhold state machine
     FEEDHOLD_EXIT_ACTIONS_COMPLETE  // completed exit actions
 } cmFeedholdState;
 
-typedef enum {                      // Motion profiles
-    PROFILE_NORMAL = 0,             // Normal jerk in effect
-    PROFILE_FAST                    // High speed jerk in effect
-} cmMotionProfile;
-
 typedef enum {                      // state machine for cycle start
     CYCLE_START_OFF = 0,            // not requested
     CYCLE_START_REQUESTED,
@@ -285,7 +280,6 @@ typedef struct cmMachine {                  // struct to manage canonical machin
 
     cmFeedholdType  hold_type;              // hold: type of feedhold requested
     cmFeedholdExit  hold_exit;              // hold: final state of hold on exit
-    cmMotionProfile hold_profile;           // hold: motion profile to use for deceleration
     cmFeedholdState hold_state;             // hold: feedhold state machine
 
     cmFlushState    queue_flush_state;      // queue flush state machine
@@ -422,7 +416,7 @@ stat_t cm_suspend_g92_offsets(void);                                        // G
 stat_t cm_resume_g92_offsets(void);                                         // G92.3
 
 // Free Space Motion (4.3.4)
-stat_t cm_straight_traverse(const float *target, const bool *flags, const uint8_t motion_profile); // G0
+stat_t cm_straight_traverse(const float *target, const bool *flags, const cmMotionProfile motion_profile); // G0
 stat_t cm_set_g28_position(void);                                           // G28.1
 stat_t cm_goto_g28_position(const float target[], const bool flags[]);      // G28
 stat_t cm_set_g30_position(void);                                           // G30.1
@@ -434,7 +428,7 @@ stat_t cm_set_feed_rate_mode(const uint8_t mode);                           // G
 stat_t cm_set_path_control(GCodeState_t *gcode_state, const uint8_t mode);  // G61, G61.1, G64
 
 // Machining Functions (4.3.6)
-stat_t cm_straight_feed(const float *target, const bool *flags, const uint8_t motion_profile); //G1
+stat_t cm_straight_feed(const float *target, const bool *flags, const cmMotionProfile motion_profile); //G1
 stat_t cm_dwell(const float seconds);                                       // G4, P parameter
 
 stat_t cm_arc_feed(const float target[], const bool target_f[],             // G2/G3 - target endpoint

g2core/cycle_feedhold.cpp

@@ -587,8 +587,6 @@ void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit)
     if ((cm1.hold_state == FEEDHOLD_OFF)) {
         cm1.hold_type = type;
         cm1.hold_exit = exit;
-        cm1.hold_profile =
-            ((type == FEEDHOLD_TYPE_ACTIONS) || (type == FEEDHOLD_TYPE_HOLD)) ? PROFILE_NORMAL : PROFILE_FAST;
 
         switch (cm1.hold_type) {
             case FEEDHOLD_TYPE_HOLD:     { op.add_action(_feedhold_no_actions); break; }

g2core/cycle_homing.cpp

@@ -67,7 +67,6 @@ struct hmHomingSingleton {          // persistent homing runtime variables
     cmDistanceMode saved_distance_mode;   // G90, G91 global setting
     cmFeedRateMode saved_feed_rate_mode;  // G93, G94 global setting
     float          saved_feed_rate;       // F setting
-    float          saved_jerk;            // saved and restored for each axis homed
 };
 static struct hmHomingSingleton hm;
 
@@ -235,12 +234,11 @@ stat_t cm_homing_cycle_callback(void) {
 /***********************************************************************************
  * cm_abort_homing() - something big happened, the queue is flushing, reset to non-homing state
  *
- *  Note: No need to worry about resetting states we saved (if we are actually homig), since
- *  when this is called everything is being reset anyway.
- *
  *  The task here is to stop sending homing moves to the planner, and ensure we can re-enter
  *  homing fresh without issue.
  *
+ *  Note that this should always be called after any feedhold and planner reset.
+ *
  */
 
 void cm_abort_homing(cmMachine_t *_cm) {
@@ -329,7 +327,6 @@ static stat_t _homing_axis_start(int8_t axis) {
     }
 
     // if homing is disabled for the axis then skip to the next axis
-    hm.saved_jerk = cm_get_axis_jerk(axis);                     // save the max jerk value
     return (_set_homing_func(_homing_axis_clear_init));         // perform an initial clear
 }
 
@@ -360,7 +357,6 @@ static stat_t _homing_axis_clear_init(int8_t axis)  // first clear move
  */
 static stat_t _homing_axis_search(int8_t axis)  // drive to switch
 {
-    cm_set_axis_max_jerk(axis, cm->a[axis].jerk_high);  // use the high-speed jerk for search onward
     _homing_axis_move(axis, hm.search_travel, hm.search_velocity);
     return (_set_homing_func(_homing_axis_clear));
 }
@@ -406,7 +402,6 @@ static stat_t _homing_axis_set_position(int8_t axis)
         kn_forward_kinematics(en_get_encoder_snapshot_vector(), contact_position);
         _homing_axis_move(axis, contact_position[AXIS_Z], hm.search_velocity);
     }
-    cm_set_axis_max_jerk(axis, hm.saved_jerk);  // restore the max jerk value
 
     din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_homing_handler);  // end homing mode
     return (_set_homing_func(_homing_axis_start));

g2core/cycle_jogging.cpp

@@ -56,7 +56,6 @@ struct jmJoggingSingleton {         // persistent jogging runtime variables
     uint8_t saved_coord_system;     // G54 - G59 setting
     uint8_t saved_distance_mode;    // G90,G91 global setting
     uint8_t saved_feed_rate_mode;
-    float   saved_jerk;             // saved and restored for each axis jogged
 };
 static struct jmJoggingSingleton jog;
 
@@ -92,7 +91,6 @@ stat_t cm_jogging_cycle_start(uint8_t axis) {
     jog.saved_distance_mode  = cm_get_distance_mode(ACTIVE_MODEL);  // cm->gm.distance_mode;
     jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL);
     jog.saved_feed_rate      = (ACTIVE_MODEL)->feed_rate;  // cm->gm.feed_rate;
-    jog.saved_jerk           = cm->a[axis].jerk_max;
 
     // set working values
     cm_set_units_mode(MILLIMETERS);

g2core/cycle_probing.cpp

@@ -62,7 +62,6 @@ struct pbProbingSingleton {             // persistent probing runtime variables
     cmUnitsMode saved_units_mode;       // G20,G21 setting
     cmDistanceMode saved_distance_mode; // G90,G91 global setting
     bool saved_soft_limits;             // turn off soft limits during probing
-    float saved_jerk[AXES];             // saved and restored for each axis
 };
 static struct pbProbingSingleton pb;
 
@@ -266,12 +265,11 @@ uint8_t cm_probing_cycle_callback(void)
 /***********************************************************************************
  * cm_abort_probing() - something big happened, the queue is flushing, reset to non-probing state
  *
- *  Note: No need to worry about resetting states we saved (if we are actually probing), since
- *  when this is called everything is being reset anyway.
- *
  *  The task here is to stop sending homing moves to the planner, and ensure we can re-enter
  *  homing fresh without issue.
  *
+ *  Note that this should always be called after any feedhold and planner reset.
+ *
  */
 
 void cm_abort_probing(cmMachine_t *_cm) {
@@ -321,7 +319,6 @@ static stat_t _probe_move(const float target[], const bool flags[])
 
 static uint8_t _probing_start()
 {
-    // so optimistic... ;)
     // These initializations are required before starting the probing cycle but must
     // be done after the planner has exhausted all current moves as they affect the
     // runtime (specifically the digital input modes). Side effects would include
@@ -341,12 +338,6 @@ static uint8_t _probing_start()
     cm_set_distance_mode(ABSOLUTE_DISTANCE_MODE);
     cm_set_units_mode(MILLIMETERS);
 
-    // Save the current jerk settings & change to the high-speed jerk settings
-    for (uint8_t axis = 0; axis < AXES; axis++) {
-        pb.saved_jerk[axis] = cm_get_axis_jerk(axis);  // save the max jerk value
-        cm_set_axis_max_jerk(axis, cm->a[axis].jerk_high);  // use the high-speed jerk for probe
-    }
-
     // Error if the probe target is too close to the current position
     if (get_axis_vector_length(cm->gmx.position, pb.target) < MINIMUM_PROBE_TRAVEL) {
         return(_probing_exception_exit(STAT_PROBE_TRAVEL_TOO_SMALL));
@@ -400,9 +391,6 @@ static void _probe_restore_settings()
 {
     din_handlers[INPUT_ACTION_INTERNAL].deregisterHandler(&_probing_handler);
 
-    for (uint8_t axis = 0; axis < AXES; axis++) {       // restore axis jerks
-        cm->a[axis].jerk_max = pb.saved_jerk[axis];
-    }
     cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_OFF); // release abs override and restore work offsets
     cm_set_distance_mode(pb.saved_distance_mode);
     cm_set_units_mode(pb.saved_units_mode);

g2core/gcode.h

@@ -90,6 +90,11 @@ typedef enum {
     UNITS_PER_REVOLUTION_MODE// G95 (unimplemented)
 } cmFeedRateMode;
 
+typedef enum {                      // Motion profiles
+    PROFILE_NORMAL = 0,             // Normal jerk in effect
+    PROFILE_FAST                    // High speed jerk in effect
+} cmMotionProfile;
+
 typedef enum {
     ORIGIN_OFFSET_SET=0,    // G92 - set origin offsets
     ORIGIN_OFFSET_CANCEL,   // G92.1 - zero out origin offsets
@@ -179,6 +184,7 @@ struct GCodeState_t {             // Gcode model state - used by model, planning
     cmDistanceMode arc_distance_mode;   // G90.1=use absolute IJK offsets, G91.1=incremental IJK offsets
     cmAbsoluteOverride absolute_override;// G53 TRUE = move using machine coordinates - this block only
     cmCoordSystem coord_system;         // G54-G59 - select coordinate system 1-9
+    cmMotionProfile motion_profile;     // NORMAL or FAST - controls what jerk is used when
     uint8_t tool;               // G    // M6 tool change - moves "tool_select" to "tool"
     uint8_t tool_select;        // G    // T value - T sets this value
 
@@ -205,6 +211,7 @@ struct GCodeState_t {             // Gcode model state - used by model, planning
         arc_distance_mode = ABSOLUTE_DISTANCE_MODE;
         absolute_override = ABSOLUTE_OVERRIDE_OFF;
         coord_system = ABSOLUTE_COORDS;
+        motion_profile = PROFILE_NORMAL;
         tool = 0;
         tool_select = 0;
 

g2core/plan_line.cpp

@@ -48,7 +48,6 @@ extern OutputPin<Motate::kDebug3_PinNumber> debug_pin3;
 
 // planner helper functions
 static mpBuf_t* _plan_block(mpBuf_t* bf);
-static void _calculate_override(mpBuf_t* bf);
 static void _calculate_jerk(mpBuf_t* bf);
 static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const float axis_square[]);
 static void _calculate_junction_vmax(mpBuf_t* bf);
@@ -455,18 +454,10 @@ static mpBuf_t* _plan_block(mpBuf_t* bf)
 
 static float _get_axis_jerk(mpBuf_t* bf, uint8_t axis)
 {
-    #ifdef TRAVERSE_AT_HIGH_JERK
-    switch (bf->gm.motion_mode) {
-        case MOTION_MODE_STRAIGHT_TRAVERSE:
-            //case MOTION_MODE_STRAIGHT_PROBE: // <-- not sure on this one
-            return cm->a[axis].jerk_high;
-            break;
-        default:
-            return cm->a[axis].jerk_max;
+    if (bf->gm.motion_profile == PROFILE_FAST) {
+        return cm->a[axis].jerk_high;
     }
-    #else
-   return cm->a[axis].jerk_max;
-    #endif
+    return cm->a[axis].jerk_max;
 }
 
 static void _calculate_jerk(mpBuf_t* bf)