Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 02:51:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Checkpoint. Running 2 planners. Passes (most) regressions; separated Gcode inits from CM inits; added CM dependency injection to arc commands

Browse Source
This commit is contained in:
Alden Hart
2017-01-10 12:36:33 -05:00
parent 53d8af3b9f
commit cb0ab6c787
10 changed files with 174 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
g2core/canonical_machine.cpp
View File

@@ -135,6 +135,100 @@ static int8_t _axis(const index_t index);
**** CODE *************************************************************************
***********************************************************************************/
/*
* canonical_machine_init() - initialize cm struct
* canonical_machine_reset() - apply startup settings or reset to startup
* canonical_machine_reset_rotation()
*/
void canonical_machine_init(cmMachine_t *_cm)
{
// 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(_cm, 0, sizeof(cmMachine_t)); // do not reset canonicalMachine once it's been initialized
memset(&_cm->gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status
canonical_machine_init_assertions(_cm); // establish assertions
ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer
cm_arc_init(_cm); // Note: spindle and coolant inits are independent
}
// Note: run canonical_machine_init and profile initializations beforehand
void canonical_machine_reset(cmMachine_t *_cm)
{
// reset canonical machine assertions
canonical_machine_init_assertions(_cm);
// set canonical machine 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
cm_select_plane(gc.default_select_plane);
cm_set_path_control(MODEL, gc.default_path_control);
cm_set_distance_mode(gc.default_distance_mode);
cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE); // always the default
cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default
cm_reset_overrides(); // set overrides to initial conditions
// NOTE: Should unhome axes here
// reset request flags
_cm->queue_flush_state = FLUSH_OFF;
_cm->end_hold_requested = false;
_cm->limit_requested = 0; // resets switch closures that occurred during initialization
_cm->safety_interlock_disengaged = 0; // ditto
_cm->safety_interlock_reengaged = 0; // ditto
_cm->shutdown_requested = 0; // ditto
// set initial state and signal that the machine is ready for action
_cm->cycle_state = CYCLE_OFF;
_cm->motion_state = MOTION_STOP;
_cm->hold_state = FEEDHOLD_OFF;
_cm->esc_boot_timer = SysTickTimer_getValue();
_cm->gmx.block_delete_switch = true;
_cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode
_cm->machine_state = MACHINE_READY;
canonical_machine_reset_rotation(_cm);
memset(&_cm->probe_state, 0, sizeof(cmProbeState)*PROBES_STORED);
memset(&_cm->probe_results, 0, sizeof(float)*PROBES_STORED*AXES);
}
void canonical_machine_reset_rotation(cmMachine_t *_cm) {
// We must make it an identity matrix for no rotation
memset(&_cm->rotation_matrix, 0, sizeof(float)*3*3);
_cm->rotation_matrix[0][0] = 1.0;
_cm->rotation_matrix[1][1] = 1.0;
_cm->rotation_matrix[2][2] = 1.0;
_cm->rotation_z_offset = 0.0;
}
/*
* canonical_machine_init_assertions()
* canonical_machine_test_assertions() - test assertions, return error code if violation exists
*/
void canonical_machine_init_assertions(cmMachine_t *_cm)
{
_cm->magic_start = MAGICNUM;
_cm->magic_end = MAGICNUM;
_cm->gmx.magic_start = MAGICNUM;
_cm->gmx.magic_end = MAGICNUM;
_cm->arc.magic_start = MAGICNUM;
_cm->arc.magic_end = MAGICNUM;
}
stat_t canonical_machine_test_assertions(cmMachine_t *_cm)
{
if ((BAD_MAGIC(_cm->magic_start)) || (BAD_MAGIC(_cm->magic_end)) ||
(BAD_MAGIC(_cm->gmx.magic_start)) || (BAD_MAGIC(_cm->gmx.magic_end)) ||
(BAD_MAGIC(_cm->arc.magic_start)) || (BAD_MAGIC(_cm->arc.magic_end))) {
return(cm_panic(STAT_CANONICAL_MACHINE_ASSERTION_FAILURE, "canonical_machine_test_assertions()"));
}
return (STAT_OK);
}
/*************************************
* Internal getters and setters *
* Canonical Machine State functions *
@@ -713,103 +807,7 @@ stat_t cm_test_soft_limits(const float target[])
* found in NIST RS274 NGCv3
************************************************************************/
/******************************************
* Initialization and Termination (4.3.2) *
******************************************/
/*
* canonical_machine_init() - initialize cm struct
* canonical_machine_reset() - apply startup settings or reset to startup
* run profile initialization beforehand
*/
void canonical_machine_init(cmMachine_t *m)
{
// 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(&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
cm_arc_init(); // Note: spindle and coolant inits are independent
}
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
cm_select_plane(gc.default_select_plane);
cm_set_path_control(MODEL, gc.default_path_control);
cm_set_distance_mode(gc.default_distance_mode);
cm_set_arc_distance_mode(INCREMENTAL_DISTANCE_MODE); // always the default
cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default
cm_reset_overrides(); // set overrides to initial conditions
// NOTE: Should unhome axes here
// reset request flags
m->queue_flush_state = FLUSH_OFF;
m->end_hold_requested = false;
m->limit_requested = 0; // resets switch closures that occurred during initialization
m->safety_interlock_disengaged = 0; // ditto
m->safety_interlock_reengaged = 0; // ditto
m->shutdown_requested = 0; // ditto
// set initial state and signal that the machine is ready for action
m->cycle_state = CYCLE_OFF;
m->motion_state = MOTION_STOP;
m->hold_state = FEEDHOLD_OFF;
m->esc_boot_timer = SysTickTimer_getValue();
m->gmx.block_delete_switch = true;
m->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode
m->machine_state = MACHINE_READY;
canonical_machine_reset_rotation(m);
memset(&m->probe_state, 0, sizeof(cmProbeState)*PROBES_STORED);
memset(&m->probe_results, 0, sizeof(float)*PROBES_STORED*AXES);
}
void canonical_machine_reset_rotation(cmMachine_t *m) {
memset(&cm->rotation_matrix, 0, sizeof(float)*3*3);
// We must make it an identity matrix for no rotation
m->rotation_matrix[0][0] = 1.0;
m->rotation_matrix[1][1] = 1.0;
m->rotation_matrix[2][2] = 1.0;
m->rotation_z_offset = 0.0;
}
/*
* canonical_machine_init_assertions()
* canonical_machine_test_assertions() - test assertions, return error code if violation exists
*/
void canonical_machine_init_assertions(cmMachine_t *m)
{
m->magic_start = MAGICNUM;
m->magic_end = MAGICNUM;
m->gmx.magic_start = MAGICNUM;
m->gmx.magic_end = MAGICNUM;
m->arc.magic_start = MAGICNUM;
m->arc.magic_end = MAGICNUM;
}
stat_t canonical_machine_test_assertions(cmMachine_t *m)
{
if ((BAD_MAGIC(m->magic_start)) || (BAD_MAGIC(m->magic_end)) ||
(BAD_MAGIC(m->gmx.magic_start)) || (BAD_MAGIC(m->gmx.magic_end)) ||
(BAD_MAGIC(m->arc.magic_start)) || (BAD_MAGIC(m->arc.magic_end))) {
return(cm_panic(STAT_CANONICAL_MACHINE_ASSERTION_FAILURE, "canonical_machine_test_assertions()"));
}
return (STAT_OK);
}
/**************************
* Alarms *

10
g2core/canonical_machine.h
View File

@@ -340,11 +340,11 @@ stat_t cm_test_soft_limits(const float target[]);
/*--- Canonical machining functions (loosely) defined by NIST [organized by NIST Gcode doc] ---*/
// Initialization and termination (4.3.2)
void canonical_machine_init(cmMachine_t *machine);
void canonical_machine_reset_rotation(cmMachine_t *machine); // NOT in NIST
void canonical_machine_reset(cmMachine_t *machine);
void canonical_machine_init_assertions(cmMachine_t *machine);
stat_t canonical_machine_test_assertions(cmMachine_t *machine);
void canonical_machine_init(cmMachine_t *_cm);
void canonical_machine_reset_rotation(cmMachine_t *_cm); // NOT in NIST
void canonical_machine_reset(cmMachine_t *_cm);
void canonical_machine_init_assertions(cmMachine_t *_cm);
stat_t canonical_machine_test_assertions(cmMachine_t *_cm);
// Alarms and state management
stat_t cm_alrm(nvObj_t *nv); // trigger alarm from command input

6
g2core/controller.cpp
View File

@@ -2,8 +2,8 @@
* controller.cpp - g2core controller and top level parser
* 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
@@ -161,7 +161,7 @@ static void _controller_HSM()
DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner
DISPATCH(mp_planner_callback()); // motion planner
DISPATCH(cm_arc_callback()); // arc generation runs as a cycle above lines
DISPATCH(cm_arc_callback(cm)); // arc generation runs as a cycle above lines
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

1
g2core/gcode.h
View File

@@ -386,6 +386,7 @@ extern GCodeSingleton_t gc; // gcode structs
/*
* Global Scope Functions
*/
void gcode_parser_init(void);
stat_t gcode_parser(char* block);
stat_t gc_get_gc(nvObj_t* nv);
stat_t gc_run_gc(nvObj_t* nv);

14
g2core/gcode_parser.cpp
View File

@@ -2,8 +2,8 @@
* gcode_parser.cpp - rs274/ngc Gcode parser
* This file is part of the g2core project
*
* Copyright (c) 2010 - 2016 Alden S. Hart, Jr.
* Copyright (c) 2016 Rob Giseburt
* Copyright (c) 2010 - 2017 Alden S. Hart, Jr.
* Copyright (c) 2016 - 2017 Rob 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
@@ -41,6 +41,16 @@ static stat_t _execute_gcode_block(char *active_comment); // Execute t
#define SET_NON_MODAL(parm,val) ({gc.gn.parm=val; gc.gf.parm=true; break;})
#define EXEC_FUNC(f,v) if(gc.gf.v) { status=f(gc.gn.v);}
/*
* gcode_parser_init()
*/
void gcode_parser_init()
{
memset(&gc.gn, 0, sizeof(GCodeInput_t)); // reset the Gcode inputs
memset(&gc.gf, 0, sizeof(GCodeFlags_t));
}
/*
* gcode_parser() - parse a block (line) of gcode
*

11
g2core/main.cpp
View File

@@ -108,10 +108,11 @@ void application_init_machine(void)
encoder_init(); // virtual encoders
gpio_init(); // inputs and outputs
pwm_init(); // pulse width modulation drivers
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
planner_init(&mp1, &mr1, mp1_queue, PLANNER_QUEUE_SIZE);
planner_init(&mp2, &mr2, mp2_queue, SECONDARY_QUEUE_SIZE);
canonical_machine_init(&cm1); // primary canonical machine
// canonical_machine_init(&cm2); // secondary canonical machine
canonical_machine_init(&cm2); // secondary canonical machine
}
void application_init_startup(void)
@@ -119,8 +120,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(&cm1);
// canonical_machine_reset(&cm2);
canonical_machine_reset(&cm1); // initialize both CMs but only reset the primary
gcode_parser_init(); // baseline Gcode parser
spindle_init(); // should be after PWM and canonical machine inits and config_init()
spindle_reset();
temperature_init();

32
g2core/plan_arc.cpp
View File

@@ -2,7 +2,7 @@
* plan_arc.c - arc planning and motion execution
* This file is part of the g2core project
*
* Copyright (c) 2010 - 2016 Alden S. Hart, Jr.
* Copyright (c) 2010 - 2017 Alden S. Hart, Jr.
*
* 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
@@ -47,10 +47,10 @@ static stat_t _test_arc_soft_limits(void);
/*
* cm_arc_init() - initialize arc structures
*/
void cm_arc_init()
void cm_arc_init(cmMachine_t *_cm)
{
cm->arc.magic_start = MAGICNUM;
cm->arc.magic_end = MAGICNUM;
_cm->arc.magic_start = MAGICNUM;
_cm->arc.magic_end = MAGICNUM;
}
/*
@@ -59,9 +59,9 @@ void cm_arc_init()
* OK to call if no arc is running
*/
void cm_abort_arc()
void cm_abort_arc(cmMachine_t *_cm)
{
cm->arc.run_state = BLOCK_INACTIVE;
_cm->arc.run_state = BLOCK_INACTIVE;
}
/*
@@ -73,26 +73,26 @@ void cm_abort_arc()
* Parts of this routine were informed by the grbl project.
*/
stat_t cm_arc_callback()
stat_t cm_arc_callback(cmMachine_t *_cm)
{
if (cm->arc.run_state == BLOCK_INACTIVE) {
if (_cm->arc.run_state == BLOCK_INACTIVE) {
return (STAT_NOOP);
}
if (mp_planner_is_full(mp)) { //+++++
return (STAT_EAGAIN);
}
cm->arc.theta += cm->arc.segment_theta;
cm->arc.gm.target[cm->arc.plane_axis_0] = cm->arc.center_0 + sin(cm->arc.theta) * cm->arc.radius;
cm->arc.gm.target[cm->arc.plane_axis_1] = cm->arc.center_1 + cos(cm->arc.theta) * cm->arc.radius;
cm->arc.gm.target[cm->arc.linear_axis] += cm->arc.segment_linear_travel;
_cm->arc.theta += _cm->arc.segment_theta;
_cm->arc.gm.target[_cm->arc.plane_axis_0] = _cm->arc.center_0 + sin(_cm->arc.theta) * _cm->arc.radius;
_cm->arc.gm.target[_cm->arc.plane_axis_1] = _cm->arc.center_1 + cos(_cm->arc.theta) * _cm->arc.radius;
_cm->arc.gm.target[_cm->arc.linear_axis] += _cm->arc.segment_linear_travel;
mp_aline(&(cm->arc.gm)); // run the line
copy_vector(cm->arc.position, cm->arc.gm.target); // update arc current position
mp_aline(&(_cm->arc.gm)); // run the line
copy_vector(_cm->arc.position, _cm->arc.gm.target); // update arc current position
if (--(cm->arc.segment_count) > 0) {
if (--(_cm->arc.segment_count) > 0) {
return (STAT_EAGAIN);
}
cm->arc.run_state = BLOCK_INACTIVE;
_cm->arc.run_state = BLOCK_INACTIVE;
return (STAT_OK);
}

46
g2core/plan_arc.h
View File

@@ -2,7 +2,7 @@
* plan_arc.h - arc planning and motion execution
* This file is part of the g2core project
*
* Copyright (c) 2010 - 2016 Alden S. Hart, Jr.
* Copyright (c) 2010 - 2017 Alden S. Hart, Jr.
*
* 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
@@ -31,49 +31,11 @@
#define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test
#define CHORDAL_TOLERANCE_MIN (0.001) // values below this are not accepted
/*
typedef struct arArcSingleton { // persistent planner and runtime variables
magic_t magic_start;
uint8_t run_state; // runtime state machine sequence
float position[AXES]; // accumulating runtime position
float offset[3]; // arc IJK offsets
float length; // length of line or helix in mm
float radius; // Raw R value, or computed via offsets
float theta; // starting angle of arc
float angular_travel; // travel along the arc in radians
float planar_travel; // travel in arc plane in mm
float linear_travel; // travel along linear axis of arc in mm
bool full_circle; // True if full circle arcs specified
float rotations; // number of full rotations to add (P value + sign)
cmAxes plane_axis_0; // arc plane axis 0 - e.g. X for G17
cmAxes plane_axis_1; // arc plane axis 1 - e.g. Y for G17
cmAxes linear_axis; // linear axis (normal to plane)
float segments; // number of segments in arc or blend
int32_t segment_count; // count of running segments
float segment_theta; // angular motion per segment
float segment_linear_travel; // linear motion per segment
float center_0; // center of circle at plane axis 0 (e.g. X for G17)
float center_1; // center of circle at plane axis 1 (e.g. Y for G17)
GCodeState_t gm; // Gcode state struct is passed for each arc segment.
// Usage:
// uint32_t linenum; // line number of the arc feed move - same for each segment
// float target[AXES]; // arc segment target
// float work_offset[AXES]; // offset from machine coord system for reporting (same for each segment)
// float block_time; // segment_time: constant time per aline segment
magic_t magic_end;
} arc_t;
extern arc_t arc;
*/
/* arc function prototypes */
void cm_arc_init(void);
void cm_abort_arc(void);
stat_t cm_arc_callback(void);
void cm_arc_init(cmMachine_t *_cm);
void cm_abort_arc(cmMachine_t *_cm);
stat_t cm_arc_callback(cmMachine_t *_cm);
#endif // End of include guard: PLAN_ARC_H_ONCE

71
g2core/planner.cpp
View File

@@ -74,8 +74,8 @@ mpPlannerRuntime_t *mr; // context for planner block runtime
mpPlannerRuntime_t mr1; // primary planner runtime context
mpPlannerRuntime_t mr2; // secondary planner runtime context
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
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
#define JSON_COMMAND_BUFFER_SIZE 3
@@ -147,7 +147,7 @@ static stat_t _exec_json_wait(mpBuf_t *bf);
*/
// initialize a planner queue
void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size)
void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *queue, uint8_t size)
{
mpBuf_t *pv, *nx;
uint8_t i, nx_i;
@@ -157,10 +157,10 @@ void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size)
q->magic_start = MAGICNUM;
q->magic_end = MAGICNUM;
memset(pool, 0, sizeof(mpBuf_t)*size); // clear all buffers in pool
q->bf = pool; // link the buffer pool first
q->w = pool; // init all buffer pointers
q->r = pool;
memset(queue, 0, sizeof(mpBuf_t)*size); // clear all buffers in queue
q->bf = queue; // link the buffer pool first
q->w = queue; // init all buffer pointers
q->r = queue;
q->queue_size = size;
q->buffers_available = size;
@@ -173,10 +173,10 @@ void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *pool, uint8_t size)
q->bf[i].pv = pv;
pv = &q->bf[i];
}
q->bf[size-1].nx = pool;
q->bf[size-1].nx = queue;
}
void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uint8_t _queue_size)
void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size)
{
mp = &mp1; // set global pointer to the primary planner
mr = &mr1; // and primary runtime
@@ -188,8 +188,8 @@ void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uin
_mp->mfo_factor = 1.00;
// init planner queues
_mp->q.bf = _pool; // assign puffer pool to queue manager structure
_init_planner_queue(_mp, _pool, _queue_size);
_mp->q.bf = queue; // assign puffer pool to queue manager structure
_init_planner_queue(_mp, queue, queue_size);
// init runtime structs
_mp->mr = _mr;
@@ -207,22 +207,19 @@ void planner_reset(mpPlanner_t *_mp)
planner_init(_mp, _mp->mr, _mp->q.bf, _mp->q.queue_size); // reset parent planner and linked Q and MR
}
stat_t planner_test_assertions(mpPlanner_t *_mp)
stat_t planner_test_assertions(const mpPlanner_t *_mp)
{
if (
(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()"));
return (cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()"));
}
for (uint8_t i=0; i < _mp->q.queue_size; i++) {
if ((_mp->q.bf[i].nx == nullptr) || (_mp->q.bf[i].pv == nullptr)) {
return (cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner buffer is corrupted"));
}
}
// for (uint8_t i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
// if (mb.bf[i].nx == nullptr) {
// _debug_trap("buffer has nullptr for nx");
// }
// if (mb.bf[i].pv == nullptr) {
// _debug_trap("buffer has nullptr for pv");
// }
// }
return (STAT_OK);
}
@@ -245,9 +242,7 @@ void mp_halt_runtime()
*/
void mp_flush_planner(mpPlanner_t *_mp)
{
cm_abort_arc();
// mp_init_planner_buffers(_mp); //+++++
// planner_init(_mp); //+++++
cm_abort_arc(cm);
planner_reset(_mp);
mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion
}
@@ -485,38 +480,19 @@ stat_t mp_exec_out_of_band_dwell(void)
* mp_has_runnable_buffer() - true if next buffer is runnable, indicating motion has not stopped.
* mp_is_it_phat_city_time() - test if there is time for non-essential processes
*/
/*
uint8_t mp_get_planner_buffers(int8_t q) // which queue are you interested in?
{
if (q == ACTIVE_Q) { q = mb.active_q; };
return (mb.q[q].buffers_available);
}
bool mp_planner_is_full(int8_t q) // which queue are you interested in?
{
if (q == ACTIVE_Q) { q = mb.active_q; };
return ((mb.q[q].buffers_available < PLANNER_BUFFER_HEADROOM)
|| (jc.available == 0)); // We also need to ensure we have room for another JSON command
}
bool mp_has_runnable_buffer(int8_t q) // which queue are you interested in?)
{
if (q == ACTIVE_Q) { q = mb.active_q; };
return (mb.q[q].r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true
}
*/
uint8_t mp_get_planner_buffers(mpPlanner_t *_mp) // which planner are you interested in?
uint8_t mp_get_planner_buffers(const mpPlanner_t *_mp) // which planner are you interested in?
{
return (_mp->q.buffers_available);
}
bool mp_planner_is_full(mpPlanner_t *_mp) // which planner are you interested in?
bool mp_planner_is_full(const mpPlanner_t *_mp) // which planner are you interested in?
{
// We also need to ensure we have room for another JSON command
return ((_mp->q.buffers_available < PLANNER_BUFFER_HEADROOM) || (jc.available == 0));
}
bool mp_has_runnable_buffer(mpPlanner_t *_mp) // which planner are you interested in?)
bool mp_has_runnable_buffer(const mpPlanner_t *_mp) // which planner are you interested in?)
{
return (_mp->q.r->buffer_state); // anything other than MP_BUFFER_EMPTY returns true
}
@@ -566,7 +542,8 @@ bool mp_is_phat_city_time()
stat_t mp_planner_callback()
{
// Test if the planner has transitioned to an IDLE state
if ((mp_get_planner_buffers(mp) == PLANNER_BUFFER_POOL_SIZE) && //+++++ // detect and set IDLE state
// if ((mp_get_planner_buffers(mp) == PLANNER_BUFFER_POOL_SIZE) && //+++++ // detect and set IDLE state
if ((mp_get_planner_buffers(mp) == mp->q.queue_size) && //+++++ // detect and set IDLE state
(cm->motion_state == MOTION_STOP) &&
(cm->hold_state == FEEDHOLD_OFF)) {
mp->planner_state = PLANNER_IDLE;

18
g2core/planner.h
View File

@@ -250,8 +250,8 @@ typedef enum {
*/
/*** Most of these factors are the result of a lot of tweaking. Change with caution.***/
#define PLANNER_BUFFER_POOL_SIZE ((uint8_t)48) // Suggest 12 min. Limit is 255
#define SECONDARY_BUFFER_POOL_SIZE ((uint8_t)12) // Secondary planner queue for feedhold operations
#define PLANNER_QUEUE_SIZE ((uint8_t)48) // Suggest 12 min. Limit is 255
#define SECONDARY_QUEUE_SIZE ((uint8_t)12) // Secondary planner queue for feedhold operations
#define PLANNER_BUFFER_HEADROOM ((uint8_t)4) // Buffers to reserve in planner before processing new input line
#define JERK_MULTIPLIER ((float)1000000) // DO NOT CHANGE - must always be 1 million
@@ -536,8 +536,8 @@ 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
extern mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers
extern mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers
/*
* Global Scope Functions
@@ -545,9 +545,9 @@ extern mpBuf_t mp2_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for s
//**** planner.cpp functions
void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *_pool, uint8_t _queue_size);
void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size);
void planner_reset(mpPlanner_t *_mp);
stat_t planner_test_assertions(mpPlanner_t *_mp);
stat_t planner_test_assertions(const mpPlanner_t *_mp);
void mp_halt_runtime(void);
void mp_flush_planner(mpPlanner_t *_mp);
@@ -567,9 +567,9 @@ 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 *_mp);
bool mp_planner_is_full(mpPlanner_t *_mp);
bool mp_has_runnable_buffer(mpPlanner_t *_mp);
uint8_t mp_get_planner_buffers(const mpPlanner_t *_mp);
bool mp_planner_is_full(const mpPlanner_t *_mp);
bool mp_has_runnable_buffer(const mpPlanner_t *_mp);
bool mp_is_phat_city_time(void);
stat_t mp_planner_callback();