Fixed position error found in regressions test FI00

g2core/cycle_feedhold.cpp

@@ -380,7 +380,6 @@ static stat_t _run_p1_hold_exit_actions()     // LATER: if value == true return
 static void _sync_to_p1_hold_exit_actions_done(float* vect, bool* flag)
 {
     cm1.hold_state = FEEDHOLD_P1_EXIT;          // penultimate state before transitioning to FEEDHOLD_OFF
- //   copy_vector(mp->position, mr->position);    // +++++ Compensate for amount of move already performed
     sr_request_status_report(SR_REQUEST_IMMEDIATE);
 }
 

g2core/plan_exec.cpp

@@ -552,8 +552,6 @@ stat_t mp_exec_aline(mpBuf_t *bf)
                 st_request_forward_plan();
             }
         }
-//+++++        copy_vector(mr->end_position, mr->position);    // record end position
-        copy_vector(mp->position, mr->position);        // record actual end position of the move
     }
     return (status);
 }
@@ -803,7 +801,6 @@ static stat_t _exec_aline_body(mpBuf_t *bf)
             debug_trap("mr->segment_time < MIN_SEGMENT_TIME (body)");
             return (STAT_OK);                               // exit without advancing position, say we're done
         }
-
         mr->section = SECTION_BODY;                         // +++++ Redundant???
         mr->section_state = SECTION_RUNNING;                // uses PERIOD_2 so last segment detection works
     }
@@ -1025,7 +1022,6 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
 
             // If in a p2 hold, exit the p2 hold immediately set up a flush of the p2 planner queue            
             if (cm == &cm2) {
-//              copy_vector(mp->position, mr->position);    // +++++ update planner position from runtime
                 cm->hold_state = FEEDHOLD_P2_EXIT;
                 return (STAT_OK);                           // will end this exec_aline() with no more movement
             }
@@ -1043,16 +1039,12 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
             else {
   
                 // Reset the state of the p1 planner regardless of how hold will ultimately be exited.
-                bf->length = get_axis_vector_length(mr->position, mr->target); // get remaining length in move
-                copy_vector(mp->position, mr->position);    // update planner position from runtime position
-  
+                bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move
                 bf->block_state = BLOCK_INITIAL_ACTION;     // tell _exec to re-use the bf buffer
                 mr->block_state = BLOCK_INACTIVE;           // invalidate mr buffer to reset the new move
                 bf->plannable = true;                       // needed so black can be adjusted
                 mp_replan_queue(mp_get_r());                // unplan current forward plan (bf head block), and reset all blocks
                 st_request_forward_plan();                  // replan the current bf buffer
-//                mp_forward_plan();                          // the one case where you can call this function directly
-                copy_vector(mp->position, mr->target);      // update planner position to the target
                 
                 // Set state to enable transition to p2 and perform entry actions in the p2 planner
                 cm->hold_state = FEEDHOLD_ACTIONS_START;    // executes entirely out of p2 planner
@@ -1068,10 +1060,6 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
     // Update the run buffer then force a replan of the whole planner queue. Replans from zero velocity
     if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) {
         cm->hold_state = FEEDHOLD_MOTORS_STOPPING;          // wait for the motors to come to a complete stop
-//        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()
     }
 

g2core/plan_line.cpp

@@ -104,7 +104,7 @@ float mp_get_runtime_display_position(uint8_t axis) {
     }
 }
 
-/*
+/****************************************************************************************
  * mp_get_runtime_busy() - returns TRUE if motion control busy (i.e. robot is moving)
  * mp_runtime_is_idle()  - returns TRUE is steppers are not actively moving
  *
@@ -233,7 +233,7 @@ stat_t mp_aline(GCodeState_t* _gm)
     return (STAT_OK);
 }
 
-/*
+/****************************************************************************************
  * mp_plan_block_list() - plan all the blocks in the list
  *
  *  This parent function is just a dispatcher that reads forward in the list
@@ -275,8 +275,8 @@ void mp_plan_block_list()
     mp->p = bf;  // update planner pointer
 }
 
-/*
- * _plan_block() - the block chain using pessimistic assumptions
+/****************************************************************************************
+ * _plan_block() - stitch and backplan a new block to the planner queue
  */
 
 static mpBuf_t* _plan_block(mpBuf_t* bf) 
@@ -492,7 +492,7 @@ static void _calculate_override(mpBuf_t* bf)  // execute ramp to adjust cruise v
     // }
 }
 
-/*
+/****************************************************************************************
  * _calculate_jerk() - calculate jerk given the dynamic state
  *
  *  Set the jerk scaling to the lowest axis with a non-zero unit vector.
@@ -542,7 +542,7 @@ static void _calculate_jerk(mpBuf_t* bf)
     bf->q_recip_2_sqrt_j = q / (2 * sqrt_j);
 }
 
-/*
+/****************************************************************************************
  * _calculate_vmaxes() - compute cruise_vmax and absolute_vmax based on velocity constraints
  *
  *  The following feeds and times are compared and the longest (slowest velocity) is returned:
@@ -647,7 +647,7 @@ static void _calculate_vmaxes(mpBuf_t* bf, const float axis_length[], const floa
     bf->block_time    = block_time;               // initial estimate - used for ramp computations
 }
 
-/*
+/****************************************************************************************
  * _calculate_junction_vmax() - Giseburt's Algorithm ;-)
  *
  *  WARNING: This description is out of date and needs to be updated.

g2core/planner.h

@@ -454,7 +454,6 @@ typedef struct mpPlannerRuntime {       // persistent runtime variables
     bool axis_flags[AXES];              // set true for axes participating in the move
     float target[AXES];                 // final target for bf (used to correct rounding errors)
     float position[AXES];               // current move position
-//    float end_position[AXES];           // endpoint position of previous move +++++
     float waypoint[SECTIONS][AXES];     // head/body/tail endpoints for correction
 
     float target_steps[MOTORS];         // current MR target (absolute target as steps)