From 3dc69f4f0220ceb7fe72ffeed1d426d5fc27e40b Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sat, 18 Mar 2017 08:38:11 -0400 Subject: [PATCH] Got probing working; moved command values from gm.target to the unit vector; some name changes; --- g2core/canonical_machine.cpp | 4 +-- g2core/canonical_machine.h | 2 +- g2core/controller.cpp | 9 ++++--- g2core/cycle_feedhold.cpp | 11 +++++--- g2core/cycle_probing.cpp | 16 +++++------ g2core/plan_exec.cpp | 52 ++++++++++++++++++++++++------------ g2core/planner.cpp | 7 ++--- g2core/planner.h | 2 ++ g2core/util.h | 11 +++++--- 9 files changed, 70 insertions(+), 44 deletions(-) diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index c19860c4..8294fe57 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -1094,7 +1094,6 @@ stat_t cm_resume_origin_offsets() * cm_straight_traverse() - G0 linear rapid */ -//stat_t cm_straight_traverse(const float target[], const bool flags[]) stat_t cm_straight_traverse(const float *target, const bool *flags, const uint8_t motion_profile) { cm->gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; @@ -1247,8 +1246,7 @@ stat_t cm_dwell(const float seconds) /**************************************************************************************** * cm_straight_feed() - G1 */ -//stat_t cm_straight_feed(const float target[], const bool flags[]) -//stat_t cm_straight_feed(const float *target, const bool *flags) + stat_t cm_straight_feed(const float *target, const bool *flags, const uint8_t motion_profile) { // trap zero feed rate condition diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 564b48ec..431e04d3 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -488,7 +488,7 @@ stat_t cm_json_wait(char *json_string); // M102 // Feedhold and related functions (cycle_feedhold.cpp) void cm_operation_init(void); -stat_t cm_operation_sequencing_callback(void); // operation action runner +stat_t cm_operation_runner_callback(void); // operation action runner void cm_request_alarm(void); void cm_request_fasthold(void); diff --git a/g2core/controller.cpp b/g2core/controller.cpp index fa4d9441..68343740 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -142,7 +142,9 @@ static void _controller_HSM() // See hardware.h for a list of ISRs and their priorities. // //----- kernel level ISR handlers ----(flags are set in ISRs)------------------------// - // Order is important: + + // Order is important, and line breaks indicate dependency groups + DISPATCH(hardware_periodic()); // give the hardware a chance to do stuff DISPATCH(_led_indicator()); // blink LEDs at the current rate DISPATCH(_shutdown_handler()); // invoke shutdown @@ -159,15 +161,16 @@ static void _controller_HSM() DISPATCH(sr_status_report_callback()); // conditionally send status report DISPATCH(qr_queue_report_callback()); // conditionally send queue report -// DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner +++++ DISPATCH(mp_planner_callback()); // motion planner + DISPATCH(cm_arc_callback(cm)); // arc generation runs as a cycle above lines - DISPATCH(cm_operation_sequencing_callback());// operation action runner + DISPATCH(cm_operation_runner_callback()); // operation action runner DISPATCH(cm_homing_cycle_callback()); // homing cycle operation (G28.2) DISPATCH(cm_probing_cycle_callback()); // probing cycle operation (G38.2) DISPATCH(cm_jogging_cycle_callback()); // jog cycle operation DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle + DISPATCH(cm_feedhold_command_blocker()); // blocks new Gcode from arriving while in feedhold //----- command readers and parsers --------------------------------------------------// diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 76f89625..16d51161 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -234,7 +234,7 @@ void cm_operation_init() */ /**************************************************************************************** - * cm_operation_sequencing_callback() - run feedhold operations and sequence queued requests + * cm_operation_runner_callback() - run feedhold operations and sequence queued requests * * Operations are requested by calling their repective request function, e.g. cm_request_feedhold(). * The operation callback runs the current operation, and sequences requests that must be queued. @@ -258,7 +258,7 @@ void cm_operation_init() * handled in the sequencer. */ -stat_t cm_operation_sequencing_callback() +stat_t cm_operation_runner_callback() { if (cm1.job_kill_state == JOB_KILL_REQUESTED) { // job kill must wait for any active hold to complete _start_job_kill(); @@ -563,8 +563,11 @@ static stat_t _feedhold_with_command() if (cm1.hold_state < FEEDHOLD_HOLD_POINT_REACHED) { return (STAT_EAGAIN); } - cm1.hold_state = FEEDHOLD_HOLD; - st_request_exec_move(); +// cm1.hold_state = FEEDHOLD_OFF; // cannot be in HOLD or command won't run (see mp_plan_block_list()) + cm1.hold_state = FEEDHOLD_OFF; // cannot be in HOLD or command won't run (see mp_plan_block_list()) + mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks + st_request_forward_plan(); // replan from the new bf buffer +// st_request_exec_move(); return (STAT_OK); } diff --git a/g2core/cycle_probing.cpp b/g2core/cycle_probing.cpp index 08ec8843..93d300e5 100644 --- a/g2core/cycle_probing.cpp +++ b/g2core/cycle_probing.cpp @@ -55,7 +55,7 @@ struct pbProbingSingleton { // persistent probing runtime variables int8_t probe_input; // digital input to read bool trip_sense; // true if contact CLOSURE trips probe (true for G38.2 and G38.3) bool alarm_flag; // true if failure triggers alarm (true for G38.2 and G38.4) - bool wait_for_motion_end; // flag to know when the motion has ended + bool waiting_for_motion_complete; // true if waiting for a motion to complete stat_t (*func)(); // binding for callback function state machine // saved gcode model state @@ -170,7 +170,7 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al // queue a function to let us know when we can start probing cm->probe_state[0] = PROBE_WAITING; // wait until planner queue empties before starting movement - pb.wait_for_motion_end = true; + pb.waiting_for_motion_complete = true; mp_queue_command(_motion_end_callback, nullptr, nullptr); // note: these args are ignored return (STAT_OK); } @@ -186,12 +186,12 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al uint8_t cm_probing_cycle_callback(void) { if ((cm->cycle_type != CYCLE_PROBE) && (cm->probe_state[0] != PROBE_WAITING)) { - return (STAT_NOOP); // exit if not in a probing cycle + return (STAT_NOOP); // exit if not in a probing cycle } - if (pb.wait_for_motion_end) { // sync to planner move ends (using callback) + if (pb.waiting_for_motion_complete) { // sync to planner move ends (using callback) return (STAT_EAGAIN); } - return (pb.func()); // execute the current probing move + return (pb.func()); // execute the current probing move } /*********************************************************************************** @@ -204,16 +204,16 @@ uint8_t cm_probing_cycle_callback(void) static void _motion_end_callback(float* vect, bool* flag) { - pb.wait_for_motion_end = false; + pb.waiting_for_motion_complete = false; } static stat_t _probe_move(const float target[], const bool flags[]) { cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON); - pb.wait_for_motion_end = true; // set this BEFORE the motion starts + pb.waiting_for_motion_complete = true; // set this BEFORE the motion starts cm_straight_feed(target, flags, PROFILE_FAST); // NB: feed rate was set earlier, so it's OK mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway - st_request_forward_plan(); //+++++ +// st_request_forward_plan(); //+++++ return (STAT_EAGAIN); } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index bb170678..823c9651 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -184,11 +184,23 @@ static stat_t _plan_aline(mpBuf_t *bf, float entry_velocity) return (STAT_OK); // report that we planned something... } +#pragma GCC push_options // DIAGNOSTIC +++++ +#pragma GCC optimize ("O0") // DIAGNOSTIC +++++ + stat_t mp_forward_plan() { mpBuf_t *bf = mp_get_run_buffer(); float entry_velocity; + if (bf->buffer_number == 15) { + if (bf->buffer_state == MP_BUFFER_BACK_PLANNED) { + bf->hint = ZERO_VELOCITY; + } + } + if (bf->buffer_number == 17) { + bf->hint = ZERO_VELOCITY; + } + // Case 0: Examine current running buffer for early exit conditions if (bf == NULL) { // case 0a: NULL means nothing is running - this is OK st_prep_null(); @@ -234,6 +246,9 @@ stat_t mp_forward_plan() return (planned_something ? STAT_OK : STAT_NOOP); } +// insert function here +#pragma GCC reset_options + /************************************************************************* * mp_exec_move() - execute runtime functions to prep move for steppers * @@ -1032,31 +1047,34 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) if (mp_runtime_is_idle()) { // wait for steppers to actually finish mp_zero_segment_velocity(); // finalize velocity for reporting purposes cm->hold_state = FEEDHOLD_MOTION_STOPPED; + cm_set_motion_state(MOTION_STOP); + // Motion has stopped, so we can rely on positions and other values to be stable // If in a p2 hold, exit the p2 hold immediately set up a flush of the p2 planner queue - if (cm == &cm2) { - cm->hold_state = FEEDHOLD_HOLD; - return (STAT_OK); // will end this exec_aline() with no more movement - } - // At this point we know we are in a p1 hold +// if (cm == &cm2) { +// cm->hold_state = FEEDHOLD_HOLD; +// return (STAT_OK); // will end this exec_aline() with no more movement +// } +// // At this point we know we are in a p1 hold // If probing or homing, exit the move and advance to the _motion_end_callback()'s. // Stop the runtime, clear the run buffer and do not transition to p2 planner. // else if ((cm->cycle_type == CYCLE_HOMING) || (cm->cycle_type == CYCLE_PROBE)) { - else if (cm->hold_type == FEEDHOLD_TYPE_COMMAND) { - mr->block_state = BLOCK_INACTIVE; // disable the rest of the runtime movement - mp_free_run_buffer(); // free buffer and enable finalization move to get loaded - copy_vector(mp->position, mr->position); +// else + // In COMMAND type, discard the remainder of the block and position to the next block + if (cm->hold_type == FEEDHOLD_TYPE_COMMAND) { + float next_entry_velocity = bf->exit_velocity; + mp_free_run_buffer(); // advance to next block, discarding the rest of the move + if (next_entry_velocity > 0) { // only need to replan if new block dis not plan from zero + mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks + st_request_forward_plan(); // replan from the new bf buffer + } + copy_vector(mp->position, mr->position); // update planner position to the final runtime position +// mr->block_state = BLOCK_INACTIVE; // disable the rest of the runtime movement + mr->reset(); // disable the rest of the runtime movement cm->hold_state = FEEDHOLD_HOLD_POINT_REACHED; - -// mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks -// st_request_forward_plan(); // replan the current bf buffer } - - // In a regular p1 hold. Motion has stopped, so we can rely on positions and other values to be stable - else { - - // Reset the state of the p1 planner regardless of how hold will ultimately be exited. + else { // Reset the state of the planner regardless of how hold will ultimately be exited. bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move 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 diff --git a/g2core/planner.cpp b/g2core/planner.cpp index 9c20f43b..77775fcb 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -77,9 +77,6 @@ mpPlannerRuntime_t mr2; // secondary planner runtime context mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers -// Local Scope Data and Functions -#define value_vector gm.target // alias for vector of values - // Execution routines (NB: These are called from the LO interrupt) static stat_t _exec_dwell(mpBuf_t *bf); static stat_t _exec_command(mpBuf_t *bf); @@ -313,7 +310,7 @@ void mp_queue_command(void(*cm_exec)(float *, bool *), float *value, bool *flag) bf->cm_func = cm_exec; // callback to canonical machine exec function for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - bf->value_vector[axis] = value[axis]; + bf->unit[axis] = value[axis]; // use the unit vector to store command values bf->axis_flags[axis] = flag[axis]; } mp_commit_write_buffer(BLOCK_TYPE_COMMAND); // must be final operation before exit @@ -327,7 +324,7 @@ static stat_t _exec_command(mpBuf_t *bf) stat_t mp_runtime_command(mpBuf_t *bf) { - bf->cm_func(bf->value_vector, bf->axis_flags); // 2 vectors used by callbacks + bf->cm_func(bf->unit, bf->axis_flags); // 2 vectors used by callbacks if (mp_free_run_buffer()) { cm_cycle_end(); // free buffer & perform cycle_end if planner is empty } diff --git a/g2core/planner.h b/g2core/planner.h index e5cd9b68..905b5037 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -491,6 +491,8 @@ typedef struct mpPlannerRuntime { // persistent runtime variables block_state = BLOCK_INACTIVE; section = SECTION_HEAD; section_state = SECTION_OFF; + entry_velocity = 0; // needed to ensure next block in forward planning starts from 0 velocity + r->exit_velocity = 0; // ditto } } mpPlannerRuntime_t; diff --git a/g2core/util.h b/g2core/util.h index b69b7c7e..b97d8479 100644 --- a/g2core/util.h +++ b/g2core/util.h @@ -25,9 +25,8 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* util.c/.h contains a dog's breakfast of supporting functions that are - * not specific to g2core: including: - * +/* util.c/.h contains a dog's breakfast of supporting functions that are not specific + * to g2core: including: * - math and min/max utilities and extensions * - vector manipulation utilities * - support for debugging routines @@ -49,6 +48,12 @@ using Motate::SysTickTimer; #include // isnan, isinf /****** Global Scope Variables and Functions ******/ +/* +#pragma GCC push_options // DIAGNOSTIC +++++ +#pragma GCC optimize ("O0") // DIAGNOSTIC +++++ +// insert function here +#pragma GCC reset_options // DIAGNOSTIC +++++ +*/ //*** vector utilities ***