diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp
index 2371c672..e7225677 100644
--- a/g2core/canonical_machine.cpp
+++ b/g2core/canonical_machine.cpp
@@ -37,9 +37,9 @@
*
* Useful reference for doing C callbacks http://www.newty.de/fpt/fpt.html
*
- * There are 3 temporal contexts for system state:
- * - The gcode model in the canonical machine (the MODEL context, held in gm)
- * - The gcode model used by the planner (PLANNER context, held in bf's and mp)
+ * There are 3 layered contexts for dynamic system state ("gcode model"):
+ * - The gcode model in the canonical machine (the MODEL context, held in cm->gm)
+ * - The gcode model used by the planner (PLANNER context, held in mp & its buffers)
* - The gcode model used during motion for reporting (RUNTIME context, held in mr)
*
* It's a bit more complicated than this. The 'gm' struct contains the core Gcode model
@@ -110,9 +110,10 @@
#include "xio.h" // for serial queue flush
/***********************************************************************************
- **** STRUCTURE ALLOCATIONS ********************************************************
+ **** CM GLOBALS & STRUCTURE ALLOCATIONS *******************************************
***********************************************************************************/
+cmMachineSelect cm_select;
cmMachine_t *cm; // pointer to active canonical machine
cmMachine_t cm1; // canonical machine primary machine
cmMachine_t cm2; // canonical machine secondary machine
@@ -136,11 +137,24 @@ static int8_t _axis(const index_t index);
***********************************************************************************/
/*
+ * canonical_machine_inits() - combined cm inits
* canonical_machine_init() - initialize cm struct
* canonical_machine_reset() - apply startup settings or reset to startup
* canonical_machine_reset_rotation()
*/
+void canonical_machine_inits()
+{
+ planner_init(&mp1, &mr1, mp1_queue, PLANNER_QUEUE_SIZE);
+ planner_init(&mp2, &mr2, mp2_queue, SECONDARY_QUEUE_SIZE);
+ canonical_machine_init(&cm1, &mp1); // primary canonical machine
+ canonical_machine_init(&cm2, &mp2); // secondary canonical machine
+ cm = &cm1; // set global canonical machine pointer to primary machine
+ mp = &mp1; // set global pointer to the primary planner
+ mr = &mr1; // and primary runtime
+ cm_select = CM_PRIMARY;
+}
+
void canonical_machine_init(cmMachine_t *_cm, void *_mp)
{
// Note cm* was assignd in main()
@@ -244,7 +258,7 @@ stat_t canonical_machine_test_assertions(cmMachine_t *_cm)
stat_t cm_switch(nvObj_t *nv)
{
- // Must be in the correct CM and must be fully stopped in a hold
+ // Must be in the primary CM and fully stopped in a hold
if ((cm != &cm1) || (cm->hold_state != FEEDHOLD_HOLD)) {
nv->valuetype = TYPE_NULL;
return (STAT_COMMAND_NOT_ACCEPTED);
@@ -259,36 +273,81 @@ stat_t cm_switch(nvObj_t *nv)
// set parameters in cm, gm and gmx so you can actually use it
cmMachine_t *_cm = &cm2;
_cm->hold_state = FEEDHOLD_OFF;
- _cm->hold_disabled = true;
_cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
_cm->gm.absolute_override = ABSOLUTE_OVERRIDE_OFF;
_cm->gm.feed_rate = 0;
+
// clear the target and set the positions to the current hold position
memset(&(_cm->gm.target), 0, sizeof(_cm->gm.target));
- memset(&(_cm->gm.target_comp), 0, sizeof(_cm->gm.target_comp));
+// memset(&(_cm->gm.target_comp), 0, sizeof(_cm->gm.target_comp));
+ copy_vector(_cm->gm.target_comp, cm->gm.target_comp); // preserve original Kahan compensation
copy_vector(_cm->gmx.position, mr->position);
copy_vector(mp2.position, mr->position);
+ copy_vector(mr2.position, mr->position);
- // reassign the globals to the secondary planner (this must be next-to-last)
+ // reassign the globals to the secondary CM (this must be next-to-last)
cm = &cm2;
mp = (mpPlanner_t *)cm->mp; // mp is a void pointer
mr = mp->mr;
+ cm_select = CM_SECONDARY;
// finally, set motion state and ACTIVE_MODEL. This must be performed after cm is set to cm2
+ cm_set_g30_position();
cm_set_motion_state(MOTION_STOP);
return (STAT_OK);
}
-stat_t cm_return(nvObj_t *nv) // if value == true return with offset corrections
+static void _return_move_callback(float* vect, bool* flag)
{
- // reset the globals to the primary planner
- cm = &cm1;
- mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer
- mr = mp->mr;
+ cm2.waiting_for_motion_end = false;
+}
+
+stat_t cm_return(nvObj_t *nv) // LATER: if value == true return with offset corrections
+{
+ // Must be in the secondary CM and fully stopped
+ if ((cm != &cm2) || (cm->motion_state != MOTION_STOP)) {
+ nv->valuetype = TYPE_NULL;
+ return (STAT_COMMAND_NOT_ACCEPTED);
+ }
+
+ // while still in secondary, perform the G30 move and queue a wait
+ float target[] = { 0,0,0,0,0,0 };
+ bool flags[] = { 0,0,0,0,0,0 };
+ cm_goto_g30_position(target, flags); // initiate a return move
+ cm->waiting_for_motion_end = true; // indicates running the final G30 move in the secondary
+ mp_queue_command(_return_move_callback, nullptr, nullptr);
+ cm_select = CM_SECONDARY_RETURN;
return (STAT_OK);
}
+stat_t cm_return_callback()
+{
+ if (cm_select != CM_SECONDARY_RETURN) { // exit if not in secondary planner
+ return (STAT_NOOP);
+ }
+ if (cm->waiting_for_motion_end) { // sync to planner move ends (using callback)
+ return (STAT_EAGAIN);
+ }
+
+ // return to primary CM
+ cm = &cm1;
+ mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer
+ mr = mp->mr;
+ cm_select = CM_PRIMARY;
+ cm_set_motion_state(MOTION_STOP); // sets active model to primary
+
+ cm_end_hold();
+ return (STAT_OK);
+}
+
+/*
+stat_t cm_return_from_hold_callback()
+{
+
+}
+*/
+
/*************************************
* Internal getters and setters *
* Canonical Machine State functions *
@@ -1828,6 +1887,13 @@ stat_t cm_feedhold_sequencing_callback()
cm_end_hold();
}
}
+/*
+ if (cm->return_hold_requested) {
+ if (cm->queue_flush_state == FLUSH_OFF) { // either no flush or wait until it's done flushing
+ cm_end_hold();
+ }
+ }
+*/
return (STAT_OK);
}
diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h
index b6e1fea7..ce0e9ec2 100644
--- a/g2core/canonical_machine.h
+++ b/g2core/canonical_machine.h
@@ -145,6 +145,13 @@ typedef enum { // queue flush state machine
FLUSH_REQUESTED, // flush has been requested but not started yet
} cmQueueFlushState;
+typedef enum {
+ CM_NOT_INIT = 0, // planners need initialization
+ CM_PRIMARY, // in primary machine/planner
+ CM_SECONDARY, // in secondary machine/planner
+ CM_SECONDARY_RETURN // in return move from secondary planner
+} cmMachineSelect;
+
/*****************************************************************************
* CANONICAL MACHINE STRUCTURES
*/
@@ -248,11 +255,9 @@ typedef struct cmMachine { // struct to manage canonical machin
cmSafetyState safety_interlock_state; // safety interlock state
uint32_t esc_boot_timer; // timer for Electronic Speed Control (Spindle electronics) to boot
-// bool g28_flag; // true = complete a G28 move (transient state)
-// bool g30_flag; // true = complete a G30 move
- bool deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them
- bool hold_disabled; // set true when in secondary planner to disable feedhold requests
+ bool waiting_for_motion_end; // set true during a return-to-primary planner operation (G30 is completing)
bool end_hold_requested; // request restart after feedhold
+ bool deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them
uint8_t limit_requested; // set non-zero to request limit switch processing (value is input number)
uint8_t shutdown_requested; // set non-zero to request shutdown in support of external estop (value is input number)
@@ -345,6 +350,7 @@ 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_inits(void);
void canonical_machine_init(cmMachine_t *_cm, void *_mp);
void canonical_machine_reset_rotation(cmMachine_t *_cm); // NOT in NIST
void canonical_machine_reset(cmMachine_t *_cm);
@@ -430,6 +436,7 @@ void cm_request_end_hold(void);
void cm_request_queue_flush(void);
void cm_request_end_queue_flush(void);
stat_t cm_feedhold_sequencing_callback(void); // process feedhold, cycle start and queue flush requests
+stat_t cm_return_callback(void); // return from secondary planner
bool cm_has_hold(void);
void cm_start_hold(void);
diff --git a/g2core/controller.cpp b/g2core/controller.cpp
index 77314281..897b3976 100644
--- a/g2core/controller.cpp
+++ b/g2core/controller.cpp
@@ -161,6 +161,7 @@ static void _controller_HSM()
DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner
DISPATCH(mp_planner_callback()); // motion planner
+ DISPATCH(cm_return_callback()); // return from secondary planner
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)
diff --git a/g2core/cycle_homing.cpp b/g2core/cycle_homing.cpp
index 4767f099..c9858f79 100644
--- a/g2core/cycle_homing.cpp
+++ b/g2core/cycle_homing.cpp
@@ -180,7 +180,7 @@ stat_t cm_homing_cycle_start(void) {
stat_t cm_homing_cycle_start_no_set(void) {
cm_homing_cycle_start();
- hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle
+ hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle
return (STAT_OK);
}
@@ -188,10 +188,10 @@ stat_t cm_homing_cycle_callback(void) {
if (cm->cycle_state != CYCLE_HOMING) { // exit if not in a homing cycle
return (STAT_NOOP);
}
- if (hm.waiting_for_motion_end) { // sync to planner move ends (using callback)
+ if (hm.waiting_for_motion_end) { // sync to planner move ends (using callback)
return (STAT_EAGAIN);
}
- return (hm.func(hm.axis)); // execute the current homing move
+ return (hm.func(hm.axis)); // execute the current homing move
}
/*
diff --git a/g2core/cycle_probing.cpp b/g2core/cycle_probing.cpp
index 45391149..e44d65ef 100644
--- a/g2core/cycle_probing.cpp
+++ b/g2core/cycle_probing.cpp
@@ -142,7 +142,7 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool failure_is_fatal, b
return (STAT_AXIS_IS_MISSING);
}
- pb.failure_is_fatal = failure_is_fatal;
+ pb.failure_is_fatal = failure_is_fatal;
pb.moving_toward_switch = moving_toward_switch;
copy_vector(pb.target, target); // set probe move endpoint
@@ -158,7 +158,7 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool failure_is_fatal, b
clear_vector(cm->probe_results[0]); // clear the old probe position
// NOTE: relying on probe_result will not detect a probe to 0,0,0.
- cm->probe_state[0] = PROBE_WAITING; // wait until planner queue empties before completing initialization
+ cm->probe_state[0] = PROBE_WAITING; // wait until planner queue empties before completing initialization
pb.waiting_for_motion_end = true;
// queue a function to let us know when we can start probing
@@ -332,7 +332,10 @@ static stat_t _probe_axis_move(const float target[], bool exact_position) {
return (STAT_EAGAIN);
}
-static void _probe_axis_move_callback(float* vect, bool* flag) { pb.waiting_for_motion_end = false; }
+static void _probe_axis_move_callback(float* vect, bool* flag)
+{
+ pb.waiting_for_motion_end = false;
+}
/*
* _probing_finish() - report probe results and clean up
diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj
index c8cc86a8..d57acc1f 100644
--- a/g2core/g2core.cppproj
+++ b/g2core/g2core.cppproj
@@ -73,7 +73,7 @@
SWD
com.atmel.avrdbg.tool.atmelice
- J41800030015
+ J41800036434
Atmel-ICE
True
@@ -100,7 +100,7 @@
True
true
- J41800030015
+ J41800036434
0x284E0A60
10000000
diff --git a/g2core/main.cpp b/g2core/main.cpp
index ccbe5c3d..4ae783f9 100644
--- a/g2core/main.cpp
+++ b/g2core/main.cpp
@@ -108,19 +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_queue, PLANNER_QUEUE_SIZE);
- planner_init(&mp2, &mr2, mp2_queue, SECONDARY_QUEUE_SIZE);
- canonical_machine_init(&cm1, &mp1); // primary canonical machine
- canonical_machine_init(&cm2, &mp2); // secondary canonical machine
+ canonical_machine_inits(); // combined inits for CMs and planner
}
void application_init_startup(void)
{
- cm = &cm1; // set global canonical machine pointer to primary machine
- mp = &mp1; // set global pointer to the primary planner
- mr = &mr1; // and primary runtime
-
// start the application
controller_init(); // should be first startup init (requires xio_init())
config_init(); // apply the config settings from persistence
diff --git a/g2core/plan_arc.cpp b/g2core/plan_arc.cpp
index 99403d63..49389f29 100644
--- a/g2core/plan_arc.cpp
+++ b/g2core/plan_arc.cpp
@@ -24,10 +24,6 @@
#include "planner.h"
#include "util.h"
-// Allocate arc planner singleton structure
-
-//arc_t arc;
-
// Local functions
static stat_t _compute_arc(const bool radius_f);
@@ -69,8 +65,6 @@ void cm_abort_arc(cmMachine_t *_cm)
*
* cm_arc_cycle_callback() is called from the controller main loop. Each time it's called
* it queues as many arc segments (lines) as it can before it blocks, then returns.
- *
- * Parts of this routine were informed by the grbl project.
*/
stat_t cm_arc_callback(cmMachine_t *_cm)
diff --git a/g2core/planner.cpp b/g2core/planner.cpp
index 3054d9af..a26208ad 100644
--- a/g2core/planner.cpp
+++ b/g2core/planner.cpp
@@ -768,7 +768,6 @@ mpBuf_t * mp_get_write_buffer() // get & clear a buffer
void mp_unget_write_buffer() // mark buffer as empty and adjust free buffer count
{
-// mpQueue_t *q = &mb.q[mb.active_q];
mpPlannerQueue_t *q = &(mp->q);
if (q->w->buffer_state != MP_BUFFER_EMPTY) { // safety. Can't unget an empty buffer
@@ -784,7 +783,6 @@ void mp_unget_write_buffer() // mark buffer as empty and adjust free buff
void mp_commit_write_buffer(const blockType block_type)
{
-// mpQueue_t *q = &mb.q[mb.active_q];
mpPlannerQueue_t *q = &(mp->q);
q->w->block_type = block_type;
@@ -815,7 +813,6 @@ void mp_commit_write_buffer(const blockType block_type)
// (2) is the buffer in error - i.e. not yet ready for running?
mpBuf_t * mp_get_run_buffer()
{
-// mpBuf_t *r = mb.q[mb.active_q].r;
mpBuf_t *r = mp->q.r;
if (r->buffer_state == MP_BUFFER_EMPTY) {