Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-08-18 17:08:38 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Got probing working; moved command values from gm.target to the unit vector; some name changes;

Browse Source
This commit is contained in:
Alden Hart
2017-03-18 08:38:11 -04:00
parent 24d0fdb421
commit 3dc69f4f02
9 changed files with 70 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/canonical_machine.cpp
+1 -3
View File
@@ -1094,7 +1094,6 @@ stat_t cm_resume_origin_offsets()
* cm_straight_traverse() - G0 linear rapid
*/
//stat_t cm_straight_traverse(const float target[], const bool flags[])
stat_t cm_straight_traverse(const float *target, const bool *flags, const uint8_t motion_profile)
{
cm->gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE;
@@ -1247,8 +1246,7 @@ stat_t cm_dwell(const float seconds)
/****************************************************************************************
* cm_straight_feed() - G1
*/
//stat_t cm_straight_feed(const float target[], const bool flags[])
//stat_t cm_straight_feed(const float *target, const bool *flags)
stat_t cm_straight_feed(const float *target, const bool *flags, const uint8_t motion_profile)
{
// trap zero feed rate condition
g2core/canonical_machine.h
+1 -1
View File
@@ -488,7 +488,7 @@ stat_t cm_json_wait(char *json_string); // M102
// Feedhold and related functions (cycle_feedhold.cpp)
void cm_operation_init(void);
stat_t cm_operation_sequencing_callback(void); // operation action runner
stat_t cm_operation_runner_callback(void); // operation action runner
void cm_request_alarm(void);
void cm_request_fasthold(void);
g2core/controller.cpp
+6 -3
View File
@@ -142,7 +142,9 @@ static void _controller_HSM()
// See hardware.h for a list of ISRs and their priorities.
//
//----- kernel level ISR handlers ----(flags are set in ISRs)------------------------//
// Order is important:
// Order is important, and line breaks indicate dependency groups
DISPATCH(hardware_periodic()); // give the hardware a chance to do stuff
DISPATCH(_led_indicator()); // blink LEDs at the current rate
DISPATCH(_shutdown_handler()); // invoke shutdown
@@ -159,15 +161,16 @@ static void _controller_HSM()
DISPATCH(sr_status_report_callback()); // conditionally send status report
DISPATCH(qr_queue_report_callback()); // conditionally send queue report
// DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner +++++
DISPATCH(mp_planner_callback()); // motion planner
DISPATCH(cm_arc_callback(cm)); // arc generation runs as a cycle above lines
DISPATCH(cm_operation_sequencing_callback());// operation action runner
DISPATCH(cm_operation_runner_callback()); // operation action runner
DISPATCH(cm_homing_cycle_callback()); // homing cycle operation (G28.2)
DISPATCH(cm_probing_cycle_callback()); // probing cycle operation (G38.2)
DISPATCH(cm_jogging_cycle_callback()); // jog cycle operation
DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle
DISPATCH(cm_feedhold_command_blocker()); // blocks new Gcode from arriving while in feedhold
//----- command readers and parsers --------------------------------------------------//
g2core/cycle_feedhold.cpp
+7 -4
View File
@@ -234,7 +234,7 @@ void cm_operation_init()
*/
/****************************************************************************************
* cm_operation_sequencing_callback() - run feedhold operations and sequence queued requests
* cm_operation_runner_callback() - run feedhold operations and sequence queued requests
*
* Operations are requested by calling their repective request function, e.g. cm_request_feedhold().
* The operation callback runs the current operation, and sequences requests that must be queued.
@@ -258,7 +258,7 @@ void cm_operation_init()
* handled in the sequencer.
*/
stat_t cm_operation_sequencing_callback()
stat_t cm_operation_runner_callback()
{
if (cm1.job_kill_state == JOB_KILL_REQUESTED) { // job kill must wait for any active hold to complete
_start_job_kill();
@@ -563,8 +563,11 @@ static stat_t _feedhold_with_command()
if (cm1.hold_state < FEEDHOLD_HOLD_POINT_REACHED) {
return (STAT_EAGAIN);
}
cm1.hold_state = FEEDHOLD_HOLD;
st_request_exec_move();
// cm1.hold_state = FEEDHOLD_OFF; // cannot be in HOLD or command won't run (see mp_plan_block_list())
cm1.hold_state = FEEDHOLD_OFF; // cannot be in HOLD or command won't run (see mp_plan_block_list())
mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks
st_request_forward_plan(); // replan from the new bf buffer
// st_request_exec_move();
return (STAT_OK);
}
g2core/cycle_probing.cpp
+8 -8
View File
@@ -55,7 +55,7 @@ struct pbProbingSingleton { // persistent probing runtime variables
int8_t probe_input; // digital input to read
bool trip_sense; // true if contact CLOSURE trips probe (true for G38.2 and G38.3)
bool alarm_flag; // true if failure triggers alarm (true for G38.2 and G38.4)
bool wait_for_motion_end; // flag to know when the motion has ended
bool waiting_for_motion_complete; // true if waiting for a motion to complete
stat_t (*func)(); // binding for callback function state machine
// saved gcode model state
@@ -170,7 +170,7 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al
// queue a function to let us know when we can start probing
cm->probe_state[0] = PROBE_WAITING; // wait until planner queue empties before starting movement
pb.wait_for_motion_end = true;
pb.waiting_for_motion_complete = true;
mp_queue_command(_motion_end_callback, nullptr, nullptr); // note: these args are ignored
return (STAT_OK);
}
@@ -186,12 +186,12 @@ uint8_t cm_straight_probe(float target[], bool flags[], bool trip_sense, bool al
uint8_t cm_probing_cycle_callback(void)
{
if ((cm->cycle_type != CYCLE_PROBE) && (cm->probe_state[0] != PROBE_WAITING)) {
return (STAT_NOOP); // exit if not in a probing cycle
return (STAT_NOOP); // exit if not in a probing cycle
}
if (pb.wait_for_motion_end) { // sync to planner move ends (using callback)
if (pb.waiting_for_motion_complete) { // sync to planner move ends (using callback)
return (STAT_EAGAIN);
}
return (pb.func()); // execute the current probing move
return (pb.func()); // execute the current probing move
}
/***********************************************************************************
@@ -204,16 +204,16 @@ uint8_t cm_probing_cycle_callback(void)
static void _motion_end_callback(float* vect, bool* flag)
{
pb.wait_for_motion_end = false;
pb.waiting_for_motion_complete = false;
}
static stat_t _probe_move(const float target[], const bool flags[])
{
cm_set_absolute_override(MODEL, ABSOLUTE_OVERRIDE_ON);
pb.wait_for_motion_end = true; // set this BEFORE the motion starts
pb.waiting_for_motion_complete = true; // set this BEFORE the motion starts
cm_straight_feed(target, flags, PROFILE_FAST); // NB: feed rate was set earlier, so it's OK
mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway
st_request_forward_plan(); //+++++
// st_request_forward_plan(); //+++++
return (STAT_EAGAIN);
}
g2core/plan_exec.cpp
+35 -17
View File
@@ -184,11 +184,23 @@ static stat_t _plan_aline(mpBuf_t *bf, float entry_velocity)
return (STAT_OK); // report that we planned something...
}
#pragma GCC push_options // DIAGNOSTIC +++++
#pragma GCC optimize ("O0") // DIAGNOSTIC +++++
stat_t mp_forward_plan()
{
mpBuf_t *bf = mp_get_run_buffer();
float entry_velocity;
if (bf->buffer_number == 15) {
if (bf->buffer_state == MP_BUFFER_BACK_PLANNED) {
bf->hint = ZERO_VELOCITY;
}
}
if (bf->buffer_number == 17) {
bf->hint = ZERO_VELOCITY;
}
// Case 0: Examine current running buffer for early exit conditions
if (bf == NULL) { // case 0a: NULL means nothing is running - this is OK
st_prep_null();
@@ -234,6 +246,9 @@ stat_t mp_forward_plan()
return (planned_something ? STAT_OK : STAT_NOOP);
}
// insert function here
#pragma GCC reset_options
/*************************************************************************
* mp_exec_move() - execute runtime functions to prep move for steppers
*
@@ -1032,31 +1047,34 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
if (mp_runtime_is_idle()) { // wait for steppers to actually finish
mp_zero_segment_velocity(); // finalize velocity for reporting purposes
cm->hold_state = FEEDHOLD_MOTION_STOPPED;
cm_set_motion_state(MOTION_STOP);
// Motion has stopped, so we can rely on positions and other values to be stable
// If in a p2 hold, exit the p2 hold immediately set up a flush of the p2 planner queue
if (cm == &cm2) {
cm->hold_state = FEEDHOLD_HOLD;
return (STAT_OK); // will end this exec_aline() with no more movement
}
// At this point we know we are in a p1 hold
// if (cm == &cm2) {
// cm->hold_state = FEEDHOLD_HOLD;
// return (STAT_OK); // will end this exec_aline() with no more movement
// }
// // At this point we know we are in a p1 hold
// If probing or homing, exit the move and advance to the _motion_end_callback()'s.
// Stop the runtime, clear the run buffer and do not transition to p2 planner.
// else if ((cm->cycle_type == CYCLE_HOMING) || (cm->cycle_type == CYCLE_PROBE)) {
else if (cm->hold_type == FEEDHOLD_TYPE_COMMAND) {
mr->block_state = BLOCK_INACTIVE; // disable the rest of the runtime movement
mp_free_run_buffer(); // free buffer and enable finalization move to get loaded
copy_vector(mp->position, mr->position);
// else
// In COMMAND type, discard the remainder of the block and position to the next block
if (cm->hold_type == FEEDHOLD_TYPE_COMMAND) {
float next_entry_velocity = bf->exit_velocity;
mp_free_run_buffer(); // advance to next block, discarding the rest of the move
if (next_entry_velocity > 0) { // only need to replan if new block dis not plan from zero
mp_replan_queue(mp_get_r()); // unplan current forward plan (bf head block), and reset all blocks
st_request_forward_plan(); // replan from the new bf buffer
}
copy_vector(mp->position, mr->position); // update planner position to the final runtime position
// mr->block_state = BLOCK_INACTIVE; // disable the rest of the runtime movement
mr->reset(); // disable the rest of the runtime movement
cm->hold_state = FEEDHOLD_HOLD_POINT_REACHED;
// 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
}
// In a regular p1 hold. Motion has stopped, so we can rely on positions and other values to be stable
else {
// Reset the state of the p1 planner regardless of how hold will ultimately be exited.
else { // Reset the state of the planner regardless of how hold will ultimately be exited.
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
g2core/planner.cpp
+2 -5
View File
@@ -77,9 +77,6 @@ mpPlannerRuntime_t mr2; // secondary planner runtime context
mpBuf_t mp1_queue[PLANNER_QUEUE_SIZE]; // storage allocation for primary planner queue buffers
mpBuf_t mp2_queue[SECONDARY_QUEUE_SIZE]; // storage allocation for secondary planner queue buffers
// Local Scope Data and Functions
#define value_vector gm.target // alias for vector of values
// Execution routines (NB: These are called from the LO interrupt)
static stat_t _exec_dwell(mpBuf_t *bf);
static stat_t _exec_command(mpBuf_t *bf);
@@ -313,7 +310,7 @@ void mp_queue_command(void(*cm_exec)(float *, bool *), float *value, bool *flag)
bf->cm_func = cm_exec; // callback to canonical machine exec function
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
bf->value_vector[axis] = value[axis];
bf->unit[axis] = value[axis]; // use the unit vector to store command values
bf->axis_flags[axis] = flag[axis];
}
mp_commit_write_buffer(BLOCK_TYPE_COMMAND); // must be final operation before exit
@@ -327,7 +324,7 @@ static stat_t _exec_command(mpBuf_t *bf)
stat_t mp_runtime_command(mpBuf_t *bf)
{
bf->cm_func(bf->value_vector, bf->axis_flags); // 2 vectors used by callbacks
bf->cm_func(bf->unit, bf->axis_flags); // 2 vectors used by callbacks
if (mp_free_run_buffer()) {
cm_cycle_end(); // free buffer & perform cycle_end if planner is empty
}
g2core/planner.h
+2
View File
@@ -491,6 +491,8 @@ typedef struct mpPlannerRuntime { // persistent runtime variables
block_state = BLOCK_INACTIVE;
section = SECTION_HEAD;
section_state = SECTION_OFF;
entry_velocity = 0; // needed to ensure next block in forward planning starts from 0 velocity
r->exit_velocity = 0; // ditto
}
} mpPlannerRuntime_t;
g2core/util.h
+8 -3
View File
@@ -25,9 +25,8 @@
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/* util.c/.h contains a dog's breakfast of supporting functions that are
* not specific to g2core: including:
*
/* util.c/.h contains a dog's breakfast of supporting functions that are not specific
* to g2core: including:
* - math and min/max utilities and extensions
* - vector manipulation utilities
* - support for debugging routines
@@ -49,6 +48,12 @@ using Motate::SysTickTimer;
#include <cmath> // isnan, isinf
/****** Global Scope Variables and Functions ******/
/*
#pragma GCC push_options // DIAGNOSTIC +++++
#pragma GCC optimize ("O0") // DIAGNOSTIC +++++
// insert function here
#pragma GCC reset_options // DIAGNOSTIC +++++
*/
//*** vector utilities ***