From 11d39c93b5ad193e2aebddad5652fc1558d22180 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sun, 23 Apr 2017 08:30:26 -0400 Subject: [PATCH] Cleanup - interim commit --- g2core/alarm.cpp | 1 - g2core/canonical_machine.h | 2 -- g2core/cycle_feedhold.cpp | 62 +++++++++++++++++++++++++++----------- g2core/plan_exec.cpp | 16 ---------- g2core/planner.h | 1 - g2core/stepper.cpp | 12 -------- g2core/util.h | 4 +-- 7 files changed, 47 insertions(+), 51 deletions(-) diff --git a/g2core/alarm.cpp b/g2core/alarm.cpp index 975a6e48..a87a2d15 100644 --- a/g2core/alarm.cpp +++ b/g2core/alarm.cpp @@ -196,7 +196,6 @@ stat_t cm_shutdown(const stat_t status, const char *msg) } cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_EXIT_SHUTDOWN); // fast stop and shutdown -//+++++ cm_halt_motion(); // halt motors (may have already been done from GPIO) // spindle_reset(); // stop spindle immediately and set speed to 0 RPM // coolant_reset(); // stop coolant immediately // temperature_reset(); // turn off heaters and fans diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 5aa0992f..beaf85a8 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -281,8 +281,6 @@ typedef struct cmMachine { // struct to manage canonical machin /**** Runtime variables (PRIVATE) ****/ - uint32_t test; //+++++ - // Global state variables and flags cmMachineState machine_state; // macs: machine/cycle/motion is the actual machine state diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 2f44894a..101f15cb 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -59,20 +59,6 @@ static stat_t _run_alarm(void); static stat_t _run_shutdown(void); static stat_t _run_interlock(void); -#pragma GCC push_options // DIAGNOSTIC +++++ -#pragma GCC optimize ("O0") // DIAGNOSTIC +++++ -static void _hold_everything (uint32_t n1, uint32_t n2) // example of function -{ - if (cm->gm.linenum > 200) { - cm1.gm.linenum += 1; - } - -// if (n1 == n2) { -// cm1.gm.linenum = n1; -// } -} -#pragma GCC reset_options // DIAGNOSTIC +++++ - /**************************************************************************************** * OPERATIONS AND ACTIONS * @@ -523,7 +509,7 @@ void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) { // Can only initiate a feedhold if you are in a machining cycle, running, and not already in a feedhold - // +++++ This needs to be extended to allow HOLDs to be requested when motion has stopped +++++ + // +++++ This needs to be extended to allow HOLDs to be requested when motion has stopped if ((cm1.hold_state == FEEDHOLD_OFF) && (cm1.machine_state == MACHINE_CYCLE) && (cm1.motion_state == MOTION_RUN)) { @@ -646,6 +632,47 @@ static void _feedhold_actions_done_callback(float* vect, bool* flag) sr_request_status_report(SR_REQUEST_IMMEDIATE); } +static void _enter_p2() +{ + // copy the primary canonical machine to the secondary, + // fix the planner pointer, and reset the secondary planner + memcpy(&cm2, &cm1, sizeof(cmMachine_t)); + cm2.mp = &mp2; + planner_reset((mpPlanner_t *)cm2.mp); // mp is a void pointer + + // set parameters in cm, gm and gmx so you can actually use it + cm2.hold_state = FEEDHOLD_OFF; + cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; + cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF; + cm2.queue_flush_state = QUEUE_FLUSH_OFF; + cm2.gm.feed_rate = 0; + + // clear the target and set the positions to the current hold position + memset(&(cm2.gm.target), 0, sizeof(cm2.gm.target)); + memset(&(cm2.return_flags), 0, sizeof(cm2.return_flags)); + copy_vector(cm2.gm.target_comp, cm1.gm.target_comp); // preserve original Kahan compensation + copy_vector(cm2.gmx.position, mr1.position); + copy_vector(mp2.position, mr1.position); + copy_vector(mr2.position, mr1.position); + + // copy encoder and error correction terms + copy_vector(mr2.target_steps, mr1.target_steps); + copy_vector(mr2.position_steps, mr1.position_steps); + copy_vector(mr2.commanded_steps, mr1.commanded_steps); + copy_vector(mr2.encoder_steps, mr1.encoder_steps); + copy_vector(mr2.following_error, mr1.following_error); + + // reassign the globals to the secondary CM + cm = &cm2; + mp = (mpPlanner_t *)cm->mp; // mp is a void pointer + mr = mp->mr; +} + +static void _exit_p2() +{ + +} + static stat_t _feedhold_with_actions() // Execute Case (5) { // if entered while OFF start a feedhold @@ -665,6 +692,8 @@ static stat_t _feedhold_with_actions() // Execute Case (5) if (cm1.hold_state == FEEDHOLD_MOTION_STOPPED) { cm->hold_state = FEEDHOLD_HOLD_ACTIONS_PENDING; // next state + _enter_p2(); +/* // copy the primary canonical machine to the secondary, // fix the planner pointer, and reset the secondary planner memcpy(&cm2, &cm1, sizeof(cmMachine_t)); @@ -697,13 +726,13 @@ static stat_t _feedhold_with_actions() // Execute Case (5) mp = (mpPlanner_t *)cm->mp; // mp is a void pointer mr = mp->mr; +*/ // set a return position cm_set_g30_position(); // execute feedhold actions if (fp_NOT_ZERO(cm->feedhold_z_lift)) { // optional Z lift cm_set_distance_mode(INCREMENTAL_DISTANCE_MODE); -// cm->gm.linenum = 6060842; // ++++++++ Diagnostic bool flags[] = { 0,0,1,0,0,0 }; float target[] = { 0,0, _to_inches(cm->feedhold_z_lift), 0,0,0 }; // convert to inches if in inches mode cm_straight_traverse(target, flags, PROFILE_NORMAL); @@ -717,7 +746,6 @@ static stat_t _feedhold_with_actions() // Execute Case (5) // wait for hold actions to complete if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_PENDING) { - _hold_everything(0,0); //+++++ return (STAT_EAGAIN); } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index e93fd33a..b4a8effa 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -48,20 +48,6 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf); static void _init_forward_diffs(float v_0, float v_1); -#pragma GCC push_options // DIAGNOSTIC +++++ -#pragma GCC optimize ("O0") // DIAGNOSTIC +++++ -static void _hold_everything (uint32_t n1, uint32_t n2) // example of function -{ - if (cm->gm.linenum > 237) { - cm1.gm.linenum += 1; - } - -// if (n1 == n2) { -// cm1.gm.linenum = n1; -// } -} -#pragma GCC reset_options // DIAGNOSTIC +++++ - /**************************************************************************************** * mp_forward_plan() - plan commands and moves ahead of exec; call ramping for moves * @@ -282,8 +268,6 @@ stat_t mp_exec_move() if (bf->block_type == BLOCK_TYPE_ALINE) { // cycle auto-start for lines only // first-time operations -// _hold_everything(0,0); //+++++ - if (bf->buffer_state != MP_BUFFER_RUNNING) { if ((bf->buffer_state < MP_BUFFER_BACK_PLANNED) && (cm->motion_state == MOTION_RUN)) { // debug_trap("mp_exec_move() buffer is not prepped. Starvation"); // IMPORTANT: can't rpt_exception from here! diff --git a/g2core/planner.h b/g2core/planner.h index 1d878aab..d2006685 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -456,7 +456,6 @@ typedef struct mpPlannerRuntime { // persistent runtime variables float position[AXES]; // current move position float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction -// float travel_steps[MOTORS]; // +++++ float target_steps[MOTORS]; // current MR target (absolute target as steps) float position_steps[MOTORS]; // current MR position (target from previous segment) float commanded_steps[MOTORS]; // will align with next encoder sample (target from 2nd previous segment) diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp index d4feb9e7..f33387b6 100644 --- a/g2core/stepper.cpp +++ b/g2core/stepper.cpp @@ -58,16 +58,6 @@ static stRunSingleton_t st_run; static void _load_move(void); -#pragma GCC push_options // DIAGNOSTIC +++++ -#pragma GCC optimize ("O0") // DIAGNOSTIC +++++ -static void _hold_everything (uint32_t n1) -{ - if (n1 > 2000000000) { // 2142329634 - cm2.gm.linenum = 1111; - } -} -#pragma GCC reset_options // DIAGNOSTIC +++++ - /**** Setup motate ****/ using namespace Motate; @@ -712,8 +702,6 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment // that results in long-term negative drift. (fabs/round order doesn't matter) st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] * DDA_SUBSTEPS)); - - _hold_everything(st_pre.mot[motor].substep_increment); } st_pre.block_type = BLOCK_TYPE_ALINE; st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready diff --git a/g2core/util.h b/g2core/util.h index e692ad56..b0feef23 100644 --- a/g2core/util.h +++ b/g2core/util.h @@ -50,7 +50,7 @@ using Motate::SysTickTimer; /****** Global Scope Variables and Functions ******/ /* #pragma GCC push_options // DIAGNOSTIC +++++ -#pragma GCC optimize ("O0") // DIAGNOSTIC +++++ +#pragma GCC optimize ("O0") // insert function here static void _hold_everything (uint32_t n1, uint32_t n2) // example of function { @@ -58,7 +58,7 @@ static void _hold_everything (uint32_t n1, uint32_t n2) // example of function cm1.gm.linenum +=1; } } -#pragma GCC reset_options // DIAGNOSTIC +++++ +#pragma GCC reset_options */ //*** vector utilities ***