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

Testing for feedhold type instead if cycle type; corrected bug in gpio edge detection for probe mode; changed feedhold with sync to feedhold with command;

Browse Source
This commit is contained in:
Alden Hart
2017-03-18 05:39:16 -04:00
parent 76924d8e80
commit 4fbaad1c33
7 changed files with 98 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
g2core/canonical_machine.h
View File

@@ -111,7 +111,7 @@ typedef enum {
typedef enum { // feedhold type parameter
FEEDHOLD_TYPE_ACTIONS = 0, // feedhold at max jerk with actions
FEEDHOLD_TYPE_NO_ACTIONS, // feedhold at max jerk with no actions
FEEDHOLD_TYPE_SYNC, // feedhold at max jerk with queue flush and sync command
FEEDHOLD_TYPE_COMMAND, // feedhold at max jerk with queue flush and sync command
FEEDHOLD_TYPE_FAST, // feedhold at high jerk with no actions. Can resume
FEEDHOLD_TYPE_SCRAM // feedhold at high jerk and stop all active devices
} cmFeedholdType;

16
g2core/cycle_feedhold.cpp
View File

@@ -44,9 +44,9 @@ static void _start_queue_flush(void);
static void _start_job_kill(void);
// Feedhold actions
static stat_t _feedhold_with_actions(void);
static stat_t _feedhold_with_command(void);
static stat_t _feedhold_no_actions(void);
static stat_t _feedhold_with_sync(void);
static stat_t _feedhold_with_actions(void);
static stat_t _feedhold_restart_with_actions(void);
static stat_t _feedhold_restart_no_actions(void);
@@ -508,10 +508,9 @@ static void _start_job_kill()
void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit)
{
// look for p2 feedhold (feedhold in a feedhold)
if ((cm1.hold_state == FEEDHOLD_HOLD) &&
(cm2.hold_state == FEEDHOLD_OFF) &&
if ((cm1.hold_state == FEEDHOLD_HOLD) && (cm2.hold_state == FEEDHOLD_OFF) &&
(cm2.machine_state == MACHINE_CYCLE)) {
op.add_action(_feedhold_with_sync);
op.add_action(_feedhold_with_command);
op.add_action(_run_program_stop);
cm2.hold_state = FEEDHOLD_SYNC;
return;
@@ -536,7 +535,7 @@ static void _start_p1_feedhold()
switch (cm1.hold_type) {
case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_with_actions); break; }
case FEEDHOLD_TYPE_NO_ACTIONS: { op.add_action(_feedhold_no_actions); break; }
case FEEDHOLD_TYPE_SYNC: { op.add_action(_feedhold_with_sync); break; }
case FEEDHOLD_TYPE_COMMAND: { op.add_action(_feedhold_with_command); break; }
default: { }
}
switch (cm1.hold_exit) {
@@ -553,12 +552,12 @@ static void _start_p1_feedhold()
// P2 feedholds only allow feedhold sync types
if ((cm2.hold_state == FEEDHOLD_REQUESTED) && (cm2.motion_state == MOTION_RUN)) {
op.add_action(_feedhold_with_sync);
op.add_action(_feedhold_with_command);
cm2.hold_state = FEEDHOLD_SYNC;
}
}
static stat_t _feedhold_with_sync()
static stat_t _feedhold_with_command()
{
if (cm1.hold_state == FEEDHOLD_OFF) { // start a feedhold
cm1.hold_state = FEEDHOLD_SYNC;
@@ -567,6 +566,7 @@ static stat_t _feedhold_with_sync()
return (STAT_EAGAIN);
}
cm1.hold_state = FEEDHOLD_HOLD;
st_request_exec_move();
return (STAT_OK);
}

46
g2core/cycle_probing.cpp
View File

@@ -36,6 +36,7 @@
#include "report.h"
#include "gpio.h"
#include "planner.h"
#include "stepper.h"
#include "util.h"
#include "xio.h"
@@ -193,6 +194,29 @@ uint8_t cm_probing_cycle_callback(void)
return (pb.func()); // execute the current probing move
}
/***********************************************************************************
* _probe_move() - function to execute probing moves
* _motion_end_callback() - callback completes when motion has stopped
*
* target[] must be provided in machine canonical coordinates (absolute, mm)
* cm_set_absolute_override() also zeros work offsets, which are restored on exit.
*/
static void _motion_end_callback(float* vect, bool* flag)
{
pb.wait_for_motion_end = 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
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(); //+++++
return (STAT_EAGAIN);
}
/***********************************************************************************
* _probing_start() - start the probe or skip it if contact is already active
*/
@@ -268,28 +292,6 @@ static stat_t _probing_backoff()
return (STAT_EAGAIN);
}
/***********************************************************************************
* _probe_move() - function to execute probing moves
* _motion_end_callback() - callback completes when motion has stopped
*
* target[] must be provided in machine canonical coordinates (absolute, mm)
* cm_set_absolute_override() also zeros work offsets, which are restored on exit.
*/
static void _motion_end_callback(float* vect, bool* flag)
{
pb.wait_for_motion_end = 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
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
return (STAT_EAGAIN);
}
/***********************************************************************************
* _probe_restore_settings() - helper for both exits
* _probing_exception_exit() - exit for probes that hit an exception

10
g2core/gpio.cpp
View File

@@ -143,7 +143,7 @@ struct ioDigitalInputExt {
if (in->homing_mode) {
if (in->edge == INPUT_EDGE_LEADING) { // we only want the leading edge to fire
en_take_encoder_snapshot();
cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP);
cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP);
}
return;
}
@@ -153,8 +153,10 @@ struct ioDigitalInputExt {
// We want to capture either way.
// Probing tests the start condition for the correct direction ahead of time.
// If we see any edge, it's the right one.
if ((in->edge == INPUT_EDGE_LEADING) || (in->edge == INPUT_EDGE_TRAILING)) {
en_take_encoder_snapshot();
cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP);
cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP);
}
return;
}
@@ -163,10 +165,10 @@ struct ioDigitalInputExt {
// trigger the action on leading edges
if (in->edge == INPUT_EDGE_LEADING) {
if (in->action == INPUT_ACTION_STOP) {
cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP);
cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP);
}
if (in->action == INPUT_ACTION_FAST_STOP) {
cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP);
cm_request_feedhold(FEEDHOLD_TYPE_COMMAND, FEEDHOLD_EXIT_STOP);
}
if (in->action == INPUT_ACTION_HALT) {
cm_halt(); // hard stop, including spindle, coolant and heaters

14
g2core/plan_exec.cpp
View File

@@ -247,7 +247,7 @@ stat_t mp_exec_move()
// It is possible to try to try to exec from a priming planner if coming off a hold
// This occurs if new p1 commands (and were held back) arrived while in a hold
// if (mp->planner_state <= PLANNER_PRIMING) {
// if (mp->planner_state <= MP_BUFFER_BACK_PLANNED) {
// st_prep_null();
// return (STAT_NOOP);
// }
@@ -266,7 +266,7 @@ stat_t mp_exec_move()
}
if (bf->block_type == BLOCK_TYPE_ALINE) { // cycle auto-start for lines only
// if ((bf->block_type == BLOCK_TYPE_ALINE) || (bf->block_type == BLOCK_TYPE_COMMAND)) {
// first-time operations
if (bf->buffer_state != MP_BUFFER_RUNNING) {
if ((bf->buffer_state < MP_BUFFER_BACK_PLANNED) && (cm->motion_state == MOTION_RUN)) {
@@ -1042,11 +1042,15 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf)
// 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_SYNC) {
// 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
cm->hold_state = FEEDHOLD_OFF;
copy_vector(mp->position, mr->position);
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

1
g2core/plan_line.cpp
View File

@@ -262,7 +262,6 @@ void mp_plan_block_list()
mp->p = mp->p->nx;
return;
}
bf = _plan_block(bf); // returns next block to plan
planned_something = true;
mp->p = bf; // DIAGNOSTIC - this is not needed but is set here for debugging purposes

82
g2core/planner.cpp
View File

@@ -77,6 +77,22 @@ 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);
static stat_t _exec_json_wait(mpBuf_t *bf);
// DIAGNOSTICS
//static void _planner_time_accounting();
static void _audit_buffers();
/****************************************************************************************
* JSON planner objects
*/
#define JSON_COMMAND_BUFFER_SIZE 3
struct json_command_buffer_t {
@@ -124,23 +140,9 @@ struct _json_commands_t {
available++;
}
};
_json_commands_t jc;
// Local Scope Data and Functions
#define spindle_speed block_time // local alias for spindle_speed to the time variable
#define value_vector gm.target // alias for vector of values
//static void _planner_time_accounting();
static void _audit_buffers();
// Execution routines (NB: These are called from the LO interrupt)
static void _exec_json_command(float *value, bool *flag);
static stat_t _exec_dwell(mpBuf_t *bf);
static stat_t _exec_command(mpBuf_t *bf);
static stat_t _exec_json_wait(mpBuf_t *bf);
/*
/****************************************************************************************
* planner_init() - initialize MP, MR and planner queue buffers
* planner_reset() - selective reset MP and MR structures
* planner_assert() - test planner assertions, PANIC if violation exists
@@ -222,7 +224,7 @@ stat_t planner_assert(const mpPlanner_t *_mp)
return (STAT_OK);
}
/*
/****************************************************************************************
* mp_halt_runtime() - stop runtime movement immediately
*/
void mp_halt_runtime()
@@ -231,18 +233,18 @@ void mp_halt_runtime()
planner_reset(mp); // reset the active planner
}
/*
/****************************************************************************************
* mp_set_planner_position() - set planner position for a single axis
* mp_set_runtime_position() - set runtime position for a single axis
* mp_set_steps_to_runtime_position() - set encoder counts to the runtime position
*
* Since steps are in motor space you have to run the position vector through inverse
* kinematics to get the right numbers. This means that in a non-Cartesian robot changing
* any position can result in changes to multiple step values. So this operation is provided
* as a single function and always uses the new position vector as an input.
* kinematics to get the right numbers. This means that in a non-Cartesian robot
* changing any position can result in changes to multiple step values. So this operation
* is provided as a single function and always uses the new position vector as an input.
*
* Keeping track of position is complicated by the fact that moves exist in several reference
* frames. The scheme to keep this straight is:
* Keeping track of position is complicated by the fact that moves exist in several
* reference frames. The scheme to keep this straight is:
*
* - mp->position - start and end position for planning
* - mr->position - current position of runtime segment
@@ -251,9 +253,9 @@ void mp_halt_runtime()
* The runtime keeps a lot more data, such as waypoints, step vectors, etc.
* See struct mpMoveRuntimeSingleton for details.
*
* Note that position is set immediately when called and may not be not an accurate representation
* of the tool position. The motors are still processing the action and the real tool position is
* still close to the starting point.
* Note that position is set immediately when called and may not be not an accurate
* representation of the tool position. The motors are still processing the action
* and the real tool position is still close to the starting point.
*/
void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; }
@@ -276,7 +278,7 @@ void mp_set_steps_to_runtime_position()
}
}
/***********************************************************************************
/****************************************************************************************
* mp_queue_command() - queue a synchronous Mcode, program control, or other command
* _exec_command() - callback to execute command
*
@@ -332,11 +334,18 @@ stat_t mp_runtime_command(mpBuf_t *bf)
return (STAT_OK);
}
/***********************************************************************************
/****************************************************************************************
* mp_json_command() - queue a json command
* _exec_json_command() - execute json string
*/
static void _exec_json_command(float *value, bool *flag)
{
char *json_string = jc.read_buffer();
json_parse_for_exec(json_string, true); // process it
jc.free_buffer();
}
stat_t mp_json_command(char *json_string)
{
// Never supposed to fail, since we stopped parsing when we were full
@@ -345,15 +354,7 @@ stat_t mp_json_command(char *json_string)
return (STAT_OK);
}
static void _exec_json_command(float *value, bool *flag)
{
char *json_string = jc.read_buffer();
json_parse_for_exec(json_string, true); // process it
jc.free_buffer();
}
/***********************************************************************************
/****************************************************************************************
* mp_json_wait() - queue a json wait command
* _exec_json_wait() - execute json wait string
*/
@@ -406,8 +407,7 @@ static stat_t _exec_json_wait(mpBuf_t *bf)
return (STAT_OK);
}
/***********************************************************************************
/****************************************************************************************
* mp_dwell() - queue a dwell
* _exec_dwell() - dwell execution
*
@@ -439,7 +439,7 @@ static stat_t _exec_dwell(mpBuf_t *bf)
return (STAT_OK);
}
/***********************************************************************************
/****************************************************************************************
* mp_request_out_of_band_dwell() - request a dwell outside of the planner queue
*
* This command is used to request that a dwell be run outside of the planner.
@@ -457,7 +457,7 @@ void mp_request_out_of_band_dwell(float seconds)
}
}
/***********************************************************************************
/****************************************************************************************
* Planner helpers
*
* mp_get_planner_buffers() - return # of available planner buffers
@@ -490,7 +490,7 @@ bool mp_is_phat_city_time()
return ((mp->plannable_time <= 0.0) || (PHAT_CITY_TIME < mp->plannable_time));
}
/***********************************************************************************
/****************************************************************************************
* mp_planner_callback()
*
* mp_planner_callback()'s job is to invoke backward planning intelligently.