From 94939ee72d93e4b71cb25afc13048fde5330f94c Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sat, 18 Mar 2017 10:51:54 -0400 Subject: [PATCH] Cleanup and comments --- g2core/canonical_machine.h | 1 - g2core/cycle_feedhold.cpp | 8 +++++--- g2core/plan_exec.cpp | 24 ++++++++---------------- g2core/planner.h | 3 ++- 4 files changed, 15 insertions(+), 21 deletions(-) diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 5c3998d8..f686a30d 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -134,7 +134,6 @@ typedef enum { // feedhold state machine FEEDHOLD_DECEL_COMPLETE, // feedhold deceleration has completed, but motors may not have stopped yet FEEDHOLD_MOTION_STOPPING, // waiting for motors to have stopped at hold point (motion stop) FEEDHOLD_MOTION_STOPPED, // motion has stopped at hold point - FEEDHOLD_HOLD_POINT_REACHED, // hold point reached and logic run - prepare for next steps FEEDHOLD_HOLD_ACTIONS_PENDING, // wait for feedhold actions to complete FEEDHOLD_HOLD_ACTIONS_COMPLETE, // FEEDHOLD_HOLD, // HOLDING (steady state) diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 7a79e93c..e5e84179 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -559,7 +559,7 @@ static stat_t _feedhold_skip() if (cm1.hold_state == FEEDHOLD_OFF) { // if entered while OFF start a feedhold cm1.hold_state = FEEDHOLD_SYNC; } - if (cm1.hold_state < FEEDHOLD_HOLD_POINT_REACHED) { + if (cm1.hold_state < FEEDHOLD_MOTION_STOPPED) { return (STAT_EAGAIN); } cm1.hold_state = FEEDHOLD_OFF; // cannot be in HOLD or command won't plan (see mp_plan_block_list()) @@ -573,9 +573,11 @@ static stat_t _feedhold_no_actions() if (cm1.hold_state == FEEDHOLD_OFF) { // start a feedhold cm1.hold_state = FEEDHOLD_SYNC; } - if (cm1.hold_state < FEEDHOLD_HOLD_POINT_REACHED) { // wait until it reaches the hold point + if (cm1.hold_state < FEEDHOLD_MOTION_STOPPED) { // wait until it reaches the hold point return (STAT_EAGAIN); } + mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks + st_request_forward_plan(); // replan from the new bf buffer cm1.hold_state = FEEDHOLD_HOLD; return (STAT_OK); } @@ -590,7 +592,7 @@ static void _feedhold_actions_done_callback(float* vect, bool* flag) static stat_t _feedhold_with_actions() // Execute Case (5) { // Check to run first-time code - if (cm1.hold_state == FEEDHOLD_HOLD_POINT_REACHED) { + if (cm1.hold_state == FEEDHOLD_MOTION_STOPPED) { cm->hold_state = FEEDHOLD_HOLD_ACTIONS_PENDING; // next state // copy the primary canonical machine to the secondary, diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index f6e3e2d9..d6d55fec 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -485,7 +485,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) // Feedhold Processing - We need to handle the following cases (listed in rough sequence order): if (cm->hold_state != FEEDHOLD_OFF) { // if running actions, or in HOLD state, or exiting with actions - if (cm->hold_state >= FEEDHOLD_HOLD_POINT_REACHED) { // handles _exec_aline_feedhold_processing case (7) + if (cm->hold_state >= FEEDHOLD_MOTION_STOPPED) { // handles _exec_aline_feedhold_processing case (7) return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. Do not load another move } // STAT_OK terminates aline execution for this move @@ -1030,27 +1030,20 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) // Case (4) - Completing the feedhold - Wait for the steppers to stop if (cm->hold_state == FEEDHOLD_MOTION_STOPPING) { if (mp_runtime_is_idle()) { // wait for steppers to actually finish - // Motion has stopped, so we can rely on positions and other values to be stable - - mp_zero_segment_velocity(); // finalize velocity for reporting purposes -// cm->hold_state = FEEDHOLD_MOTION_STOPPED; - cm_set_motion_state(MOTION_STOP); + // Motion has stopped, so we can rely on positions and other values to be stable // If SKIP type, discard the remainder of the block and position to the next block if (cm->hold_type == FEEDHOLD_TYPE_SKIP) { copy_vector(mp->position, mr->position); // update planner position to the final runtime position mp_free_run_buffer(); // advance to next block, discarding the rest of the move - mr->reset(); // disable the rest of the runtime movement - } - else { // Reset the state of the planner regardless of how hold will ultimately be exited. + } else { // Otherwise setup the block to complete motion (regardless of how hold will ultimately be exited) bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer - mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move - bf->plannable = true; // needed so block can be adjusted - mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks - st_request_forward_plan(); // replan the current bf buffer + bf->plannable = true; // needed so block can be replanned } - cm->hold_state = FEEDHOLD_HOLD_POINT_REACHED; + mr->reset(); // reset MR for next use and for forward planning + cm_set_motion_state(MOTION_STOP); + cm->hold_state = FEEDHOLD_MOTION_STOPPED; sr_request_status_report(SR_REQUEST_IMMEDIATE); cs.controller_state = CONTROLLER_READY; // remove controller readline() PAUSE } @@ -1058,8 +1051,7 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) } // Case (3') - Decelerated to zero. See also Feedhold Case (3) in mp_exec_aline() - // Update the run buffer then force a replan of the whole planner queue. Replans from zero velocity - // This state might not appear necessary, but it is to handle closely packed !~ and other cases + // This state is needed to return an OK to complete the aline exec before transitioning to case (4). if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) { cm->hold_state = FEEDHOLD_MOTION_STOPPING; // wait for motion to come to a complete stop return (STAT_OK); // exit from mp_exec_aline() diff --git a/g2core/planner.h b/g2core/planner.h index 905b5037..29f87744 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -493,8 +493,9 @@ typedef struct mpPlannerRuntime { // persistent runtime variables section_state = SECTION_OFF; entry_velocity = 0; // needed to ensure next block in forward planning starts from 0 velocity r->exit_velocity = 0; // ditto + segment_velocity = 0; } - + } mpPlannerRuntime_t; //**** Master Planner Structure ***