From 526fbcc30fd3a9c1b229f192b971c50503276f9c Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Wed, 22 Feb 2017 13:38:35 -0500 Subject: [PATCH] Modifications to mp_exec_aline(). Still has tool farts. Working on that. --- g2core/canonical_machine.cpp | 11 +---- g2core/canonical_machine.h | 2 +- g2core/cycle_feedhold.cpp | 2 +- g2core/g2core.cppproj | 8 ++-- g2core/plan_exec.cpp | 92 ++++++++++++++++++++---------------- g2core/plan_line.cpp | 10 ++-- g2core/plan_zoid.cpp | 24 ++++++++-- 7 files changed, 82 insertions(+), 67 deletions(-) diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 329837c8..8ddd1bb5 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -1123,11 +1123,6 @@ stat_t cm_straight_traverse(const float target[], const bool flags[]) stat_t status = mp_aline(&cm->gm); // send the move to the planner cm_update_model_position(); // update gmx.position to ready for next incoming move - -// if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //mp applies to currently active planner -// cm_cycle_end(); -// return (STAT_OK); -// } if (status == STAT_MINIMUM_LENGTH_MOVE) { if (!mp_has_runnable_buffer(mp)) { // handle condition where zero-length move is last or only move cm_cycle_end(); // ...otherwise cycle will not end properly @@ -1284,10 +1279,6 @@ stat_t cm_straight_feed(const float target[], const bool flags[]) stat_t status = mp_aline(&cm->gm); // send the move to the planner cm_update_model_position(); // <-- ONLY safe because we don't care about status... -// if (status == STAT_MINIMUM_LENGTH_MOVE && !mp_has_runnable_buffer(mp)) { //mp applies to currently active planner -// cm_cycle_end(); -// return (STAT_OK); -// } if (status == STAT_MINIMUM_LENGTH_MOVE) { if (!mp_has_runnable_buffer(mp)) { // handle condition where zero-length move is last or only move cm_cycle_end(); // ...otherwise cycle will not end properly @@ -1564,7 +1555,7 @@ stat_t cm_tro_control(const float P_word, const bool P_flag) // M50.1 static void _exec_program_finalize(float *value, bool *flag) { cmMachineState machine_state = (cmMachineState)value[0]; - cm_set_motion_state(MOTION_STOP); + cm_set_motion_state(MOTION_STOP); // also changes active model back to MODEL // Allow update in the alarm state, to accommodate queue flush (RAS) if ((cm->cycle_state == CYCLE_MACHINING || cm->cycle_state == CYCLE_OFF) && diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 8522d83f..2bd36292 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -115,7 +115,7 @@ typedef enum { // feedhold state machine FEEDHOLD_SYNC, // start hold - sync to latest aline segment FEEDHOLD_DECEL_CONTINUE, // in deceleration that will not end at zero FEEDHOLD_DECEL_TO_ZERO, // in deceleration that will go to zero - FEEDHOLD_DECEL_END, // end the deceleration + FEEDHOLD_DECEL_COMPLETE, // feedhold deceleration has completed FEEDHOLD_STOPPING, // waiting to complete deceleration once planner motion stops FEEDHOLD_ACTIONS_START, // enter secondary planner and perform feedhold actions (once) FEEDHOLD_ACTIONS_WAIT, // wait for feedhold actions to complete diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 83681881..190c62e8 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -325,7 +325,7 @@ static stat_t _run_p1_hold_entry_actions() // set motion state and ACTIVE_MODEL. This must be performed after cm is set to cm2 cm_set_g30_position(); - cm_set_motion_state(MOTION_STOP); + cm_set_motion_state(MOTION_STOP); // sets cm2 active model to MODEL // execute feedhold actions if (fp_NOT_ZERO(cm->feedhold_z_lift)) { // optional Z lift diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj index f1ace99b..fd10f7b0 100644 --- a/g2core/g2core.cppproj +++ b/g2core/g2core.cppproj @@ -68,12 +68,12 @@ - 2000000 + 10000000 SWD com.atmel.avrdbg.tool.atmelice - J41800036434 + J41800030015 Atmel-ICE True @@ -100,9 +100,9 @@ True true - J41800036434 + J41800030015 0x284E0A60 - 2000000 + 10000000 diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index aaaca620..a5eb8ffa 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -294,13 +294,13 @@ stat_t mp_exec_move() // Manage motion state transitions if ((cm->motion_state != MOTION_RUN) && (cm->motion_state != MOTION_HOLD)) { - cm_set_motion_state(MOTION_RUN); + cm_set_motion_state(MOTION_RUN); // also sets active model to RUNTIME } } if (bf->bf_func == NULL) { return(cm_panic(STAT_INTERNAL_ERROR, "mp_exec_move()")); // never supposed to get here } - return (bf->bf_func(bf)); // run the move callback in the planner buffer + return (bf->bf_func(bf)); // run the move callback in the planner buffer } /*************************************************************************/ @@ -404,9 +404,6 @@ stat_t mp_exec_aline(mpBuf_t *bf) // Zero length moves (and other too-short moves) should have already been removed earlier // But let's still alert the condition should it ever occur -// if (fp_ZERO(bf->length)) { // ...looks for an actual zero here -// rpt_exception(STAT_PLANNER_ASSERTION_FAILURE, "mp_exec_aline() zero length move"); -// } debug_trap_if_zero(bf->length, "mp_exec_aline() zero length move"); // Equalities that must be true for this to work: @@ -425,10 +422,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) // Start a new move by setting up the runtime singleton (mr) memcpy(&mr->gm, &(bf->gm), sizeof(GCodeState_t)); // copy in the gcode model state bf->block_state = BLOCK_ACTIVE; // note that this buffer is running - // note the planner doesn't look at block_state - mr->block_state = BLOCK_INITIAL_ACTION; - mr->section = SECTION_HEAD; - mr->section_state = SECTION_NEW; + mr->block_state = BLOCK_INITIAL_ACTION; // note the planner doesn't look at block_state // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // !!! THIS IS THE ONLY PLACE WHERE mr->r AND mr->p ARE ALLOWED TO BE CHANGED !!! @@ -437,6 +431,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) mr->r = mr->p; // we are now going to run the planning block mr->p = mr->p->nx; // re-use the old running block as the new planning block + // Check to make sure no sections are less than MIN_SEGMENT_TIME & adjust if necessary if ((!fp_ZERO(mr->r->head_length)) && (mr->r->head_time < MIN_SEGMENT_TIME)) { // head_time !== body_time @@ -495,10 +490,21 @@ stat_t mp_exec_aline(mpBuf_t *bf) } } + // transfer move parameters from planner buffer to the runtime copy_vector(mr->unit, bf->unit); - copy_vector(mr->target, bf->gm.target); // save the final target of the move + copy_vector(mr->target, bf->gm.target); copy_vector(mr->axis_flags, bf->axis_flags); + // characterize the move for starting section - head/body/tail + mr->section_state = SECTION_NEW; + mr->section = SECTION_HEAD; + if (fp_ZERO(mr->r->head_length)) { + mr->section = SECTION_BODY; + if (fp_ZERO(mr->r->body_length)) { + mr->section = SECTION_TAIL; + } + } + // generate the way points for position correction at section ends for (uint8_t axis=0; axiswaypoint[SECTION_HEAD][axis] = mr->position[axis] + mr->unit[axis] * mr->r->head_length; @@ -540,7 +546,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) if (mp_runtime_is_idle()) { // wait for steppers to actually finish // finalize position and velocity copy_vector(mr->position, mr->gm.target); // update position from target - bf->length = get_axis_vector_length(mr->target, mr->position); // reset length in buffer //+++++ TEsT + bf->length = get_axis_vector_length(mr->target, mr->position); // reset length in buffer //+++++ TEST THIS mp_zero_segment_velocity(); // for reporting purposes // when homing or probing don't stay in HOLD or execute entry actions @@ -560,7 +566,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) // Case (5) - Decelerated to zero. See also Feedhold Case (5, continued), toward end of mp_exec_aline() // Update the run buffer then force a replan of the whole planner queue. Replans from 0 velocity - if (cm->hold_state == FEEDHOLD_DECEL_END) { + if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) { 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 cm->hold_state = FEEDHOLD_STOPPING; @@ -579,7 +585,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) ((cm->hold_state == FEEDHOLD_DECEL_CONTINUE) && (mr->block_state == BLOCK_INITIAL_ACTION))) { // Case (3a) - Already decelerating, continue the deceleration. - if (mr->section == SECTION_TAIL) { // if already in a tail don't decelerate. You already are + if (mr->section == SECTION_TAIL) { // if already in a tail don't decelerate. You already are if (mr->r->exit_velocity < EPSILON2) { // allow near-zero velocities to be treated as zero cm->hold_state = FEEDHOLD_DECEL_TO_ZERO; } else { @@ -595,6 +601,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) mr->section_state = SECTION_NEW; mr->r->head_length = 0; mr->r->body_length = 0; + mr->r->head_time = 0; // +++++ can this be taken out? + mr->r->body_time = 0; // +++++ ditto float available_length = get_axis_vector_length(mr->target, mr->position); mr->r->tail_length = mp_get_target_length(0, mr->r->cruise_velocity, bf); // braking length @@ -606,7 +614,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) mr->r->tail_length = available_length; mr->r->exit_velocity = 0; - // (1b) The deceleration clearly has to span multiple moves + // (1b) The deceleration has to span multiple moves } else if (available_length < mr->r->tail_length) { mr->r->tail_length = available_length; mr->r->exit_velocity = mp_get_decel_velocity(mr->r->cruise_velocity, mr->r->tail_length, bf); @@ -617,12 +625,13 @@ stat_t mp_exec_aline(mpBuf_t *bf) cm->hold_state = FEEDHOLD_DECEL_CONTINUE; } - // (1a) The deceleration will fit easily into the current move - } else { + // (1a) The deceleration will fit into the current move + } else { cm->hold_state = FEEDHOLD_DECEL_TO_ZERO; mr->r->exit_velocity = 0; } mr->r->tail_time = mr->r->tail_length*2 / (mr->r->exit_velocity + mr->r->cruise_velocity); + bf->block_time = mr->r->tail_time; } } } @@ -656,7 +665,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) // Feedhold Case (5, continued): Look for the end of the deceleration to go into HOLD state if (cm->hold_state == FEEDHOLD_DECEL_TO_ZERO) { if ((status == STAT_OK) || (status == STAT_NOOP)) { - cm->hold_state = FEEDHOLD_DECEL_END; + cm->hold_state = FEEDHOLD_DECEL_COMPLETE; bf->block_state = BLOCK_INITIAL_ACTION; // reset bf so it can restart the rest of the move } } @@ -872,15 +881,15 @@ static void _init_forward_diffs(const float v_0, const float v_1) static stat_t _exec_aline_head(mpBuf_t *bf) { bool first_pass = false; - if (mr->section_state == SECTION_NEW) { // INITIALIZATION + if (mr->section_state == SECTION_NEW) { // INITIALIZATION first_pass = true; if (fp_ZERO(mr->r->head_length)) { mr->section = SECTION_BODY; - return(_exec_aline_body(bf)); // skip ahead to the body generator + return(_exec_aline_body(bf)); // skip ahead to the body generator } mr->segments = ceil(uSec(mr->r->head_time) / NOM_SEGMENT_USEC);// # of segments for the section mr->segment_count = (uint32_t)mr->segments; - mr->segment_time = mr->r->head_time / mr->segments; // time to advance for each segment + mr->segment_time = mr->r->head_time / mr->segments; // time to advance for each segment if (mr->segment_count == 1) { // We will only have one segment, simply average the velocities @@ -890,22 +899,22 @@ static stat_t _exec_aline_head(mpBuf_t *bf) } if (mr->segment_time < MIN_SEGMENT_TIME) { debug_trap("mr->segment_time < MIN_SEGMENT_TIME (head)"); - return(STAT_OK); // exit without advancing position, say we're done + return(STAT_OK); // exit without advancing position, say we're done } - mr->section = SECTION_HEAD; + mr->section = SECTION_HEAD; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; } else { mr->segment_velocity += mr->forward_diff_5; } - if (_exec_aline_segment() == STAT_OK) { // set up for second half + if (_exec_aline_segment() == STAT_OK) { // set up for second half if ((fp_ZERO(mr->r->body_length)) && (fp_ZERO(mr->r->tail_length))) { - return(STAT_OK); // ends the move + return(STAT_OK); // ends the move } - - mr->section = SECTION_BODY; + mr->section = SECTION_BODY; // advance to body mr->section_state = SECTION_NEW; - } else if (!first_pass) { + } + else if (!first_pass) { mr->forward_diff_5 += mr->forward_diff_4; mr->forward_diff_4 += mr->forward_diff_3; mr->forward_diff_3 += mr->forward_diff_2; @@ -925,9 +934,8 @@ static stat_t _exec_aline_body(mpBuf_t *bf) if (mr->section_state == SECTION_NEW) { if (fp_ZERO(mr->r->body_length)) { mr->section = SECTION_TAIL; - return(_exec_aline_tail(bf)); // skip ahead to tail periods + return(_exec_aline_tail(bf)); // skip ahead to tail generator } - float body_time = mr->r->body_time; mr->segments = ceil(uSec(body_time) / NOM_SEGMENT_USEC); mr->segment_time = body_time / mr->segments; @@ -938,14 +946,14 @@ static stat_t _exec_aline_body(mpBuf_t *bf) return(STAT_OK); // exit without advancing position, say we're done } - mr->section = SECTION_BODY; + mr->section = SECTION_BODY; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; // uses PERIOD_2 so last segment detection works } if (_exec_aline_segment() == STAT_OK) { // OK means this section is done if (fp_ZERO(mr->r->tail_length)) { return(STAT_OK); // ends the move } - mr->section = SECTION_TAIL; + mr->section = SECTION_TAIL; // advance to tail mr->section_state = SECTION_NEW; } return(STAT_EAGAIN); @@ -960,34 +968,34 @@ static stat_t _exec_aline_tail(mpBuf_t *bf) bool first_pass = false; if (mr->section_state == SECTION_NEW) { // INITIALIZATION first_pass = true; + bf->plannable = false; // Mark the block as unplannable - // Mark the block as unplannable - bf->plannable = false; - - if (fp_ZERO(mr->r->tail_length)) { return(STAT_OK);} // end the move + if (fp_ZERO(mr->r->tail_length)) { // end the move + return(STAT_OK); + } mr->segments = ceil(uSec(mr->r->tail_time) / NOM_SEGMENT_USEC);// # of segments for the section mr->segment_count = (uint32_t)mr->segments; - mr->segment_time = mr->r->tail_time / mr->segments; // time to advance for each segment + mr->segment_time = mr->r->tail_time / mr->segments; // time to advance for each segment if (mr->segment_count == 1) { mr->segment_velocity = mr->r->tail_length / mr->segment_time; } else { - _init_forward_diffs(mr->r->cruise_velocity, mr->r->exit_velocity); // <-- sets inital segment_velocity + _init_forward_diffs(mr->r->cruise_velocity, mr->r->exit_velocity); // sets initial segment_velocity } if (mr->segment_time < MIN_SEGMENT_TIME) { debug_trap("mr->segment_time < MIN_SEGMENT_TIME (tail)"); - return(STAT_OK); // exit without advancing position, say we're done - // return(STAT_MINIMUM_TIME_MOVE); // exit without advancing position + return(STAT_OK); // exit without advancing position, say we're done } - mr->section = SECTION_TAIL; + mr->section = SECTION_TAIL; // +++++ Redundant??? mr->section_state = SECTION_RUNNING; } else { mr->segment_velocity += mr->forward_diff_5; } if (_exec_aline_segment() == STAT_OK) { - return(STAT_OK); // STAT_OK completes the move - } else if (!first_pass) { + return(STAT_OK); // STAT_OK completes the move + } + else if (!first_pass) { mr->forward_diff_5 += mr->forward_diff_4; mr->forward_diff_4 += mr->forward_diff_3; mr->forward_diff_3 += mr->forward_diff_2; diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp index 40deeef0..7958b0b3 100644 --- a/g2core/plan_line.cpp +++ b/g2core/plan_line.cpp @@ -655,7 +655,7 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa /* * _calculate_junction_vmax() - Giseburt's Algorithm ;-) * - * WARNING: This description is out of date and needs updated. + * WARNING: This description is out of date and needs to be updated. * * Computes the maximum allowable junction speed by finding the velocity that will not * violate the jerk value of any axis. @@ -664,14 +664,14 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa * of the corner, at the point from vector a to vector b. The unit vectors of those two * moves are provided as the current block (a_unit) and previous block (b_unit). * - * Delta[i] = (b_unit[i] - a_unit[i]) (1) + * Delta[i] = (b_unit[i] - a_unit[i]) (1) * * We take, axis by axis, the difference in "unit velocity" to get a vector that * represents the direction of acceleration - which may be the opposite direction * as that of the "a" vector to achieve deceleration. To get the actual acceleration, * we use the corner velocity (what we intend to calculate) as the magnitude. * - * Acceleration[i] = UnitAccel[i] * Velocity[i] (2) + * Acceleration[i] = UnitAccel[i] * Velocity[i] (2) * * Since we need the jerk value, which is defined as the "rate of change of acceleration, * that is, the derivative of acceleration with respect to time" (Wikipedia), we need to @@ -679,14 +679,14 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa * physics. That will give us the time over which to "apply" the change of acceleration * in order to get a physically realistic jerk. The yields a fairly simple formula: * - * Jerk[i] = Acceleration[i] / Time (3) + * Jerk[i] = Acceleration[i] / Time (3) * * Now that we can compute the jerk for a given corner, we need to know the maximum * velocity that we can take the corner without violating that jerk for any axis. * Let's incorporate formula (2) into formula (3), and solve for Velocity, using * the known max Jerk and UnitAccel for this corner: * - * Velocity[i] = (Jerk[i] * Time) / UnitAccel[i] (4) + * Velocity[i] = (Jerk[i] * Time) / UnitAccel[i] (4) * * We then compute (4) for each axis, and use the smallest (most limited) result or * vmax, whichever is smaller. diff --git a/g2core/plan_zoid.cpp b/g2core/plan_zoid.cpp index bbff69e0..866322f2 100644 --- a/g2core/plan_zoid.cpp +++ b/g2core/plan_zoid.cpp @@ -139,11 +139,16 @@ stat_t mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float e block->body_length = 0; block->tail_length = 0; - block->cruise_velocity = min(bf->cruise_velocity, bf->cruise_vmax); + // these conditions should have been met earlier, but if they are not trap and correct them + debug_trap_if_true((bf->exit_velocity > bf->exit_vmax), "mp_calculate_ramps() - Vexit > Vexit_max"); block->exit_velocity = min(bf->exit_velocity, bf->exit_vmax); + // +++++ THIS WILL NEED TO CHANGE TO SUPPORT OVERRIDES +// debug_trap_if_true((bf->cruise_velocity, bf->cruise_vmax), "mp_calculate_ramps() - Vcruise > Vcruise_max"); + block->cruise_velocity = min(bf->cruise_velocity, bf->cruise_vmax); + // We *might* do this exact computation later, so cache the value - float test_velocity = 0; + float test_velocity = 0; bool test_velocity_valid = false; // record if we have a validly cached value // *** Perfect-Fit Cases (1) *** Cases where curve fitting has already been done @@ -397,7 +402,7 @@ float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t* bf) /* * mp_get_decel_velocity() - mp_get_target_velocity but ONLY for deceleration * - * Get "the velocity" that we would end up at if we *decelerated* from v_0, + * Get the velocity that we would end up at if we decelerated from v_0, * over the provided L (length) and J (jerk, provided in the bf structure). * * We have to use a root finding solution, since there is actually three possible @@ -409,6 +414,9 @@ float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t* bf) * * This function may generate minor errors in target velocity, and should only * be used to compute feedholds or other cases where exact velocity is not mandatory. + * + * This function can fail if the length is too short to get a good answer. + * Failures return (float)-1.0 Negative velocities should never be returned. */ float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t* bf) @@ -426,7 +434,7 @@ float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t* bf) // The return condition allows a minor error in length (in mm). // Note: This comparison does NOT affect actual lengths or steps, which would be bad. // The actual lengths traveled must be controlled by the caller. - if (fabs(l_t) < 0.001) { + if (fabs(l_t) < 0.001) { break; } // For the first pass we tested velocity 0. If velocity 0 yields a l_t > 0, @@ -439,10 +447,18 @@ float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t* bf) const float v_1x3 = 3 * v_1; const float recip_l_t = (2 * sqrt_delta_v_0) / ((v_0 - v_1x3) * q_recip_2_sqrt_j); v_1 = v_1 - (l_t * recip_l_t); + + // hack for cases where there is no solution because the length is so short + if (v_1 > v_0) { +// return (-1.0); // cannot decelerate. Return an error + return (v_0); // cannot decelerate. Return entry velocity + } } return v_1; } +//Is there a way to derive the average slope of a deceleration given the starting velocity, length and jerk? We don't need the + /* * _get_meet_velocity() - find intersection velocity *