From dc28c0006ea5075b97d44f2958c3ea57c7f5abb1 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Thu, 2 Mar 2017 15:20:33 -0500 Subject: [PATCH] Checkpoint --- g2core/gcode.h | 3 ++- g2core/plan_exec.cpp | 14 +++++++------- g2core/plan_line.cpp | 33 ++++++++++++++++----------------- g2core/planner.cpp | 2 +- g2core/planner.h | 5 ++++- 5 files changed, 30 insertions(+), 27 deletions(-) diff --git a/g2core/gcode.h b/g2core/gcode.h index 048355c3..e8fee655 100644 --- a/g2core/gcode.h +++ b/g2core/gcode.h @@ -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 diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index ad0e0ed4..938f47cf 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -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 diff --git a/g2core/plan_line.cpp b/g2core/plan_line.cpp index 18eae645..dde71515 100644 --- a/g2core/plan_line.cpp +++ b/g2core/plan_line.cpp @@ -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() diff --git a/g2core/planner.cpp b/g2core/planner.cpp index 64eaff3a..3c6771b7 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -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]; diff --git a/g2core/planner.h b/g2core/planner.h index 7f9ee27e..c6ebe396 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -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);