Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-28 16:11:44 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Added test for extremely small step increment in _exec_aline_segment(); Left diagnostic trap in for now.

Browse Source
This commit is contained in:
Alden Hart
2017-04-26 06:41:38 -04:00
parent 57873bbee6
commit 86194a410c
4 changed files with 91 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/cycle_feedhold.cpp
+52 -51
View File
@@ -559,6 +559,56 @@ static void _start_p2_feedhold()
}
*/
/*
* _enter_p2() - enter p2 planner with proper state transfer from p1
* _exit_p2() - reenter p1 planner with proper state transfer from p2
*
* Encapsulate entering and exiting p2, as this is tricky and must be done exactly right
*/
static void _enter_p2()
{
// Copy the primary canonical machine to the secondary. Here it's OK to co a memcpy.
// Set parameters in cm, gm and gmx so you can actually use it
memcpy(&cm2, &cm1, sizeof(cmMachine_t));
cm2.hold_state = FEEDHOLD_OFF;
cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF;
cm2.queue_flush_state = QUEUE_FLUSH_OFF;
cm2.gm.feed_rate = 0;
// Set mp planner to p2 and reset it
cm2.mp = &mp2;
planner_reset((mpPlanner_t *)cm2.mp); // mp is a void pointer
// Clear the target and set the positions to the current hold position
memset(&(cm2.return_flags), 0, sizeof(cm2.return_flags));
memset(&(cm2.gm.target), 0, sizeof(cm2.gm.target));
memset(&(cm2.gm.target_comp), 0, sizeof(cm2.gm.target_comp)); // zero Kahan compensation
copy_vector(cm2.gmx.position, mr1.position);
copy_vector(mp2.position, mr1.position);
copy_vector(mr2.position, mr1.position);
// Copy MR position and encoder terms - needed for following error correction state
copy_vector(mr2.target_steps, mr1.target_steps);
copy_vector(mr2.position_steps, mr1.position_steps);
copy_vector(mr2.commanded_steps, mr1.commanded_steps);
copy_vector(mr2.encoder_steps, mr1.encoder_steps); // NB: following error is re-computed in p2
// Reassign the globals to the secondary CM
cm = &cm2;
mp = (mpPlanner_t *)cm->mp; // mp is a void pointer
mr = mp->mr;
}
static void _exit_p2()
{
cm = &cm1; // return to primary planner (p1)
mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer
mr = mp->mr;
}
static void _check_motion_stopped()
{
if (mp_runtime_is_idle()) { // wait for steppers to actually finish
@@ -606,11 +656,11 @@ static stat_t _feedhold_no_actions()
cm1.hold_type = FEEDHOLD_TYPE_HOLD;
// cm1.hold_exit = FEEDHOLD_EXIT_STOP; // default exit for NO_ACTIONS is STOP...
if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold
if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold
_check_motion_stopped();
cm1.hold_state = FEEDHOLD_HOLD;
} else {
cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function
cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function
return (STAT_EAGAIN);
}
}
@@ -632,55 +682,6 @@ static void _feedhold_actions_done_callback(float* vect, bool* flag)
sr_request_status_report(SR_REQUEST_IMMEDIATE);
}
/****************************************************************************************
* _enter_p2() - enter p2 planner with proper state transfer from p1
* _exit_p2() - reenter p1 planner with proper state transfer from p2
*
* Encapsulate entering and exiting p2, as this is tricky and must be done exactly right
*/
static void _enter_p2()
{
// Copy the primary canonical machine to the secondary. Here it's OK to co a memcpy.
// Set parameters in cm, gm and gmx so you can actually use it
memcpy(&cm2, &cm1, sizeof(cmMachine_t));
cm2.hold_state = FEEDHOLD_OFF;
cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF;
cm2.queue_flush_state = QUEUE_FLUSH_OFF;
cm2.gm.feed_rate = 0;
// Set mp planner to p2 and reset it
cm2.mp = &mp2;
planner_reset((mpPlanner_t *)cm2.mp); // mp is a void pointer
// Clear the target and set the positions to the current hold position
memset(&(cm2.gm.target), 0, sizeof(cm2.gm.target));
memset(&(cm2.return_flags), 0, sizeof(cm2.return_flags));
copy_vector(cm2.gm.target_comp, cm1.gm.target_comp); // preserve original Kahan compensation
copy_vector(cm2.gmx.position, mr1.position);
copy_vector(mp2.position, mr1.position);
copy_vector(mr2.position, mr1.position);
// Copy MR position and encoder terms - needed for following error correction state
copy_vector(mr2.target_steps, mr1.target_steps);
copy_vector(mr2.position_steps, mr1.position_steps);
copy_vector(mr2.commanded_steps, mr1.commanded_steps);
copy_vector(mr2.encoder_steps, mr1.encoder_steps); // NB: following error is re-computed in p2
// Reassign the globals to the secondary CM
cm = &cm2;
mp = (mpPlanner_t *)cm->mp; // mp is a void pointer
mr = mp->mr;
}
static void _exit_p2()
{
cm = &cm1; // return to primary planner (p1)
mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer
mr = mp->mr;
}
static stat_t _feedhold_with_actions() // Execute Case (5)
{
// if entered while OFF start a feedhold
g2core/plan_exec.cpp
+18 -2
View File
@@ -892,6 +892,18 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
* -100 -90 -10 encoder is 10 steps behind commanded steps
*/
//+++++ DIAGNOSTIC +++++
#pragma GCC push_options
#pragma GCC optimize ("O0")
// insert function here
static void _hold_everything (int32_t linenum, uint32_t segments)
{
if ((mr2.gm.linenum == linenum) && (mr2.segment_count == segments)) {
cm1.gm.linenum +=1;
}
}
#pragma GCC reset_options
static stat_t _exec_aline_segment()
{
float travel_steps[MOTORS];
@@ -930,9 +942,14 @@ static stat_t _exec_aline_segment()
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...
_hold_everything(159, 2); //+++++
for (uint8_t m=0; m<MOTORS; m++) { // and compute the distances to be traveled
// mr->travel_steps[m] = mr->target_steps[m] - mr->position_steps[m];
travel_steps[m] = mr->target_steps[m] - mr->position_steps[m];
if (fabs(travel_steps[m]) < 0.01) { // truncate very small moves to deal with rounding erors
travel_steps[m] = 0;
}
}
// Update the mb->run_time_remaining -- we know it's missing the current segment's time before it's loaded, that's ok.
@@ -942,7 +959,6 @@ static stat_t _exec_aline_segment()
}
// Call the stepper prep function
// ritorno(st_prep_line(mr->travel_steps, mr->following_error, mr->segment_time));
ritorno(st_prep_line(travel_steps, mr->following_error, mr->segment_time));
copy_vector(mr->position, mr->gm.target); // update position from target
if (mr->segment_count == 0) {
g2core/stepper.cpp
+19
View File
@@ -78,6 +78,25 @@ Motate::SysTickEvent dwell_systick_event {[&] {
}
}, nullptr};
/* Note on the above:
It's a lambda function creating a closure function.
The full implementation that uses it is small and may help:
https://github.com/synthetos/Motate/blob/41e5b92a98de4b268d1804bf6eadf3333298fc75/MotateProject/motate/Atmel_sam_common/SamTimers.h#L1147-L1218
It's just like a function, and is used as a function pointer.
But the closure part means that whatever variables that were in scope where the
[&](parameters){code} is will be captured by the compiler as references in the generated
function and used wherever the function gets called. In this particular use, there isn't
anything that wouldn't be available anywhere in that file, but they're not being called
from that file. They're being called by the systick interrupt which is over in SamTmers.cpp
So this saves a bunch of work exposing bits that the systick would need to call and encapsulates it.
And there's almost no runtime overhead. Just a check for a valid function pointer and then a call of it.
I'd like to get rid of that check but it's more work than its worth.
See here for some good info on lambda functions in C++
http://www.cprogramming.com/c++11/c++11-lambda-closures.html
http://en.cppreference.com/w/cpp/language/lambda
*/
/************************************************************************************
**** CODE **************************************************************************
g2core/util.h
+2 -1
View File
@@ -49,7 +49,8 @@ using Motate::SysTickTimer;
/****** Global Scope Variables and Functions ******/
/*
#pragma GCC push_options // DIAGNOSTIC +++++
// +++++ DIAGNOSTIC +++++
#pragma GCC push_options
#pragma GCC optimize ("O0")
// insert function here
static void _hold_everything (uint32_t n1, uint32_t n2) // example of function