From 53d8af3b9f1ae7ea9efd97a337fcedc9d3e8026d Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Tue, 10 Jan 2017 11:16:46 -0500 Subject: [PATCH] Fixed queue size error; update local variable naming --- g2core/canonical_machine.cpp | 15 ++-- g2core/canonical_machine.h | 4 +- g2core/controller.cpp | 6 +- .../device/step_dir_driver/step_dir_driver.h | 10 +-- g2core/main.cpp | 14 +-- g2core/planner.cpp | 88 +++++++++---------- g2core/planner.h | 45 +++++----- g2core/stepper.cpp | 11 ++- g2core/stepper.h | 4 +- 9 files changed, 99 insertions(+), 98 deletions(-) diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index ae57f2dd..ecfa0264 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -114,8 +114,8 @@ ***********************************************************************************/ cmMachine_t *cm; // pointer to active canonical machine -cmMachine_t cm0; // canonical machine primary machine -cmMachine_t cm1; // canonical machine secondary machine +cmMachine_t cm1; // canonical machine primary machine +cmMachine_t cm2; // canonical machine secondary machine cmToolTable_t tt; // global tool table /*********************************************************************************** @@ -724,13 +724,13 @@ stat_t cm_test_soft_limits(const float target[]) void canonical_machine_init(cmMachine_t *m) { - // NoteL cm* was assignd in main() + // Note cm* was assignd in main() // If you can assume all memory has been zeroed by a hard reset you don't need this code: memset(m, 0, sizeof(cmMachine_t)); // do not reset canonicalMachine once it's been initialized - memset(&m->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status +// memset(&m->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status - memset(&gc.gn, 0, sizeof(GCodeInput_t)); // reset the Gcode inputs - memset(&gc.gf, 0, sizeof(GCodeFlags_t)); +// memset(&gc.gn, 0, sizeof(GCodeInput_t)); // reset the Gcode inputs +// memset(&gc.gf, 0, sizeof(GCodeFlags_t)); canonical_machine_init_assertions(m); // establish assertions ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer @@ -739,6 +739,9 @@ void canonical_machine_init(cmMachine_t *m) void canonical_machine_reset(cmMachine_t *m) { + // reset canonical machine assertions + canonical_machine_init_assertions(m); + // set gcode defaults cm_set_units_mode(gc.default_units_mode); cm_set_coord_system(gc.default_coord_system); // NB: queues a block to the planner with the coordinates diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 4f56c927..24c1da8d 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -278,8 +278,8 @@ typedef struct cmToolTable { // struct to keep a global tool tabl /**** Externs - See canonical_machine.cpp for allocation ****/ extern cmMachine_t *cm; // pointer to active canonical machine -extern cmMachine_t cm0; // canonical machine primary machine -extern cmMachine_t cm1; // canonical machine secondary machine +extern cmMachine_t cm1; // canonical machine primary machine +extern cmMachine_t cm2; // canonical machine secondary machine extern cmToolTable_t tt; /***************************************************************************** diff --git a/g2core/controller.cpp b/g2core/controller.cpp index 097bb099..cdae6602 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -475,8 +475,10 @@ stat_t _test_system_assertions() // these functions will panic if an assertion fails _test_assertions(); // controller assertions (local) config_test_assertions(); - canonical_machine_test_assertions(&cm0); // +++++ cleanup later - planner_test_assertions(&mp0); // +++++ cleanup later + canonical_machine_test_assertions(&cm1); // +++++ cleanup later +// canonical_machine_test_assertions(&cm2); // +++++ cleanup later + planner_test_assertions(&mp1); // +++++ cleanup later +// planner_test_assertions(&mp2); // +++++ cleanup later stepper_test_assertions(); encoder_test_assertions(); xio_test_assertions(); diff --git a/g2core/device/step_dir_driver/step_dir_driver.h b/g2core/device/step_dir_driver/step_dir_driver.h index da3cfc1e..1d0ccd28 100644 --- a/g2core/device/step_dir_driver/step_dir_driver.h +++ b/g2core/device/step_dir_driver/step_dir_driver.h @@ -2,8 +2,8 @@ * step_dir_driver.cpp - control over a Step/Direction/Enable stepper motor driver * This file is part of G2 project * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt + * Copyright (c) 2016 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2016 - 2017 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -25,8 +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. */ -#ifndef STEPP_DIR_DRIVER_H_ONCE -#define STEPP_DIR_DRIVER_H_ONCE +#ifndef STEP_DIR_DRIVER_H_ONCE +#define STEP_DIR_DRIVER_H_ONCE #include "MotatePins.h" #include "MotateTimers.h" @@ -146,4 +146,4 @@ struct StepDirStepper final : Stepper { }; }; -#endif // STEPP_DIR_DRIVER_H_ONCE +#endif // STEP_DIR_DRIVER_H_ONCE diff --git a/g2core/main.cpp b/g2core/main.cpp index f6858f31..f48c9df7 100644 --- a/g2core/main.cpp +++ b/g2core/main.cpp @@ -101,17 +101,17 @@ void application_init_services(void) void application_init_machine(void) { - cm = &cm0; // set global canonical machine pointer to primary machine + cm = &cm1; // set global canonical machine pointer to primary machine cm->machine_state = MACHINE_INITIALIZING; stepper_init(); // stepper subsystem encoder_init(); // virtual encoders gpio_init(); // inputs and outputs pwm_init(); // pulse width modulation drivers - planner_init(&mp0, mp0_pool, &mr0); //+++++ //primary motion planner -// planner_init(&mp1, mp1_pool, &mr1); //+++++ //secondary motion planner - canonical_machine_init(&cm0); //+++++ cleanup required // canonical machine -// canonical_machine_init(&cm1); //+++++ cleanup required // canonical machine + planner_init(&mp1, &mr1, mp1_pool, PLANNER_BUFFER_POOL_SIZE); // primary motion planner + planner_init(&mp2, &mr2, mp2_pool, SECONDARY_BUFFER_POOL_SIZE); // secondary motion planner + canonical_machine_init(&cm1); // primary canonical machine +// canonical_machine_init(&cm2); // secondary canonical machine } void application_init_startup(void) @@ -119,8 +119,8 @@ void application_init_startup(void) // start the application controller_init(); // should be first startup init (requires xio_init()) config_init(); // apply the config settings from persistence - canonical_machine_reset(&cm0); // +++++ -// canonical_machine_reset(&cm1); // +++++ + canonical_machine_reset(&cm1); +// canonical_machine_reset(&cm2); spindle_init(); // should be after PWM and canonical machine inits and config_init() spindle_reset(); temperature_init(); diff --git a/g2core/planner.cpp b/g2core/planner.cpp index 83f6b033..da61dd42 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -67,15 +67,15 @@ // Allocate planner structures mpPlanner_t *mp; // currently active planner (global variable) -mpPlanner_t mp0; // primary planning context -mpPlanner_t mp1; // secondary planning context +mpPlanner_t mp1; // primary planning context +mpPlanner_t mp2; // secondary planning context mpPlannerRuntime_t *mr; // context for planner block runtime -mpPlannerRuntime_t mr0; // runtime context for primary planner -mpPlannerRuntime_t mr1; // runtime context for secondary planner +mpPlannerRuntime_t mr1; // primary planner runtime context +mpPlannerRuntime_t mr2; // secondary planner runtime context -mpBuf_t mp0_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers -mpBuf_t mp1_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers +mpBuf_t mp1_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers +mpBuf_t mp2_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers #define JSON_COMMAND_BUFFER_SIZE 3 @@ -143,15 +143,15 @@ static stat_t _exec_json_wait(mpBuf_t *bf); /* * planner_init() * planner_reset() - * runtime_init() + * planner_test_assertions() - test assertions, PANIC if violation exists */ // initialize a planner queue -void _init_planner_queue(mpPlanner_t *mpl, mpBuf_t *pool, uint8_t size) +void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size) { mpBuf_t *pv, *nx; uint8_t i, nx_i; - mpPlannerQueue_t *q = &(mpl->q); + mpPlannerQueue_t *q = &(_mp->q); memset(q, 0, sizeof(mpPlannerQueue_t)); // clear values, pointers and status q->magic_start = MAGICNUM; @@ -161,7 +161,7 @@ void _init_planner_queue(mpPlanner_t *mpl, mpBuf_t *pool, uint8_t size) q->bf = pool; // link the buffer pool first q->w = pool; // init all buffer pointers q->r = pool; - q->queue_size = size-1; + q->queue_size = size; q->buffers_available = size; pv = &q->bf[size-1]; @@ -176,46 +176,42 @@ void _init_planner_queue(mpPlanner_t *mpl, mpBuf_t *pool, uint8_t size) q->bf[size-1].nx = pool; } -void planner_init(mpPlanner_t *mpl, mpBuf_t *pool, mpPlannerRuntime_t *mrl) +void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uint8_t _queue_size) { - mp = &mp0; // set global pointer to the primary planner - mr = &mr0; // and primary runtime + mp = &mp1; // set global pointer to the primary planner + mr = &mr1; // and primary runtime // init planner master structure - memset(mpl, 0, sizeof(mpPlanner_t));// clear all values, pointers and status - mpl->magic_start = MAGICNUM; // set boundary condition assertions - mpl->magic_end = MAGICNUM; - mpl->mfo_factor = 1.00; + memset(_mp, 0, sizeof(mpPlanner_t));// clear all values, pointers and status + _mp->magic_start = MAGICNUM; // set boundary condition assertions + _mp->magic_end = MAGICNUM; + _mp->mfo_factor = 1.00; // init planner queues - mpl->q.bf = pool; // assign puffer pool to queue manager structure - _init_planner_queue(mpl, pool, PLANNER_BUFFER_POOL_SIZE); + _mp->q.bf = _pool; // assign puffer pool to queue manager structure + _init_planner_queue(_mp, _pool, _queue_size); // init runtime structs - mpl->mr = mrl; - memset(mrl, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status - mrl->bf[0].nx = &mrl->bf[1]; // Handle the two "stub blocks" in the runtime structure. - mrl->bf[1].nx = &mrl->bf[0]; - mrl->r = &mrl->bf[0]; - mrl->p = &mrl->bf[1]; - mpl->mr->magic_start = MAGICNUM; // assertions - mpl->mr->magic_end = MAGICNUM; + _mp->mr = _mr; + memset(_mr, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status + _mr->bf[0].nx = &_mr->bf[1]; // Handle the two "stub blocks" in the runtime structure. + _mr->bf[1].nx = &_mr->bf[0]; + _mr->r = &_mr->bf[0]; + _mr->p = &_mr->bf[1]; + _mp->mr->magic_start = MAGICNUM; // assertions + _mp->mr->magic_end = MAGICNUM; } -void planner_reset(mpPlanner_t *mpl) +void planner_reset(mpPlanner_t *_mp) { - planner_init(mpl, mpl->q.bf, mpl->mr); // reset parent planner and linked Q and MR + planner_init(_mp, _mp->mr, _mp->q.bf, _mp->q.queue_size); // reset parent planner and linked Q and MR } -/* - * planner_test_assertions() - test assertions, PANIC if violation exists - */ - -stat_t planner_test_assertions(mpPlanner_t *mpl) +stat_t planner_test_assertions(mpPlanner_t *_mp) { if ( - (BAD_MAGIC(mpl->magic_start)) || (BAD_MAGIC(mpl->magic_end)) || - (BAD_MAGIC(mpl->mr->magic_start)) || (BAD_MAGIC(mpl->mr->magic_end)) + (BAD_MAGIC(_mp->magic_start)) || (BAD_MAGIC(_mp->magic_end)) || + (BAD_MAGIC(_mp->mr->magic_start)) || (BAD_MAGIC(_mp->mr->magic_end)) ) { return(cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()")); } @@ -247,12 +243,12 @@ void mp_halt_runtime() * This function is designed to be called during a hold to reset the planner * This function should not generally be called; call cm_queue_flush() instead */ -void mp_flush_planner(mpPlanner_t *mpl) +void mp_flush_planner(mpPlanner_t *_mp) { cm_abort_arc(); -// mp_init_planner_buffers(mpl); //+++++ -// planner_init(mpl); //+++++ - planner_reset(mpl); +// mp_init_planner_buffers(_mp); //+++++ +// planner_init(_mp); //+++++ + planner_reset(_mp); mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion } @@ -509,20 +505,20 @@ bool mp_has_runnable_buffer(int8_t q) // which queue are you interested in?) return (mb.q[q].r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true } */ -uint8_t mp_get_planner_buffers(mpPlanner_t *mpl) // which planner are you interested in? +uint8_t mp_get_planner_buffers(mpPlanner_t *_mp) // which planner are you interested in? { - return (mpl->q.buffers_available); + return (_mp->q.buffers_available); } -bool mp_planner_is_full(mpPlanner_t *mpl) // which planner are you interested in? +bool mp_planner_is_full(mpPlanner_t *_mp) // which planner are you interested in? { // We also need to ensure we have room for another JSON command - return ((mpl->q.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0)); + return ((_mp->q.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0)); } -bool mp_has_runnable_buffer(mpPlanner_t *mpl) // which planner are you interested in?) +bool mp_has_runnable_buffer(mpPlanner_t *_mp) // which planner are you interested in?) { - return (mpl->q.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true + return (_mp->q.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true } bool mp_is_phat_city_time() diff --git a/g2core/planner.h b/g2core/planner.h index 03c54643..130a717f 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -416,7 +416,7 @@ typedef struct mpPlannerQueue { // control structure for queue magic_t magic_start; // magic number to test memory integrity mpBuf_t *r; // run buffer pointer mpBuf_t *w; // write buffer pointer - uint8_t queue_size; // total number of buffers, zero-based (e.g. 47 not 48) + uint8_t queue_size; // total number of buffers, one-based (e.g. 48 not 47) uint8_t buffers_available; // running count of available buffers in queue mpBuf_t *bf; // pointer to buffer pool (storage array) magic_t magic_end; @@ -528,28 +528,29 @@ typedef struct mpPlanner { // common variables for a planner contex // Reference global scope structures -extern mpPlanner_t *mp; // currently active planner (global variable) -extern mpPlanner_t mp0; // primary planning context -extern mpPlanner_t mp1; // secondary planning context -extern mpBuf_t mp0_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers -extern mpBuf_t mp1_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers +extern mpPlanner_t *mp; // currently active planner (global variable) +extern mpPlanner_t mp1; // primary planning context +extern mpPlanner_t mp2; // secondary planning context -extern mpPlannerRuntime_t *mr; // context for block runtime -extern mpPlannerRuntime_t mr0; // runtime context for primary planner -extern mpPlannerRuntime_t mr1; // runtime context for secondary planner +extern mpPlannerRuntime_t *mr; // context for block runtime +extern mpPlannerRuntime_t mr1; // primary planner runtime context +extern mpPlannerRuntime_t mr2; // secondary planner runtime context + +extern mpBuf_t mp1_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers +extern mpBuf_t mp2_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers /* * Global Scope Functions */ -//planner.cpp functions +//**** planner.cpp functions -void planner_init(mpPlanner_t *mpl, mpBuf_t *pool, mpPlannerRuntime_t *mrl); -void planner_reset(mpPlanner_t *mpl); -stat_t planner_test_assertions(mpPlanner_t *mpl); +void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uint8_t _queue_size); +void planner_reset(mpPlanner_t *_mp); +stat_t planner_test_assertions(mpPlanner_t *_mp); void mp_halt_runtime(void); -void mp_flush_planner(mpPlanner_t *mpl); +void mp_flush_planner(mpPlanner_t *_mp); void mp_set_planner_position(uint8_t axis, const float position); void mp_set_runtime_position(uint8_t axis, const float position); void mp_set_steps_to_runtime_position(void); @@ -565,10 +566,10 @@ void mp_end_dwell(void); void mp_request_out_of_band_dwell(float seconds); stat_t mp_exec_out_of_band_dwell(void); -// planner functions and helpers -uint8_t mp_get_planner_buffers(mpPlanner_t *mpl); -bool mp_planner_is_full(mpPlanner_t *mpl); -bool mp_has_runnable_buffer(mpPlanner_t *mpl); +//**** planner functions and helpers +uint8_t mp_get_planner_buffers(mpPlanner_t *_mp); +bool mp_planner_is_full(mpPlanner_t *_mp); +bool mp_has_runnable_buffer(mpPlanner_t *_mp); bool mp_is_phat_city_time(void); stat_t mp_planner_callback(); @@ -577,7 +578,7 @@ void mp_start_feed_override(const float ramp_time, const float override); void mp_end_feed_override(const float ramp_time); void mp_planner_time_accounting(void); -// planner buffer primitives +//**** planner buffer primitives //void mp_init_planner_buffers(void); //mpBuf_t * mp_get_w(int8_t q); //mpBuf_t * mp_get_r(int8_t q); @@ -594,7 +595,7 @@ void mp_commit_write_buffer(const blockType block_type); mpBuf_t * mp_get_run_buffer(void); bool mp_free_run_buffer(void); -// plan_line.c functions +//**** plan_line.c functions void mp_zero_segment_velocity(void); // getters and setters... float mp_get_runtime_velocity(void); float mp_get_runtime_absolute_position(uint8_t axis); @@ -607,7 +608,7 @@ stat_t mp_aline(GCodeState_t *gm_in); // line planning... void mp_plan_block_list(void); void mp_plan_block_forward(mpBuf_t *bf); -// plan_zoid.c functions +//**** plan_zoid.c functions void mp_calculate_ramps(mpBlockRuntimeBuf_t *block, mpBuf_t *bf, const float entry_velocity); float mp_get_target_length(const float v_0, const float v_1, const mpBuf_t *bf); float mp_get_target_velocity(const float v_0, const float L, const mpBuf_t *bf); // acceleration ONLY @@ -619,7 +620,7 @@ float mp_calc_a(const float t, const float v_0, const float v_1, const float T); float mp_calc_j(const float t, const float v_0, const float v_1, const float T); // compute jerk over curve accelerating from v_0 to v_1, at position t=[0,1], total time T //float mp_calc_l(const float t, const float v_0, const float v_1, const float T); // compute length over curve accelerating from v_0 to v_1, at position t=[0,1], total time T -// plan_exec.c functions +//**** plan_exec.c functions stat_t mp_forward_plan(void); stat_t mp_exec_move(void); stat_t mp_exec_aline(mpBuf_t *bf); diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp index f3a69482..31d99aa7 100644 --- a/g2core/stepper.cpp +++ b/g2core/stepper.cpp @@ -2,8 +2,8 @@ * stepper.cpp - stepper motor controls * This file is part of the g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2017 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the @@ -80,7 +80,7 @@ load_timer_type load_timer; // triggers load of next stepper segment exec_timer_type exec_timer; // triggers calculation of next+1 stepper segment fwd_plan_timer_type fwd_plan_timer; // triggers planning of next block -// SystickEvent for handling dweels (must be registered before it is active) +// SystickEvent for handling dwells (must be registered before it is active) Motate::SysTickEvent dwell_systick_event {[&] { if (--st_run.dwell_ticks_downcount == 0) { SysTickTimer.unregisterEvent(&dwell_systick_event); @@ -800,11 +800,10 @@ void st_request_out_of_band_dwell(float microseconds) */ static void _set_hw_microsteps(const uint8_t motor, const uint8_t microsteps) { - if (motor >= MOTORS) {return;} + if (motor >= MOTORS) { return; } Motors[motor]->setMicrosteps(microsteps); - } - +} /*********************************************************************************** * CONFIGURATION AND INTERFACE FUNCTIONS diff --git a/g2core/stepper.h b/g2core/stepper.h index 26204c61..35f1770c 100644 --- a/g2core/stepper.h +++ b/g2core/stepper.h @@ -2,8 +2,8 @@ * stepper.h - stepper motor interface * This file is part of g2core project * - * Copyright (c) 2010 - 2016 Alden S. Hart, Jr. - * Copyright (c) 2013 - 2016 Robert Giseburt + * Copyright (c) 2010 - 2017 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2017 Robert Giseburt * * This file ("the software") is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License, version 2 as published by the