Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-22 03:08:42 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Modifications to mp_exec_aline(). Still has tool farts. Working on that.

Browse Source
This commit is contained in:
Alden Hart
2017-02-22 13:38:35 -05:00
parent ac00174800
commit 526fbcc30f
7 changed files with 82 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/canonical_machine.cpp
+1 -10
View File
@@ -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) &&
g2core/canonical_machine.h
+1 -1
View File
@@ -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
g2core/cycle_feedhold.cpp
+1 -1
View File
@@ -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
g2core/g2core.cppproj
+4 -4
View File
@@ -68,12 +68,12 @@
<com_atmel_avrdbg_tool_atmelice>
<ToolOptions>
<InterfaceProperties>
<SwdClock>2000000</SwdClock>
<SwdClock>10000000</SwdClock>
</InterfaceProperties>
<InterfaceName>SWD</InterfaceName>
</ToolOptions>
<ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
<ToolNumber>J41800036434</ToolNumber>
<ToolNumber>J41800030015</ToolNumber>
<ToolName>Atmel-ICE</ToolName>
</com_atmel_avrdbg_tool_atmelice>
<UseGdb>True</UseGdb>
@@ -100,9 +100,9 @@
<HWProgramCounterSampling>True</HWProgramCounterSampling>
</PercepioTrace>
<preserveEEPROM>true</preserveEEPROM>
<avrtoolserialnumber>J41800036434</avrtoolserialnumber>
<avrtoolserialnumber>J41800030015</avrtoolserialnumber>
<avrdeviceexpectedsignature>0x284E0A60</avrdeviceexpectedsignature>
<avrtoolinterfaceclock>2000000</avrtoolinterfaceclock>
<avrtoolinterfaceclock>10000000</avrtoolinterfaceclock>
<custom>
<ToolOptions xmlns="">
<InterfaceProperties>
g2core/plan_exec.cpp
+50 -42
View File
@@ -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; axis<AXES; axis++) {
mr->waypoint[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;
g2core/plan_line.cpp
+5 -5
View File
@@ -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.
g2core/plan_zoid.cpp
+20 -4
View File
@@ -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
*