diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp
index 0bd4baa6..253ff5e1 100644
--- a/g2core/canonical_machine.cpp
+++ b/g2core/canonical_machine.cpp
@@ -1110,10 +1110,6 @@ stat_t cm_resume_origin_offsets()
stat_t cm_straight_traverse(const float target[], const bool flags[])
{
- if (target[AXIS_Y] == 20) { // +++ DEBUG TRAP
- cm->gm.P_word = 20;
- }
-
cm->gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE;
// it's legal for a G0 to have no axis words but we don't want to process it
diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp
index f202cb93..04238e0c 100644
--- a/g2core/cycle_feedhold.cpp
+++ b/g2core/cycle_feedhold.cpp
@@ -78,7 +78,7 @@ static stat_t _feedhold_alarm_exit(void);
* Feedhold Processing - Performs the following cases (listed in rough sequence order):
* (0) - Feedhold request arrives or cm_start_hold()
*
- * Control transfers to plan_exec.cpp feedhold functions:
+ * Control transfers to plan_exec.cpp feedhold functions:
*
* (1) - Feedhold arrives while we are in the middle executing of a block
* (1a) - The block is currently accelerating - wait for the end of acceleration
@@ -91,24 +91,28 @@ static stat_t _feedhold_alarm_exit(void);
* (1d) - The block is currently in the tail - wait until the end of the block
* (1e) - We have a new block and a new feedhold request that arrived at EXACTLY the same time
* (unlikely, but handled as 1b).
+ *
* (2) - The block has decelerated to some velocity > zero, so needs continuation into next block
+ *
* (3) - The block has decelerated to zero velocity
* (3a) - The end of deceleration is detected inline in mp_exec_aline()
* (3b) - The end of deceleration is signaled and state is transitioned
- * (4) - We have finished all the runtime work now we have to wait for the motors to stop
+ *
+ * (4) - Finished all the runtime work, now wait for the motors to stop
* (4a) - It's a homing or probing feedhold - ditch the remaining buffer & go directly to OFF
* (4b) - It's a p2 feedhold - ditch the remaining buffer & signal we want a p2 queue flush
* (4c) - It's a normal feedhold - signal we want the p2 entry actions to execute
+ *
* (5) - The steppers have stopped. No motion should occur. Allows hold actions to complete
*
- * Control transfers back to cycle_feedhold.cpp feedhold functions:
+ * Control transfers back to cycle_feedhold.cpp feedhold functions:
*
* (6) - Removing the hold state and there is queued motion - see cycle_feedhold.cpp
* (7) - Removing the hold state and there is no queued motion - see cycle_feedhold.cpp
*/
/****************************************************************************************
- * cm_has_hold() - return true if a hold condition exists (or a pending hold request)
+ * cm_has_hold() - return true if a hold condition exists (or a pending hold request)
*/
bool cm_has_hold()
{
diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj
index bb1cf7a0..0014b2ec 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/plan_exec.cpp b/g2core/plan_exec.cpp
index 8a2a59c9..20a1e8da 100644
--- a/g2core/plan_exec.cpp
+++ b/g2core/plan_exec.cpp
@@ -460,9 +460,6 @@ stat_t mp_exec_aline(mpBuf_t *bf)
mr->waypoint[SECTION_BODY][axis] = mr->position[axis] + mr->unit[axis] * (mr->r->head_length + mr->r->body_length);
mr->waypoint[SECTION_TAIL][axis] = mr->position[axis] + mr->unit[axis] * (mr->r->head_length + mr->r->body_length + mr->r->tail_length);
}
-// if (mr->waypoint[SECTION_TAIL][AXIS_X] < 0) { //+++++
-// bf->hint = (blockHint)0;
-// }
}
// Feed Override Processing - We need to handle the following cases (listed in rough sequence order):
@@ -994,14 +991,14 @@ static void _exec_aline_normalize_block(mpBlockRuntimeBuf_t *b)
static stat_t _exec_aline_feedhold(mpBuf_t *bf)
{
- // Case (4) - Wait for the steppers to stop
+ // Case (4) - Completing the feedhold - Wait for the steppers to stop
if (cm->hold_state == FEEDHOLD_MOTORS_STOPPING) {
if (mp_runtime_is_idle()) { // wait for steppers to actually finish
mp_zero_segment_velocity(); // finalize velocity for reporting purposes
// If in a p2 hold, exit the p2 hold set up a flush of the p2 planner queue
if (cm == &cm2) {
-// copy_vector(mp->position, mr->position); // +++++ update planner position from runtime
+// copy_vector(mp->position, mr->position); // +++++ update planner position from runtime
cm->hold_state = FEEDHOLD_P2_EXIT;
}
// At this point we know we are in a p1 hold
@@ -1017,12 +1014,17 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
// If exiting a regular p1 hold set state to FEEDHOLD_ACTIONS_START.
// This enables transition to p2 planner; then Z-lift, spindle, coolant actions
else {
- if (bf->gm.linenum == 10) { // +++ DEBUG TRAP
- bf->override_factor *= 1.01;
- }
-// copy_vector(mp->position, mr->position); // ++++ update planner position from runtime
-// bf->length = get_axis_vector_length(mr->position, mr->target); //+++++
- cm->hold_state = FEEDHOLD_ACTIONS_START;
+ mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move
+ bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer
+
+ bf->length = get_axis_vector_length(mr->position, mr->target); // get remaining length in move
+// copy_vector(mr->position, mr->target); // update position from previous target
+ copy_vector(mp->position, mr->position); // update planner position from runtime
+
+ bf->plannable = true; // needed so black can be adjusted
+ mp_replan_queue(mp_get_r()); // make it replan all the blocks
+
+ cm->hold_state = FEEDHOLD_ACTIONS_START; // executes entirely out of p2 planner
}
sr_request_status_report(SR_REQUEST_IMMEDIATE);
@@ -1034,10 +1036,11 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
// Case (3b) - Decelerated to zero. See also Feedhold Case (3a) in mp_exec_aline()
// Update the run buffer then force a replan of the whole planner queue. Replans from zero velocity
if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) {
- mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move
- bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer
cm->hold_state = FEEDHOLD_MOTORS_STOPPING; // wait for the motors to come to a complete stop
- mp_replan_queue(mp_get_r()); // make it replan all the blocks
+// mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move
+// bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer
+// bf->plannable = true; // needed so black can be adjusted
+// mp_replan_queue(mp_get_r()); // make it replan all the blocks
return (STAT_OK); // exit from mp_exec_aline()
}
diff --git a/g2core/planner.cpp b/g2core/planner.cpp
index e5be1e51..eded8eb2 100644
--- a/g2core/planner.cpp
+++ b/g2core/planner.cpp
@@ -179,22 +179,22 @@ void _init_planner_queue(mpPlanner_t *_mp, mpBuf_t *queue, uint8_t size)
void planner_init(mpPlanner_t *_mp, mpPlannerRuntime_t *_mr, mpBuf_t *queue, uint8_t queue_size)
{
// init planner master structure
- memset(_mp, 0, sizeof(mpPlanner_t));// clear all values, pointers and status
- _mp->magic_start = MAGICNUM; // set boundary condition assertions
+ 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
- _mp->q.bf = queue; // assign puffer pool to queue manager structure
+ _mp->q.bf = queue; // assign puffer pool to queue manager structure
_init_planner_queue(_mp, queue, queue_size);
// init runtime structs
_mp->mr = _mr;
- memset(_mr, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status
- _mr->magic_start = MAGICNUM; // mr assertions
+ memset(_mr, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status
+ _mr->magic_start = MAGICNUM; // mr assertions
_mr->magic_end = MAGICNUM;
- _mr->block[0].nx = &_mr->block[1]; // Handle the two "stub blocks" in the runtime structure
+ _mr->block[0].nx = &_mr->block[1]; // Handle the two "stub blocks" in the runtime structure
_mr->block[1].nx = &_mr->block[0];
_mr->r = &_mr->block[0];
_mr->p = &_mr->block[1];