mirror of
https://github.com/synthetos/g2.git
synced 2026-09-21 10:35:38 +08:00
Fixed Y bump problem in double-weirdness.txt
This commit is contained in:
Alden Hart
@@ -295,6 +295,8 @@ static stat_t _run_p1_hold_entry_actions()
|
||||
{
|
||||
cm->hold_state = FEEDHOLD_ACTIONS_WAIT; // penultimate state before transitioning to HOLD
|
||||
|
||||
debug_trap_if_true(st_runtime_isbusy(), "_run_p1_hold_entry_actions() - runtime is busy");
|
||||
|
||||
// copy the primary canonical machine to the secondary,
|
||||
// fix the planner pointer, and reset the secondary planner
|
||||
memcpy(&cm2, &cm1, sizeof(cmMachine_t));
|
||||
@@ -326,16 +328,15 @@ static stat_t _run_p1_hold_entry_actions()
|
||||
cm_set_motion_state(MOTION_STOP);
|
||||
|
||||
// execute feedhold actions
|
||||
if (fp_NOT_ZERO(cm->feedhold_z_lift)) { // optional Z lift
|
||||
float stored_distance_mode = cm_get_distance_mode(MODEL);
|
||||
if (fp_NOT_ZERO(cm->feedhold_z_lift)) { // optional Z lift
|
||||
cm_set_distance_mode(INCREMENTAL_DISTANCE_MODE);
|
||||
bool flags[] = { 0,0,1,0,0,0 };
|
||||
float target[] = { 0,0, _to_inches(cm->feedhold_z_lift), 0,0,0 }; // convert to inches if in inches mode
|
||||
cm_straight_traverse(target, flags);
|
||||
cm_set_distance_mode(stored_distance_mode);
|
||||
cm_set_distance_mode(cm1.gm.distance_mode); // restore distance mode to p1 setting
|
||||
}
|
||||
spindle_control_sync(SPINDLE_PAUSE); // optional spindle pause
|
||||
coolant_control_sync(COOLANT_PAUSE, COOLANT_BOTH); // optional coolant pause
|
||||
spindle_control_sync(SPINDLE_PAUSE); // optional spindle pause
|
||||
coolant_control_sync(COOLANT_PAUSE, COOLANT_BOTH); // optional coolant pause
|
||||
mp_queue_command(_sync_to_p1_hold_entry_actions_done, nullptr, nullptr);
|
||||
return (STAT_OK);
|
||||
}
|
||||
|
||||
@@ -68,12 +68,12 @@
|
||||
<com_atmel_avrdbg_tool_atmelice>
|
||||
<ToolOptions>
|
||||
<InterfaceProperties>
|
||||
<SwdClock>10000000</SwdClock>
|
||||
<SwdClock>2000000</SwdClock>
|
||||
</InterfaceProperties>
|
||||
<InterfaceName>SWD</InterfaceName>
|
||||
</ToolOptions>
|
||||
<ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
|
||||
<ToolNumber>J41800030015</ToolNumber>
|
||||
<ToolNumber>J41800036434</ToolNumber>
|
||||
<ToolName>Atmel-ICE</ToolName>
|
||||
</com_atmel_avrdbg_tool_atmelice>
|
||||
<UseGdb>True</UseGdb>
|
||||
@@ -100,9 +100,9 @@
|
||||
<HWProgramCounterSampling>True</HWProgramCounterSampling>
|
||||
</PercepioTrace>
|
||||
<preserveEEPROM>true</preserveEEPROM>
|
||||
<avrtoolserialnumber>J41800030015</avrtoolserialnumber>
|
||||
<avrtoolserialnumber>J41800036434</avrtoolserialnumber>
|
||||
<avrdeviceexpectedsignature>0x284E0A60</avrdeviceexpectedsignature>
|
||||
<avrtoolinterfaceclock>10000000</avrtoolinterfaceclock>
|
||||
<avrtoolinterfaceclock>2000000</avrtoolinterfaceclock>
|
||||
<custom>
|
||||
<ToolOptions xmlns="">
|
||||
<InterfaceProperties>
|
||||
|
||||
+42
-30
@@ -400,12 +400,27 @@ stat_t mp_exec_aline(mpBuf_t *bf)
|
||||
// Initialize all new blocks, regardless of normal or feedhold operation
|
||||
if (mr->block_state == BLOCK_INACTIVE) {
|
||||
|
||||
// Zero length moves (and other too-short moves) should have already been removed...
|
||||
// ...so the following code is no longer needed.
|
||||
// ASSERTIONS
|
||||
|
||||
// Zero length moves (and other too-short moves) should have already been removed earlier
|
||||
// But let's still alert the condition should it ever occur
|
||||
if (fp_ZERO(bf->length)) { // ...looks for an actual zero here
|
||||
rpt_exception(STAT_PLANNER_ASSERTION_FAILURE, "mp_exec_aline() zero length move");
|
||||
}
|
||||
// if (fp_ZERO(bf->length)) { // ...looks for an actual zero here
|
||||
// rpt_exception(STAT_PLANNER_ASSERTION_FAILURE, "mp_exec_aline() zero length move");
|
||||
// }
|
||||
debug_trap_if_zero(bf->length, "mp_exec_aline() zero length move");
|
||||
|
||||
// Equalities that must be true for this to work:
|
||||
// entry velocity <= cruise velocity &&
|
||||
// exit velocity <= cruise velocity
|
||||
//
|
||||
// Even if the move is head or tail only, cruise velocity needs to be valid.
|
||||
// This is because a "head" is *always* entry->cruise, and a "tail" is *always* cruise->exit,
|
||||
// even if there are no other sections in the move. (This is a significant time savings.)
|
||||
debug_trap_if_true((mr->entry_velocity > mr->r->cruise_velocity),
|
||||
"mp_exec_aline() mr->entry_velocity > mr->r->cruise_velocity");
|
||||
|
||||
debug_trap_if_true((mr->r->exit_velocity > mr->r->cruise_velocity),
|
||||
"mp_exec_aline() mr->exit_velocity > mr->r->cruise_velocity");
|
||||
|
||||
// Start a new move by setting up the runtime singleton (mr)
|
||||
memcpy(&mr->gm, &(bf->gm), sizeof(GCodeState_t)); // copy in the gcode model state
|
||||
@@ -415,20 +430,14 @@ stat_t mp_exec_aline(mpBuf_t *bf)
|
||||
mr->section = SECTION_HEAD;
|
||||
mr->section_state = SECTION_NEW;
|
||||
|
||||
// This is the only place in the system where mr->r and mr->p are allowed to be changed
|
||||
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
||||
// !!! THIS IS THE ONLY PLACE WHERE mr->r AND mr->p ARE ALLOWED TO BE CHANGED !!!
|
||||
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
||||
// Swap P and R blocks
|
||||
mr->r = mr->p; // we are now going to run the planning block
|
||||
mr->p = mr->p->nx; // re-use the old running block as the new planning block
|
||||
|
||||
// Equalities that must be true for this to work:
|
||||
// entry velocity <= cruise velocity &&
|
||||
// cruise velocity >= exit velocity
|
||||
//
|
||||
// Even if the move is head or tail only, cruise velocity needs to be valid.
|
||||
// This is because a "head" is *always* entry->cruise, and a "tail" is *always* cruise->exit,
|
||||
// even if there are not other sections in the move. (This is a significant time savings.)
|
||||
|
||||
// Here we will check to make sure that the sections are longer than MIN_SEGMENT_TIME
|
||||
|
||||
// Check to make sure no sections are less than MIN_SEGMENT_TIME & adjust if necessary
|
||||
if ((!fp_ZERO(mr->r->head_length)) && (mr->r->head_time < MIN_SEGMENT_TIME)) {
|
||||
// head_time !== body_time
|
||||
// We have to compute the new body time addition.
|
||||
@@ -529,6 +538,11 @@ stat_t mp_exec_aline(mpBuf_t *bf)
|
||||
// Case (6) - Wait for the steppers to stop
|
||||
if (cm->hold_state == FEEDHOLD_STOPPING) {
|
||||
if (mp_runtime_is_idle()) { // wait for steppers to actually finish
|
||||
// finalize position and velocity
|
||||
copy_vector(mr->position, mr->gm.target); // update position from target
|
||||
bf->length = get_axis_vector_length(mr->target, mr->position); // reset length in buffer //+++++ TEsT
|
||||
mp_zero_segment_velocity(); // for reporting purposes
|
||||
|
||||
// when homing or probing don't stay in HOLD or execute entry actions
|
||||
if ((cm->cycle_state == CYCLE_HOMING) || (cm->cycle_state == CYCLE_PROBE)) {
|
||||
cm->hold_state = FEEDHOLD_OFF;
|
||||
@@ -537,7 +551,7 @@ stat_t mp_exec_aline(mpBuf_t *bf)
|
||||
} else {
|
||||
cm->hold_state = FEEDHOLD_ACTIONS_START; // perform Z-lift, spindle, coolant actions
|
||||
}
|
||||
mp_zero_segment_velocity(); // for reporting purposes
|
||||
|
||||
sr_request_status_report(SR_REQUEST_IMMEDIATE);
|
||||
cs.controller_state = CONTROLLER_READY; // remove controller readline() PAUSE
|
||||
}
|
||||
@@ -549,7 +563,6 @@ stat_t mp_exec_aline(mpBuf_t *bf)
|
||||
if (cm->hold_state == FEEDHOLD_DECEL_END) {
|
||||
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->target, mr->position);// reset length
|
||||
cm->hold_state = FEEDHOLD_STOPPING;
|
||||
|
||||
// No point bothering with the rest of this move if homing or probing
|
||||
@@ -859,15 +872,15 @@ static void _init_forward_diffs(const float v_0, const float v_1)
|
||||
static stat_t _exec_aline_head(mpBuf_t *bf)
|
||||
{
|
||||
bool first_pass = false;
|
||||
if (mr->section_state == SECTION_NEW) { // INITIALIZATION
|
||||
if (mr->section_state == SECTION_NEW) { // INITIALIZATION
|
||||
first_pass = true;
|
||||
if (fp_ZERO(mr->r->head_length)) {
|
||||
mr->section = SECTION_BODY;
|
||||
return(_exec_aline_body(bf)); // skip ahead to the body generator
|
||||
return(_exec_aline_body(bf)); // skip ahead to the body generator
|
||||
}
|
||||
mr->segments = ceil(uSec(mr->r->head_time) / NOM_SEGMENT_USEC);// # of segments for the section
|
||||
mr->segment_count = (uint32_t)mr->segments;
|
||||
mr->segment_time = mr->r->head_time / mr->segments; // time to advance for each segment
|
||||
mr->segment_time = mr->r->head_time / mr->segments; // time to advance for each segment
|
||||
|
||||
if (mr->segment_count == 1) {
|
||||
// We will only have one segment, simply average the velocities
|
||||
@@ -1016,8 +1029,8 @@ static stat_t _exec_aline_segment()
|
||||
copy_vector(mr->gm.target, mr->waypoint[mr->section]);
|
||||
} else {
|
||||
float segment_length = mr->segment_velocity * mr->segment_time;
|
||||
// see https://en.wikipedia.org/wiki/Kahan_summation_algorithm
|
||||
// for the summation compensation description
|
||||
// See https://en.wikipedia.org/wiki/Kahan_summation_algorithm
|
||||
// for the summation compensation description
|
||||
for (uint8_t a=0; a<AXES; a++) {
|
||||
float to_add = (mr->unit[a] * segment_length) - mr->gm.target_comp[a];
|
||||
float target = mr->position[a] + to_add;
|
||||
@@ -1032,16 +1045,15 @@ static stat_t _exec_aline_segment()
|
||||
// Bucket-brigade the old target down the chain before getting the new target from kinematics
|
||||
//
|
||||
// NB: The direct manipulation of steps to compute travel_steps only works for Cartesian kinematics.
|
||||
// Other kinematics may require transforming travel distance as opposed to simply subtracting steps.
|
||||
|
||||
// Other kinematics may require transforming travel distance as opposed to simply subtracting steps.
|
||||
|
||||
for (uint8_t m=0; m<MOTORS; m++) {
|
||||
mr->commanded_steps[m] = mr->position_steps[m]; // previous segment's position, delayed by 1 segment
|
||||
mr->position_steps[m] = mr->target_steps[m]; // previous segment's target becomes position
|
||||
mr->encoder_steps[m] = en_read_encoder(m); // get current encoder position (time aligns to commanded_steps)
|
||||
mr->commanded_steps[m] = mr->position_steps[m]; // previous segment's position, delayed by 1 segment
|
||||
mr->position_steps[m] = mr->target_steps[m]; // previous segment's target becomes position
|
||||
mr->encoder_steps[m] = en_read_encoder(m); // get current encoder position (time aligns to commanded_steps)
|
||||
mr->following_error[m] = mr->encoder_steps[m] - mr->commanded_steps[m];
|
||||
}
|
||||
kn_inverse_kinematics(mr->gm.target, mr->target_steps); // now determine the target steps...
|
||||
kn_inverse_kinematics(mr->gm.target, mr->target_steps); // now determine the target steps...
|
||||
for (uint8_t m=0; m<MOTORS; m++) { // and compute the distances to be traveled
|
||||
travel_steps[m] = mr->target_steps[m] - mr->position_steps[m];
|
||||
}
|
||||
@@ -1054,7 +1066,7 @@ static stat_t _exec_aline_segment()
|
||||
|
||||
// Call the stepper prep function
|
||||
ritorno(st_prep_line(travel_steps, mr->following_error, mr->segment_time));
|
||||
copy_vector(mr->position, mr->gm.target); // update position from target
|
||||
copy_vector(mr->position, mr->gm.target); // update position from target
|
||||
if (mr->segment_count == 0) {
|
||||
return (STAT_OK); // this section has run all its segments
|
||||
}
|
||||
|
||||
+5
-5
@@ -205,28 +205,28 @@ inline T avg(const T a,const T b) {return (a+b)/2; }
|
||||
#pragma GCC optimize ("O0")
|
||||
|
||||
inline void debug_trap(const char *reason) {
|
||||
__NOP();
|
||||
#if IN_DEBUGGER == 1
|
||||
__NOP();
|
||||
__asm__("BKPT");
|
||||
#endif
|
||||
}
|
||||
|
||||
inline void debug_trap_if_zero(float value, const char *reason) {
|
||||
#if IN_DEBUGGER == 1
|
||||
if (fp_ZERO(value)) {
|
||||
__NOP();
|
||||
#if IN_DEBUGGER == 1
|
||||
__asm__("BKPT");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
inline void debug_trap_if_true(bool condition, const char *reason) {
|
||||
#if IN_DEBUGGER == 1
|
||||
if (condition) {
|
||||
__NOP();
|
||||
#if IN_DEBUGGER == 1
|
||||
__asm__("BKPT");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#pragma GCC reset_options
|
||||
|
||||
Reference in New Issue
Block a user