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
*