static stat_t _test_arc_soft_limits()
{
// Test if target falls outside boundaries. This is a 3 dimensional test
// so it also checks the linear axis of the arc (helix axis)
ritorno(cm_test_soft_limits(arc.gm.target));
// test arc excursions
ritorno(_test_arc_soft_limit_plane_axis(arc.center_0, arc.plane_axis_0));
ritorno(_test_arc_soft_limit_plane_axis(arc.center_1, arc.plane_axis_1));
return(STAT_OK);
}
static stat_t _homing_axis_search(int8_t axis) // start the search
{
ritorno(_verify_position(axis));
cm.a[axis].jerk_max = cm.a[axis].jerk_homing; // use the homing jerk for search onward
_homing_axis_move(axis, hm.search_travel, hm.search_velocity);
return (_set_homing_func(_homing_axis_latch));
}
static stat_t _json_parser_kernal(char *str)
{
stat_t status;
int8_t depth;
nvObj_t *nv = nv_reset_nv_list(); // get a fresh nvObj list
char group[GROUP_LEN+1] = {""}; // group identifier - starts as NUL
int8_t i = NV_BODY_LEN;
ritorno(_normalize_json_string(str, JSON_OUTPUT_STRING_MAX)); // return if error
// parse the JSON command into the nv body
do {
if (--i == 0) { return (STAT_JSON_TOO_MANY_PAIRS); } // length error
// if ((status = _get_nv_pair_strict(nv, &str, &depth)) > STAT_EAGAIN) { // erred out
if ((status = _get_nv_pair(nv, &str, &depth)) > STAT_EAGAIN) { // erred out
return (status);
}
// propagate the group from previous NV pair (if relevant)
if (group[0] != NUL) {
strncpy(nv->group, group, GROUP_LEN); // copy the parent's group to this child
}
// validate the token and get the index
if ((nv->index = nv_get_index(nv->group, nv->token)) == NO_MATCH) {
nv->valuetype = TYPE_NULL;
return (STAT_UNRECOGNIZED_NAME);
}
if ((nv_index_is_group(nv->index)) && (nv_group_is_prefixed(nv->token))) {
strncpy(group, nv->token, GROUP_LEN); // record the group ID
}
if ((nv = nv->nx) == NULL) return (STAT_JSON_TOO_MANY_PAIRS);// Not supposed to encounter a NULL
} while (status != STAT_OK); // breaks when parsing is complete
// execute the command
nv = nv_body;
if (nv->valuetype == TYPE_NULL){ // means GET the value
ritorno(nv_get(nv)); // ritorno returns w/status on any errors
} else {
cm_parse_clear(*nv->stringp); // parse Gcode and clear alarms if M30 or M2 is found
ritorno(cm_is_alarmed()); // return error status if in alarm, shutdown or panic
ritorno(nv_set(nv)); // set value or call a function (e.g. gcode)
nv_persist(nv);
}
return (STAT_OK); // only successful commands exit through this point
}
static uint8_t _execute_gcode_block()
{
uint8_t status = TG_OK;
cm_set_model_linenum(gn.linenum);
EXEC_FUNC(cm_set_inverse_feed_rate_mode, inverse_feed_rate_mode);
EXEC_FUNC(cm_set_feed_rate, feed_rate);
EXEC_FUNC(cm_set_spindle_speed, spindle_speed);
EXEC_FUNC(cm_select_tool, tool);
EXEC_FUNC(cm_change_tool, tool);
EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off
EXEC_FUNC(cm_mist_coolant_control, mist_coolant);
EXEC_FUNC(cm_flood_coolant_control, flood_coolant); // also disables mist coolant if OFF
EXEC_FUNC(cm_feed_override_enable, feed_override_enable);
if (gn.next_action == NEXT_ACTION_DWELL) { // G4 - dwell
ritorno(cm_dwell(gn.dwell_time)); // return if error, otherwise complete the block
}
EXEC_FUNC(cm_select_plane, select_plane);
EXEC_FUNC(cm_set_units_mode, units_mode);
//--> cutter radius compensation goes here
//--> cutter length compensation goes here
EXEC_FUNC(cm_set_coord_system, coord_system);
EXEC_FUNC(cm_set_path_control, path_control);
EXEC_FUNC(cm_set_distance_mode, distance_mode);
//--> set retract mode goes here
switch (gn.next_action) {
case NEXT_ACTION_GO_HOME: { status = cm_return_to_home(); break;}
case NEXT_ACTION_SEARCH_HOME: { status = cm_homing_cycle(); break;}
case NEXT_ACTION_SET_COORD_DATA: { status = cm_set_coord_offsets(coord_select, gn.target, gf.target); break;}
case NEXT_ACTION_SET_ORIGIN_OFFSETS: { status = cm_set_origin_offsets(gn.target, gf.target); break;}
case NEXT_ACTION_RESET_ORIGIN_OFFSETS: { status = cm_reset_origin_offsets(); break;}
case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: { status = cm_suspend_origin_offsets(); break;}
case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: { status = cm_resume_origin_offsets(); break;}
case NEXT_ACTION_DEFAULT: {
cm_set_absolute_override(gn.absolute_override); // apply override setting to gm struct
switch (gn.motion_mode) {
case MOTION_MODE_CANCEL_MOTION_MODE: { gm.motion_mode = gn.motion_mode; break;}
case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(gn.target, gf.target); break;}
case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(gn.target, gf.target); break;}
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// gf.radius sets radius mode if radius was collected in gn
{ status = cm_arc_feed(gn.target, gf.target, gn.arc_offset[0], gn.arc_offset[1],
gn.arc_offset[2], gn.arc_radius, gn.motion_mode); break;}
}
cm_set_absolute_override(false); // now un-set it (for reporting purposes)
}
}
if (gf.program_flow == true) {
// do the M stops: M0, M1, M2, M30, M60
}
return (status);
}
stat_t _json_parser_kernal(char *str)
{
uint8_t status;
int8_t depth;
cmdObj_t *cmd = cmd_reset_list(); // get a fresh cmdObj list
char group[CMD_GROUP_LEN+1] = {""}; // group identifier - starts as NUL
int8_t i = CMD_BODY_LEN;
ritorno(_normalize_json_string(str, JSON_OUTPUT_STRING_MAX)); // return if error
// parse the JSON command into the cmd body
do {
if (--i == 0) { return (STAT_JSON_TOO_MANY_PAIRS); } // length error
if ((status = _get_nv_pair_strict(cmd, &str, &depth)) > STAT_EAGAIN) { // erred out
return (status);
}
// propagate the group from previous NV pair (if relevant)
if (group[0] != NUL) {
strncpy(cmd->group, group, CMD_GROUP_LEN);// copy the parent's group to this child
}
// validate the token and get the index
if ((cmd->index = cmd_get_index(cmd->group, cmd->token)) == NO_MATCH) {
return (STAT_UNRECOGNIZED_COMMAND);
}
if ((cmd_index_is_group(cmd->index)) && (cmd_group_is_prefixed(cmd->token))) {
strncpy(group, cmd->token, CMD_GROUP_LEN);// record the group ID
}
if ((cmd = cmd->nx) == NULL) return (STAT_JSON_TOO_MANY_PAIRS);// Not supposed to encounter a NULL
} while (status != STAT_OK); // breaks when parsing is complete
// execute the command
cmd = cmd_body;
if (cmd->objtype == TYPE_NULL){ // means GET the value
ritorno(cmd_get(cmd)); // ritorno returns w/status on any errors
} else {
ritorno(cmd_set(cmd)); // set value or call a function (e.g. gcode)
cmd_persist(cmd);
}
return (STAT_OK); // only successful commands exit through this point
}
static stat_t _homing_axis_move(int8_t axis, float target, float velocity)
{
float vect[] = {0,0,0,0,0,0};
float flags[] = {false, false, false, false, false, false};
vect[axis] = target;
flags[axis] = true;
cm_set_feed_rate(velocity);
mp_flush_planner(); // don't use cm_request_queue_flush() here
cm_request_cycle_start();
ritorno(cm_straight_feed(vect, flags));
return (STAT_EAGAIN);
}
stat_t write_persistent_value(nvObj_t *nv)
{
if (cm.cycle_state != CYCLE_OFF)
return(rpt_exception(STAT_FILE_NOT_OPEN)); // can't write when machine is moving
/* not needed
if (nv->valuetype == TYPE_FLOAT) {
if (isnan((double)nv->value)) return(rpt_exception(STAT_FLOAT_IS_NAN)); // bad floating point value
if (isinf((double)nv->value)) return(rpt_exception(STAT_FLOAT_IS_INFINITE));// bad floating point value
}
*/
nvm.tmp_value = nv->value;
ritorno(read_persistent_value(nv));
if ((isnan((double)nv->value)) || (isinf((double)nv->value)) || (fp_NE(nv->value, nvm.tmp_value))) {
memcpy(&nvm.byte_array, &nvm.tmp_value, NVM_VALUE_LEN);
nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN);
(void)EEPROM_WriteBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN);
}
nv->value =nvm.tmp_value; // always restore value
return (STAT_OK);
}
/****************************************************************************
* cmd_text_parser() - update a config setting from a text block (text mode)
* _text_parser() - helper for above
*
* Use cases handled:
* - $xfr=1200 set a parameter
* - $xfr display a parameter
* - $x display a group
* - ? generate a status report (multiline format)
*/
uint8_t cmd_text_parser(char *str)
{
// return (SC_OK); // There is no text parser in this code - just JSON
//}
cmdObj_t *cmd = cmd_reset_list(); // returns first object in the body
uint8_t status = SC_OK;
// single-unit parser processing
ritorno(_text_parser(str, cmd)); // decode the request or return if error
if ((cmd->type == TYPE_PARENT) || (cmd->type == TYPE_NULL)) {
if (cmd_get(cmd) == SC_COMPLETE) { // populate value, group values, or run uber-group displays
return (SC_OK); // return for uber-group displays so they don't print twice
}
} else { // process SET and RUN commands
status = cmd_set(cmd); // set single value
cmd_persist(cmd);
}
cmd_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results
return (status);
return (SC_OK);
}
/******************************************************************************
* text_parser() - update a config setting from a text block (text mode)
* _text_parser_kernal() - helper for above
*
* Use cases handled:
* - $xfr=1200 set a parameter (strict separators))
* - $xfr 1200 set a parameter (relaxed separators)
* - $xfr display a parameter
* - $x display a group
* - ? generate a status report (multiline format)
*/
stat_t text_parser(char_t *str)
{
nvObj_t *nv = nv_reset_nv_list(); // returns first object in the body
stat_t status = STAT_OK;
// trap special displays
if (str[0] == '?') { // handle status report case
sr_run_text_status_report();
return (STAT_OK);
}
if (str[0] == 'H') { // print help screens
help_general((nvObj_t *)NULL);
return (STAT_OK);
}
// pre-process the command
if ((str[0] == '$') && (str[1] == NUL)) { // treat a lone $ as a sys request
strcat(str,"sys");
}
// parse and execute the command (only processes 1 command per line)
ritorno(_text_parser_kernal(str, nv)); // run the parser to decode the command
if ((nv->valuetype == TYPE_NULL) || (nv->valuetype == TYPE_PARENT)) {
if (nv_get(nv) == STAT_COMPLETE){ // populate value, group values, or run uber-group displays
return (STAT_OK); // return for uber-group displays so they don't print twice
}
} else { // process SET and RUN commands
if (cm.machine_state == MACHINE_ALARM)
return (STAT_MACHINE_ALARMED);
status = nv_set(nv); // set (or run) single value
if (status == STAT_OK) {
nv_persist(nv); // conditionally persist depending on flags in array
}
}
nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results
return (status);
}
static stat_t _get_nv_pair(nvObj_t *nv, char **pstr, int8_t *depth)
{
uint8_t i;
char *tmp;
char leaders[] = {"{,\""}; // open curly, quote and leading comma
char separators[] = {":\""}; // colon and quote
char terminators[] = {"},\""}; // close curly, comma and quote
char value[] = {"{\".-+"}; // open curly, quote, period, minus and plus
nv_reset_nv(nv); // wipes the object and sets the depth
// --- Process name part ---
// Find, terminate and set pointers for the name. Allow for leading and trailing name quotes.
char * name = *pstr;
for (i=0; true; i++, (*pstr)++) {
if (strchr(leaders, (int)**pstr) == NULL) { // find leading character of name
name = (*pstr)++;
break;
}
if (i == MAX_PAD_CHARS) return (STAT_JSON_SYNTAX_ERROR);
}
// Find the end of name, NUL terminate and copy token
for (i=0; true; i++, (*pstr)++) {
if (strchr(separators, (int)**pstr) != NULL) {
*(*pstr)++ = NUL;
strncpy(nv->token, name, TOKEN_LEN+1); // copy the string to the token
break;
}
if (i == MAX_NAME_CHARS) return (STAT_JSON_SYNTAX_ERROR);
}
// --- Process value part --- (organized from most to least frequently encountered)
// Find the start of the value part
for (i=0; true; i++, (*pstr)++) {
if (isalnum((int)**pstr)) break;
if (strchr(value, (int)**pstr) != NULL) break;
if (i == MAX_PAD_CHARS) return (STAT_JSON_SYNTAX_ERROR);
}
// nulls (gets)
if ((**pstr == 'n') || ((**pstr == '\"') && (*(*pstr+1) == '\"'))) { // process null value
nv->valuetype = TYPE_NULL;
nv->value = TYPE_NULL;
// numbers
} else if (isdigit(**pstr) || (**pstr == '-')) {// value is a number
nv->value = (float)strtod(*pstr, &tmp); // tmp is the end pointer
if(tmp == *pstr) { return (STAT_BAD_NUMBER_FORMAT);}
nv->valuetype = TYPE_FLOAT;
// object parent
} else if (**pstr == '{') {
nv->valuetype = TYPE_PARENT;
// *depth += 1; // nv_reset_nv() sets the next object's level so this is redundant
(*pstr)++;
return(STAT_EAGAIN); // signal that there is more to parse
// strings
} else if (**pstr == '\"') { // value is a string
(*pstr)++;
nv->valuetype = TYPE_STRING;
if ((tmp = strchr(*pstr, '\"')) == NULL) { return (STAT_JSON_SYNTAX_ERROR);} // find the end of the string
*tmp = NUL;
// if string begins with 0x it might be data, needs to be at least 3 chars long
if( strlen(*pstr)>=3 && (*pstr)[0]=='0' && (*pstr)[1]=='x')
{
uint32_t *v = (uint32_t*)&nv->value;
*v = strtoul((const char *)*pstr, 0L, 0);
nv->valuetype = TYPE_DATA;
} else {
ritorno(nv_copy_string(nv, *pstr));
}
*pstr = ++tmp;
// boolean true/false
} else if (**pstr == 't') {
nv->valuetype = TYPE_BOOL;
nv->value = true;
} else if (**pstr == 'f') {
nv->valuetype = TYPE_BOOL;
nv->value = false;
// arrays
} else if (**pstr == '[') {
nv->valuetype = TYPE_ARRAY;
ritorno(nv_copy_string(nv, *pstr)); // copy array into string for error displays
return (STAT_INPUT_VALUE_UNSUPPORTED); // return error as the parser doesn't do input arrays yet
// general error condition
} else { return (STAT_JSON_SYNTAX_ERROR); } // ill-formed JSON
// process comma separators and end curlies
if ((*pstr = strpbrk(*pstr, terminators)) == NULL) { // advance to terminator or err out
return (STAT_JSON_SYNTAX_ERROR);
}
if (**pstr == '}') {
*depth -= 1; // pop up a nesting level
(*pstr)++; // advance to comma or whatever follows
}
if (**pstr == ',') { return (STAT_EAGAIN);} // signal that there is more to parse
(*pstr)++;
return (STAT_OK); // signal that parsing is complete
}
/*
* _parse_gcode_block() - parses one line of NULL terminated G-Code.
*
* All the parser does is load the state values in gn (next model state) and set flags
* in gf (model state flags). The execute routine applies them. The buffer is assumed to
* contain only uppercase characters and signed floats (no whitespace).
*
* A number of implicit things happen when the gn struct is zeroed:
* - inverse feed rate mode is cancelled - set back to units_per_minute mode
*/
static stat_t _parse_gcode_block(char_t *buf)
{
char *pstr = (char *)buf; // persistent pointer into gcode block for parsing words
char letter; // parsed letter, eg.g. G or X or Y
float value = 0; // value parsed from letter (e.g. 2 for G2)
stat_t status = STAT_OK;
// set initial state for new move
memset(&gp, 0, sizeof(gp)); // clear all parser values
memset(&gf, 0, sizeof(gf)); // clear all next-state flags
memset(&gn, 0, sizeof(gn)); // clear all next-state values
gn.motion_mode = cm_get_model_motion_mode();// get motion mode from previous block
// extract commands and parameters
while((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) {
switch(letter) {
case 'G':
switch((uint8_t)value) {
case 0: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE);
case 1: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED);
case 2: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC);
case 3: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC);
case 4: SET_NON_MODAL (next_action, NEXT_ACTION_DWELL);
case 10: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA);
case 17: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XY);
case 18: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XZ);
case 19: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_YZ);
case 20: SET_MODAL (MODAL_GROUP_G6, units_mode, INCHES);
case 21: SET_MODAL (MODAL_GROUP_G6, units_mode, MILLIMETERS);
case 28: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION);
case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION);
case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SEARCH_HOME);
case 3: SET_NON_MODAL (next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN);
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
}
case 30: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION);
case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION);
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
}
/* case 38:
switch (_point(value)) {
case 2: SET_NON_MODAL (next_action, NEXT_ACTION_STRAIGHT_PROBE);
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
}
*/ case 40: break; // ignore cancel cutter radius compensation
case 49: break; // ignore cancel tool length offset comp.
case 53: SET_NON_MODAL (absolute_override, true);
case 54: SET_MODAL (MODAL_GROUP_G12, coord_system, G54);
case 55: SET_MODAL (MODAL_GROUP_G12, coord_system, G55);
case 56: SET_MODAL (MODAL_GROUP_G12, coord_system, G56);
case 57: SET_MODAL (MODAL_GROUP_G12, coord_system, G57);
case 58: SET_MODAL (MODAL_GROUP_G12, coord_system, G58);
case 59: SET_MODAL (MODAL_GROUP_G12, coord_system, G59);
case 61: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_PATH);
case 1: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_STOP);
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
}
case 64: SET_MODAL (MODAL_GROUP_G13,path_control, PATH_CONTINUOUS);
case 80: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CANCEL_MOTION_MODE);
case 90: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
case 91: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
case 92: {
switch (_point(value)) {
case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS);
case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS);
case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS);
case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS);
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
}
case 93: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, true);
case 94: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, false);
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
case 'M':
switch((uint8_t)value) {
case 0: case 1: case 60:
SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_STOP);
case 2: case 30:
SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_END);
case 3: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CW);
case 4: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW);
case 5: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF);
case 6: SET_NON_MODAL (change_tool, true);
case 7: SET_MODAL (MODAL_GROUP_M8, mist_coolant, true);
case 8: SET_MODAL (MODAL_GROUP_M8, flood_coolant, true);
case 9: SET_MODAL (MODAL_GROUP_M8, flood_coolant, false);
case 48: SET_MODAL (MODAL_GROUP_M9, override_enables, true);
case 49: SET_MODAL (MODAL_GROUP_M9, override_enables, false);
case 50: SET_MODAL (MODAL_GROUP_M9, feed_rate_override_enable, true); // conditionally true
case 51: SET_MODAL (MODAL_GROUP_M9, spindle_override_enable, true); // conditionally true
default: status = STAT_UNRECOGNIZED_COMMAND;
}
break;
case 'T': SET_NON_MODAL (tool, (uint8_t)trunc(value));
case 'F': SET_NON_MODAL (feed_rate, value);
case 'P': SET_NON_MODAL (parameter, value); // used for dwell time, G10 coord select
case 'S': SET_NON_MODAL (spindle_speed, value);
case 'X': SET_NON_MODAL (target[AXIS_X], value);
case 'Y': SET_NON_MODAL (target[AXIS_Y], value);
case 'Z': SET_NON_MODAL (target[AXIS_Z], value);
case 'A': SET_NON_MODAL (target[AXIS_A], value);
case 'B': SET_NON_MODAL (target[AXIS_B], value);
case 'C': SET_NON_MODAL (target[AXIS_C], value);
// case 'U': SET_NON_MODAL (target[AXIS_U], value); // reserved
// case 'V': SET_NON_MODAL (target[AXIS_V], value); // reserved
// case 'W': SET_NON_MODAL (target[AXIS_W], value); // reserved
case 'I': SET_NON_MODAL (arc_offset[0], value);
case 'J': SET_NON_MODAL (arc_offset[1], value);
case 'K': SET_NON_MODAL (arc_offset[2], value);
case 'R': SET_NON_MODAL (arc_radius, value);
case 'N': SET_NON_MODAL (linenum,(uint32_t)value); // line number
case 'L': break; // not used for anything
default: status = STAT_UNRECOGNIZED_COMMAND;
}
if(status != STAT_OK) break;
}
if ((status != STAT_OK) && (status != STAT_COMPLETE)) return (status);
ritorno(_validate_gcode_block());
return (_execute_gcode_block()); // if successful execute the block
}
/*
* cm_arc_feed() - canonical machine entry point for arc
*
* Generates an arc by queuing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear arc_segments.
*/
stat_t cm_arc_feed(float target[], float flags[], // arc endpoints
float i, float j, float k, // raw arc offsets
float radius, // non-zero radius implies radius mode
uint8_t motion_mode) // defined motion mode
{
////////////////////////////////////////////////////
// Set axis plane and trap arc specification errors
// trap missing feed rate
if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// set radius mode flag and do simple test(s)
bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc
if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) { // radius value must be + and > minimum radius
return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE);
}
// setup some flags
bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been specified
bool target_y = fp_NOT_ZERO(flags[AXIS_Y]);
bool target_z = fp_NOT_ZERO(flags[AXIS_Z]);
bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // set true if offset I has been specified
bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J
bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K
// Set the arc plane for the current G17/G18/G19 setting and test arc specification
// Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane.
if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
if (radius_f) {
if (!(target_x || target_y)) { // must have at least one endpoint specified
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
}
} else { // center format arc tests
if (offset_k) { // it's OK to be missing either or both i and j, but error if k is present
return (STAT_ARC_SPECIFICATION_ERROR);
}
}
} else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_Y;
if (radius_f) {
if (!(target_x || target_z))
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
} else {
if (offset_j)
return (STAT_ARC_SPECIFICATION_ERROR);
}
} else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19
arc.plane_axis_0 = AXIS_Y;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_X;
if (radius_f) {
if (!(target_y || target_z))
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
} else {
if (offset_i)
return (STAT_ARC_SPECIFICATION_ERROR);
}
}
// set values in the Gcode model state & copy it (linenum was already captured)
cm_set_model_target(target, flags);
// in radius mode it's an error for start == end
if(radius_f) {
if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) &&
(fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) &&
(fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) {
return (STAT_ARC_ENDPOINT_IS_STARTING_POINT);
}
}
// now get down to the rest of the work setting up the arc for execution
cm.gm.motion_mode = motion_mode;
cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm
memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments
copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model
arc.radius = _to_millimeters(radius); // set arc radius or zero
arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm)
arc.offset[1] = _to_millimeters(j);
arc.offset[2] = _to_millimeters(k);
arc.rotations = floor(fabs(cm.gn.parameter)); // P must be a positive integer - force it if not
// determine if this is a full circle arc. Evaluates true if no target is set
arc.full_circle = (fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]));
// compute arc runtime values
ritorno(_compute_arc());
if (fp_ZERO(arc.length)) {
return (STAT_MINIMUM_LENGTH_MOVE); // trap zero length arcs that _compute_arc can throw
}
/* // test arc soft limits
stat_t status = _test_arc_soft_limits();
if (status != STAT_OK) {
cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
copy_vector(cm.gm.target, cm.gmx.position); // reset model position
return (cm_soft_alarm(status));
}
*/
cm_cycle_start(); // if not already started
arc.run_state = MOVE_RUN; // enable arc to be run from the callback
cm_finalize_move();
return (STAT_OK);
}
static stat_t _homing_axis_zero_backoff(int8_t axis) // backoff to zero position
{
ritorno(_verify_position(axis));
_homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_set_zero));
}
/*
* cm_arc_feed() - canonical machine entry point for arc
*
* Generates an arc by queueing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear segments.
*/
stat_t cm_arc_feed(float target[], float flags[],// arc endpoints
float i, float j, float k, // raw arc offsets
float radius, // non-zero radius implies radius mode
uint8_t motion_mode) // defined motion mode
{
// trap zero feed rate condition
if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// Trap conditions where no arc movement will occur, but the system is still in
// arc motion mode - this is not an error. This can happen when a F word or M
// word is by itself.(The tests below are organized for execution efficiency)
if ( fp_ZERO(i) && fp_ZERO(j) && fp_ZERO(k) && fp_ZERO(radius) ) {
if ( fp_ZERO((flags[AXIS_X] + flags[AXIS_Y] + flags[AXIS_Z] +
flags[AXIS_A] + flags[AXIS_B] + flags[AXIS_C]))) {
return (STAT_OK);
}
}
// set values in the Gcode model state & copy it (linenum was already captured)
cm_set_model_target(target, flags);
cm.gm.motion_mode = motion_mode;
cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm
memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments
// populate the arc control singleton
copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model
arc.radius = _to_millimeters(radius); // set arc radius or zero
arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm)
arc.offset[1] = _to_millimeters(j);
arc.offset[2] = _to_millimeters(k);
// Set the arc plane for the current G17/G18/G19 setting
// Plane axis 0 and 1 are the arc plane, 2 is the linear axis normal to the arc plane
if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
} else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_Y;
} else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19
arc.plane_axis_0 = AXIS_Y;
arc.plane_axis_1 = AXIS_Z;
arc.linear_axis = AXIS_X;
}
// compute arc runtime values and prep for execution by the callback
ritorno(_compute_arc());
// test arc soft limits
stat_t status = _test_arc_soft_limits();
if (status != STAT_OK) {
cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
copy_vector(cm.gm.target, cm.gmx.position); // reset model position
return (cm_soft_alarm(status));
}
cm_cycle_start(); // if not already started
arc.run_state = MOVE_RUN; // enable arc to be run from the callback
cm_finalize_move();
return (STAT_OK);
}
/*
* _get_nv_pair_strict() - get the next name-value pair w/strict JSON rules
*
* Parse the next statement and populate the command object (cmdObj).
*
* Leaves string pointer (str) on the first character following the object.
* Which is the character just past the ',' separator if it's a multi-valued
* object or the terminating NUL if single object or the last in a multi.
*
* Keeps track of tree depth and closing braces as much as it has to.
* If this were to be extended to track multiple parents or more than two
* levels deep it would have to track closing curlies - which it does not.
*
* ASSUMES INPUT STRING HAS FIRST BEEN NORMALIZED BY _normalize_json_string()
*
* If a group prefix is passed in it will be pre-pended to any name parsed
* to form a token string. For example, if "x" is provided as a group and
* "fr" is found in the name string the parser will search for "xfr"in the
* cfgArray.
*/
static stat_t _get_nv_pair_strict(cmdObj_t *cmd, char **pstr, int8_t *depth)
{
char *tmp;
char terminators[] = {"},"};
cmd_reset_obj(cmd); // wipes the object and sets the depth
// --- Process name part ---
// find leading and trailing name quotes and set pointers.
if ((*pstr = strchr(*pstr, '\"')) == NULL) { return (STAT_JSON_SYNTAX_ERROR);}
if ((tmp = strchr(++(*pstr), '\"')) == NULL) { return (STAT_JSON_SYNTAX_ERROR);}
*tmp = NUL;
strncpy(cmd->token, *pstr, CMD_TOKEN_LEN); // copy the string to the token
// --- Process value part --- (organized from most to least encountered)
*pstr = ++tmp;
if ((*pstr = strchr(*pstr, ':')) == NULL) return (STAT_JSON_SYNTAX_ERROR);
(*pstr)++; // advance to start of value field
// nulls (gets)
if ((**pstr == 'n') || ((**pstr == '\"') && (*(*pstr+1) == '\"'))) { // process null value
cmd->objtype = TYPE_NULL;
cmd->value = TYPE_NULL;
// numbers
} else if (isdigit(**pstr) || (**pstr == '-')) {// value is a number
cmd->value = strtod(*pstr, &tmp); // tmp is the end pointer
if(tmp == *pstr) { return (STAT_BAD_NUMBER_FORMAT);}
cmd->objtype = TYPE_FLOAT;
// object parent
} else if (**pstr == '{') {
cmd->objtype = TYPE_PARENT;
// *depth += 1; // cmd_reset_obj() sets the next object's level so this is redundant
(*pstr)++;
return(STAT_EAGAIN); // signal that there is more to parse
// strings
} else if (**pstr == '\"') { // value is a string
(*pstr)++;
cmd->objtype = TYPE_STRING;
if ((tmp = strchr(*pstr, '\"')) == NULL) { return (STAT_JSON_SYNTAX_ERROR);} // find the end of the string
*tmp = NUL;
ritorno(cmd_copy_string(cmd, *pstr));
*pstr = ++tmp;
// boolean true/false
} else if (**pstr == 't') {
cmd->objtype = TYPE_BOOL;
cmd->value = true;
} else if (**pstr == 'f') {
cmd->objtype = TYPE_BOOL;
cmd->value = false;
// arrays
} else if (**pstr == '[') {
cmd->objtype = TYPE_ARRAY;
ritorno(cmd_copy_string(cmd, *pstr)); // copy array into string for error displays
return (STAT_INPUT_VALUE_UNSUPPORTED); // return error as the parser doesn't do input arrays yet
// general error condition
} else { return (STAT_JSON_SYNTAX_ERROR); } // ill-formed JSON
// process comma separators and end curlies
if ((*pstr = strpbrk(*pstr, terminators)) == NULL) { // advance to terminator or err out
return (STAT_JSON_SYNTAX_ERROR);
}
if (**pstr == '}') {
*depth -= 1; // pop up a nesting level
(*pstr)++; // advance to comma or whatever follows
}
if (**pstr == ',') { return (STAT_EAGAIN);} // signal that there is more to parse
(*pstr)++;
return (STAT_OK); // signal that parsing is complete
}
/*
* _compute_arc() - compute arc from I and J (arc center point)
*
* The theta calculation sets up an clockwise or counterclockwise arc from the current
* position to the target position around the center designated by the offset vector.
* All theta-values measured in radians of deviance from the positive y-axis.
*
* | <- theta == 0
* * * *
* * *
* * *
* * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* * /
* C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*
* Parts of this routine were originally sourced from the grbl project.
*/
static stat_t _compute_arc()
{
// A non-zero radius value indicates a radius arc
// Compute IJK offset coordinates. These override any current IJK offsets
if (fp_NOT_ZERO(arc.radius)) ritorno(_compute_arc_offsets_from_radius()); // returns if error
// Calculate the theta (angle) of the current point (see header notes)
// Arc.theta is starting point for theta (theta_start)
arc.theta = _get_theta(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
if(isnan(arc.theta) == true) return(STAT_ARC_SPECIFICATION_ERROR);
// calculate the theta (angle) of the target point
float theta_end = _get_theta(
arc.gm.target[arc.plane_axis_0] - arc.offset[arc.plane_axis_0] - arc.position[arc.plane_axis_0],
arc.gm.target[arc.plane_axis_1] - arc.offset[arc.plane_axis_1] - arc.position[arc.plane_axis_1]);
if(isnan(theta_end) == true) return (STAT_ARC_SPECIFICATION_ERROR);
// ensure that the difference is positive so we have clockwise travel
if (theta_end < arc.theta) { theta_end += 2*M_PI; }
// compute angular travel and invert if gcode wants a counterclockwise arc
// if angular travel is zero interpret it as a full circle
arc.angular_travel = theta_end - arc.theta;
if (fp_ZERO(arc.angular_travel)) {
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) {
arc.angular_travel -= 2*M_PI;
} else {
arc.angular_travel = 2*M_PI;
}
} else {
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) {
arc.angular_travel -= 2*M_PI;
}
}
// Find the radius, calculate travel in the depth axis of the helix,
// and compute the time it should take to perform the move
arc.radius = hypot(arc.offset[arc.plane_axis_0], arc.offset[arc.plane_axis_1]);
arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
// length is the total mm of travel of the helix (or just a planar arc)
arc.length = hypot(arc.angular_travel * arc.radius, fabs(arc.linear_travel));
if (arc.length < cm.arc_segment_len) return (STAT_MINIMUM_LENGTH_MOVE); // arc is too short to draw
arc.time = _get_arc_time(arc.linear_travel, arc.angular_travel, arc.radius);
// Find the minimum number of segments that meets these constraints...
float segments_required_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
float segments_required_for_minimum_distance = arc.length / cm.arc_segment_len;
float segments_required_for_minimum_time = arc.time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
arc.segments = floor(min3(segments_required_for_chordal_accuracy,
segments_required_for_minimum_distance,
segments_required_for_minimum_time));
arc.segments = max(arc.segments, 1); //...but is at least 1 segment
arc.gm.move_time = arc.time / arc.segments; // gcode state struct gets segment_time, not arc time
arc.segment_count = (int32_t)arc.segments;
arc.segment_theta = arc.angular_travel / arc.segments;
arc.segment_linear_travel = arc.linear_travel / arc.segments;
arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target
return (STAT_OK);
}
static stat_t _execute_gcode_block()
{
stat_t status = STAT_OK;
cm_set_model_linenum(gn.linenum);
EXEC_FUNC(cm_set_inverse_feed_rate_mode, inverse_feed_rate_mode);
EXEC_FUNC(cm_set_feed_rate, feed_rate);
EXEC_FUNC(cm_feed_rate_override_factor, feed_rate_override_factor);
EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor);
EXEC_FUNC(cm_set_spindle_speed, spindle_speed);
EXEC_FUNC(cm_spindle_override_factor, spindle_override_factor);
EXEC_FUNC(cm_select_tool, tool);
EXEC_FUNC(cm_change_tool, tool);
EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off
EXEC_FUNC(cm_mist_coolant_control, mist_coolant);
EXEC_FUNC(cm_flood_coolant_control, flood_coolant); // also disables mist coolant if OFF
EXEC_FUNC(cm_feed_rate_override_enable, feed_rate_override_enable);
EXEC_FUNC(cm_traverse_override_enable, traverse_override_enable);
EXEC_FUNC(cm_spindle_override_enable, spindle_override_enable);
EXEC_FUNC(cm_override_enables, override_enables);
if (gn.next_action == NEXT_ACTION_DWELL) { // G4 - dwell
ritorno(cm_dwell(gn.parameter)); // return if error, otherwise complete the block
}
EXEC_FUNC(cm_select_plane, select_plane);
EXEC_FUNC(cm_set_units_mode, units_mode);
//--> cutter radius compensation goes here
//--> cutter length compensation goes here
EXEC_FUNC(cm_set_coord_system, coord_system);
EXEC_FUNC(cm_set_path_control, path_control);
EXEC_FUNC(cm_set_distance_mode, distance_mode);
//--> set retract mode goes here
switch (gn.next_action) {
case NEXT_ACTION_SEARCH_HOME: { status = cm_homing_cycle_start(); break;} // G28.2
// case NEXT_ACTION_STRAIGHT_PROBE: { status = cm_probe_cycle_start(); break;}
case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: { status = cm_set_absolute_origin(gn.target, gf.target); break;} // G28.3
case NEXT_ACTION_SET_G28_POSITION: { status = cm_set_g28_position(); break;} // G28.1
case NEXT_ACTION_GOTO_G28_POSITION: { status = cm_goto_g28_position(gn.target, gf.target); break;} // G28
case NEXT_ACTION_SET_G30_POSITION: { status = cm_set_g30_position(); break;} // G30.1
case NEXT_ACTION_GOTO_G30_POSITION: { status = cm_goto_g30_position(gn.target, gf.target); break;} // G30
case NEXT_ACTION_SET_COORD_DATA: { status = cm_set_coord_offsets(gn.parameter, gn.target, gf.target); break;}
case NEXT_ACTION_SET_ORIGIN_OFFSETS: { status = cm_set_origin_offsets(gn.target, gf.target); break;}
case NEXT_ACTION_RESET_ORIGIN_OFFSETS: { status = cm_reset_origin_offsets(); break;}
case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: { status = cm_suspend_origin_offsets(); break;}
case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: { status = cm_resume_origin_offsets(); break;}
case NEXT_ACTION_DEFAULT: {
cm_set_absolute_override(gn.absolute_override); // apply override setting to gm struct
switch (gn.motion_mode) {
case MOTION_MODE_CANCEL_MOTION_MODE: { gm.motion_mode = gn.motion_mode; break;}
case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(gn.target, gf.target); break;}
case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(gn.target, gf.target); break;}
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// gf.radius sets radius mode if radius was collected in gn
{ status = cm_arc_feed(gn.target, gf.target, gn.arc_offset[0], gn.arc_offset[1],
gn.arc_offset[2], gn.arc_radius, gn.motion_mode); break;}
}
}
}
cm_set_absolute_override(false); // un-set abs overrride (for reporting purposes)
// do the M stops: M0, M1, M2, M30, M60
if (gf.program_flow == true) {
if (gn.program_flow == PROGRAM_STOP) { cm_program_stop(); }
else { cm_program_end(); }
}
return (status);
}
stat_t gc_get_gc(nvObj_t *nv)
{
ritorno(nv_copy_string(nv, cs.saved_buf));
nv->valuetype = TYPE_STRING;
return (STAT_OK);
}
