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

Checkpoint

Browse Source
This commit is contained in:
Alden Hart
2017-03-02 15:20:33 -05:00
parent 926d8d3d3d
commit dc28c0006e
5 changed files with 30 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/gcode.h
+2 -1
View File
@@ -214,7 +214,7 @@ typedef struct GCodeState { // Gcode model state - used by model, pl
cmMotionMode motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81,
// G82, G83 G84, G85, G86, G87, G88, G89
float target[AXES]; // XYZABC where the move should go
float target[AXES]; // XYZABC target where the move should go
float target_comp[AXES]; // summation compensation (Kahan) overflow value
float display_offset[AXES]; // work offsets from the machine coordinate system (for reporting only)
@@ -267,6 +267,7 @@ typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used
float origin_offset[AXES]; // XYZABC G92 offsets (Note: not used in gn or gf)
float g28_position[AXES]; // XYZABC stored machine position for G28
float g30_position[AXES]; // XYZABC stored machine position for G30
float p1_position[AXES]; // XYZABC stored machine position for return to p1 planner
bool m48_enable; // master feedrate / spindle speed override enable
bool mfo_enable; // feedrate override enable
g2core/plan_exec.cpp
+7 -7
View File
@@ -472,16 +472,14 @@ stat_t mp_exec_aline(mpBuf_t *bf)
if (cm->hold_state >= FEEDHOLD_ACTIONS_START) { // handles _exec_aline_feedhold_processing case (7)
return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. Do not load another move
}
// if (_exec_aline_feedhold(bf) == STAT_OK) {
// return (STAT_OK);
// }
// STAT_OK terminates aline execution for this move
// STAT_NOOP terminates execution and does not load another move
status = _exec_aline_feedhold(bf);
if ((status == STAT_OK) || // STAT_OK terminates aline execution for this move
(status == STAT_NOOP)) { // STAT_NOOP terminates execution and does not load another move
if ((status == STAT_OK) || (status == STAT_NOOP)) {
return (status);
}
}
mr->block_state = BLOCK_ACTIVE;
// NB: from this point on the contents of the bf buffer do not affect execution
@@ -542,6 +540,8 @@ 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 end position
}
return (status);
}
@@ -1033,12 +1033,12 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
// 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
copy_vector(mr->end_position, mr->position);// record end position
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(); // queue replan of the current bf block
mp_forward_plan(); // replan current bf block
copy_vector(mp->position, mr->target); // update planner position to the target
g2core/plan_line.cpp
+16 -17
View File
@@ -142,7 +142,7 @@ bool mp_runtime_is_idle() { return (!st_runtime_isbusy()); }
* exceeds the minimums.
*/
stat_t mp_aline(GCodeState_t* gm_in)
stat_t mp_aline(GCodeState_t* _gm)
{
float target_rotated[AXES] = {0,0,0,0,0,0};
float axis_square[AXES] = {0,0,0,0,0,0};
@@ -153,7 +153,7 @@ stat_t mp_aline(GCodeState_t* gm_in)
// A few notes about the rotated coordinate space:
// These are positions PRE-rotation:
// gm_in.* (anything in gm_in)
// _gm.* (anything in _gm)
//
// These are positions POST-rotation:
// target_rotated (after the rotation here, of course)
@@ -170,23 +170,23 @@ stat_t mp_aline(GCodeState_t* gm_in)
// c being target[2],
// x_1 being cm->rotation_matrix[1][0]
target_rotated[0] = gm_in->target[0] * cm->rotation_matrix[0][0] +
gm_in->target[1] * cm->rotation_matrix[0][1] +
gm_in->target[2] * cm->rotation_matrix[0][2];
target_rotated[0] = _gm->target[0] * cm->rotation_matrix[0][0] +
_gm->target[1] * cm->rotation_matrix[0][1] +
_gm->target[2] * cm->rotation_matrix[0][2];
target_rotated[1] = gm_in->target[0] * cm->rotation_matrix[1][0] +
gm_in->target[1] * cm->rotation_matrix[1][1] +
gm_in->target[2] * cm->rotation_matrix[1][2];
target_rotated[1] = _gm->target[0] * cm->rotation_matrix[1][0] +
_gm->target[1] * cm->rotation_matrix[1][1] +
_gm->target[2] * cm->rotation_matrix[1][2];
target_rotated[2] = gm_in->target[0] * cm->rotation_matrix[2][0] +
gm_in->target[1] * cm->rotation_matrix[2][1] +
gm_in->target[2] * cm->rotation_matrix[2][2] +
target_rotated[2] = _gm->target[0] * cm->rotation_matrix[2][0] +
_gm->target[1] * cm->rotation_matrix[2][1] +
_gm->target[2] * cm->rotation_matrix[2][2] +
cm->rotation_z_offset;
// copy rotation axes for ABC (no changes)
target_rotated[3] = gm_in->target[3];
target_rotated[4] = gm_in->target[4];
target_rotated[5] = gm_in->target[5];
target_rotated[3] = _gm->target[3];
target_rotated[4] = _gm->target[4];
target_rotated[5] = _gm->target[5];
for (uint8_t axis = 0; axis < AXES; axis++) {
axis_length[axis] = target_rotated[axis] - mp->position[axis];
@@ -212,13 +212,12 @@ stat_t mp_aline(GCodeState_t* gm_in)
if (bf == NULL) { // never supposed to fail
return (cm_panic(STAT_FAILED_GET_PLANNER_BUFFER, "aline()"));
}
memcpy(&bf->gm, gm_in, sizeof(GCodeState_t));
// Since bf->gm.target is being used all over the place, we'll make it the rotated target
memcpy(&bf->gm, _gm, sizeof(GCodeState_t));
copy_vector(bf->gm.target, target_rotated); // copy the rotated target in place
// setup the buffer
bf->bf_func = mp_exec_aline; // register the callback to the exec function
bf->length = length; // record the length
bf->length = length; // record the length
for (uint8_t axis = 0; axis < AXES; axis++) { // compute the unit vector and set flags
if ((bf->axis_flags[axis] = flags[axis])) { // yes, this is supposed to be = and not ==
bf->unit[axis] = axis_length[axis] / length;// nb: bf-> unit was cleared by mp_get_write_buffer()
g2core/planner.cpp
+1 -1
View File
@@ -262,7 +262,7 @@ void mp_set_runtime_position(uint8_t axis, const float position) { mr->position[
void mp_set_steps_to_runtime_position()
{
float step_position[MOTORS];
kn_inverse_kinematics(mr->position, step_position); // convert lengths to steps in floating point
kn_inverse_kinematics(mr->position, step_position); // convert lengths to steps in floating point
for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
mr->target_steps[motor] = step_position[motor];
mr->position_steps[motor] = step_position[motor];
g2core/planner.h
+4 -1
View File
@@ -338,6 +338,7 @@ typedef struct mpBuffer {
blockHint hint; // hint the block for zoid and other planning operations. Must be accurate or NO_HINT
// block parameters
// float position[AXES]; // XYZABC position at start of move
float unit[AXES]; // unit vector for axis scaling & planning
bool axis_flags[AXES]; // set true for axes participating in the move & for command parameters
@@ -390,6 +391,7 @@ typedef struct mpBuffer {
hint = NO_HINT;
for (uint8_t i = 0; i< AXES; i++) {
// position[i] = 0;
unit[i] = 0;
axis_flags[i] = 0;
}
@@ -454,6 +456,7 @@ 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)
@@ -629,7 +632,7 @@ void mp_set_runtime_display_offset(float offset[]);
bool mp_get_runtime_busy(void);
bool mp_runtime_is_idle(void);
stat_t mp_aline(GCodeState_t *gm_in); // line planning...
stat_t mp_aline(GCodeState_t *_gm); // line planning...
void mp_plan_block_list(void);
void mp_plan_block_forward(mpBuf_t *bf);