From a0710b3aaee41b8f30a122398aad8de76dc3f87b Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Fri, 3 Mar 2017 12:54:21 -0500 Subject: [PATCH] Fixed position error found in regressions test FI00 --- g2core/cycle_feedhold.cpp | 1 - g2core/plan_exec.cpp | 14 +------------- g2core/plan_line.cpp | 14 +++++++------- g2core/planner.h | 1 - 4 files changed, 8 insertions(+), 22 deletions(-) diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 04238e0c..496a2d2b 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -380,7 +380,6 @@ static stat_t _run_p1_hold_exit_actions() // LATER: if value == true return static void _sync_to_p1_hold_exit_actions_done(float* vect, bool* flag) { cm1.hold_state = FEEDHOLD_P1_EXIT; // penultimate state before transitioning to FEEDHOLD_OFF - // copy_vector(mp->position, mr->position); // +++++ Compensate for amount of move already performed sr_request_status_report(SR_REQUEST_IMMEDIATE); } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 22a562ac..80445f63 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -552,8 +552,6 @@ stat_t mp_exec_aline(mpBuf_t *bf) st_request_forward_plan(); } } -//+++++ copy_vector(mr->end_position, mr->position); // record end position - copy_vector(mp->position, mr->position); // record actual end position of the move } return (status); } @@ -803,7 +801,6 @@ static stat_t _exec_aline_body(mpBuf_t *bf) debug_trap("mr->segment_time < MIN_SEGMENT_TIME (body)"); return (STAT_OK); // exit without advancing position, say we're done } - mr->section = SECTION_BODY; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; // uses PERIOD_2 so last segment detection works } @@ -1025,7 +1022,6 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) // If in a p2 hold, exit the p2 hold immediately set up a flush of the p2 planner queue if (cm == &cm2) { -// copy_vector(mp->position, mr->position); // +++++ update planner position from runtime cm->hold_state = FEEDHOLD_P2_EXIT; return (STAT_OK); // will end this exec_aline() with no more movement } @@ -1043,16 +1039,12 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) else { // Reset the state of the p1 planner regardless of how hold will ultimately be exited. - bf->length = get_axis_vector_length(mr->position, mr->target); // get remaining length in move - copy_vector(mp->position, mr->position); // update planner position from runtime position - + 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 black 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 -// mp_forward_plan(); // the one case where you can call this function directly - copy_vector(mp->position, mr->target); // update planner position to the target // Set state to enable transition to p2 and perform entry actions in the p2 planner cm->hold_state = FEEDHOLD_ACTIONS_START; // executes entirely out of p2 planner @@ -1068,10 +1060,6 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) // Update the run buffer then force a replan of the whole planner queue. Replans from zero velocity if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) { cm->hold_state = FEEDHOLD_MOTORS_STOPPING; // wait for the motors to come to a complete stop -// mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move -// bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer -// bf->plannable = true; // needed so black can be adjusted -// mp_replan_queue(mp_get_r()); // make it replan all the blocks return (STAT_OK); // exit from mp_exec_aline() } diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp index 176a3393..c9a90714 100644 --- a/g2core/plan_line.cpp +++ b/g2core/plan_line.cpp @@ -104,7 +104,7 @@ float mp_get_runtime_display_position(uint8_t axis) { } } -/* +/**************************************************************************************** * mp_get_runtime_busy() - returns TRUE if motion control busy (i.e. robot is moving) * mp_runtime_is_idle() - returns TRUE is steppers are not actively moving * @@ -233,7 +233,7 @@ stat_t mp_aline(GCodeState_t* _gm) return (STAT_OK); } -/* +/**************************************************************************************** * mp_plan_block_list() - plan all the blocks in the list * * This parent function is just a dispatcher that reads forward in the list @@ -275,8 +275,8 @@ void mp_plan_block_list() mp->p = bf; // update planner pointer } -/* - * _plan_block() - the block chain using pessimistic assumptions +/**************************************************************************************** + * _plan_block() - stitch and backplan a new block to the planner queue */ static mpBuf_t* _plan_block(mpBuf_t* bf) @@ -492,7 +492,7 @@ static void _calculate_override(mpBuf_t* bf) // execute ramp to adjust cruise v // } } -/* +/**************************************************************************************** * _calculate_jerk() - calculate jerk given the dynamic state * * Set the jerk scaling to the lowest axis with a non-zero unit vector. @@ -542,7 +542,7 @@ static void _calculate_jerk(mpBuf_t* bf) bf->q_recip_2_sqrt_j = q / (2 * sqrt_j); } -/* +/**************************************************************************************** * _calculate_vmaxes() - compute cruise_vmax and absolute_vmax based on velocity constraints * * The following feeds and times are compared and the longest (slowest velocity) is returned: @@ -647,7 +647,7 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa bf->block_time = block_time; // initial estimate - used for ramp computations } -/* +/**************************************************************************************** * _calculate_junction_vmax() - Giseburt's Algorithm ;-) * * WARNING: This description is out of date and needs to be updated. diff --git a/g2core/planner.h b/g2core/planner.h index f4894cbb..ed84c5c6 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -454,7 +454,6 @@ typedef struct mpPlannerRuntime { // persistent runtime variables bool axis_flags[AXES]; // set true for axes participating in the move float target[AXES]; // final target for bf (used to correct rounding errors) float position[AXES]; // current move position -// float end_position[AXES]; // endpoint position of previous move +++++ float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction float target_steps[MOTORS]; // current MR target (absolute target as steps)