From: Joseph Coffland Date: Sun, 2 Jul 2017 06:18:32 +0000 (-0700) Subject: Added support for GCode expressions and vars, Added probe and seek commands X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=e002b40f43b2daec8f0bdbe5abc994481751dd67;p=bbctrl-firmware Added support for GCode expressions and vars, Added probe and seek commands --- diff --git a/avr/src/axis.c b/avr/src/axis.c index 50ee734..30b744c 100644 --- a/avr/src/axis.c +++ b/avr/src/axis.c @@ -32,6 +32,7 @@ #include #include +#include int motor_map[AXES] = {-1, -1, -1, -1, -1, -1}; @@ -70,7 +71,7 @@ char axis_get_char(int axis) { int axis_get_id(char axis) { const char *axes = "XYZABCUVW"; - char *ptr = strchr(axes, axis); + char *ptr = strchr(axes, toupper(axis)); return ptr == 0 ? -1 : (ptr - axes); } diff --git a/avr/src/command.c b/avr/src/command.c index 4c4f14f..5e79172 100644 --- a/avr/src/command.c +++ b/avr/src/command.c @@ -33,8 +33,6 @@ #include "report.h" #include "vars.h" #include "estop.h" -#include "homing.h" -#include "probing.h" #include "i2c.h" #include "plan/jog.h" #include "plan/calibrate.h" @@ -82,7 +80,6 @@ static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { case I2C_STEP: mp_request_step(); break; case I2C_FLUSH: mp_request_flush(); break; case I2C_REPORT: report_request_full(); break; - case I2C_HOME: break; // TODO case I2C_REBOOT: _reboot(); break; case I2C_ZERO: if (length == 0) mach_zero_all(); @@ -274,8 +271,6 @@ void command_callback() { else if (!mp_queue_get_room() || mp_is_resuming() || mach_arc_active() || - mach_is_homing() || - mach_is_probing() || calibrate_busy() || mp_jog_busy()) return; // Wait diff --git a/avr/src/command.def b/avr/src/command.def index ec2d85d..b11766f 100644 --- a/avr/src/command.def +++ b/avr/src/command.def @@ -34,4 +34,3 @@ CMD(mreset, 0, 1, "Reset motor") CMD(calibrate, 0, 0, "Calibrate motors") CMD(messages, 0, 0, "Dump all possible status messages") CMD(resume, 0, 0, "Resume processing after a flush") -CMD(home, 0, 0, "Home all axes") diff --git a/avr/src/config.h b/avr/src/config.h index 17a3d2f..62199a5 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -262,3 +262,5 @@ enum { #define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE #define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE +#define GCODE_MAX_OPERATOR_DEPTH 16 +#define GCODE_MAX_VALUE_DEPTH 32 diff --git a/avr/src/drv8711.h b/avr/src/drv8711.h index b077114..8356322 100644 --- a/avr/src/drv8711.h +++ b/avr/src/drv8711.h @@ -169,6 +169,9 @@ typedef enum { } drv8711_state_t; +typedef void (*stall_callback_t)(int driver); + + void drv8711_init(); drv8711_state_t drv8711_get_state(int driver); void drv8711_set_state(int driver, drv8711_state_t state); diff --git a/avr/src/estop.c b/avr/src/estop.c index d0a9613..99ed0f5 100644 --- a/avr/src/estop.c +++ b/avr/src/estop.c @@ -32,7 +32,6 @@ #include "switch.h" #include "report.h" #include "hardware.h" -#include "homing.h" #include "config.h" #include "plan/planner.h" @@ -96,9 +95,6 @@ void estop_trigger(stat_t reason) { // Set machine state mp_state_estop(); - // Set axes not homed - mach_set_not_homed(); - // Save reason _set_reason(reason); diff --git a/avr/src/gcode_expr.c b/avr/src/gcode_expr.c new file mode 100644 index 0000000..083bdb6 --- /dev/null +++ b/avr/src/gcode_expr.c @@ -0,0 +1,297 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "gcode_expr.h" + +#include "gcode_parser.h" +#include "vars.h" + +#include +#include +#include + + +static float _parse_gcode_number(char **p) { + // Avoid parsing G0X10 as a hexadecimal number + if (**p == '0' && toupper(*(*p + 1)) == 'X') { + (*p)++; // pointer points to X + return 0; + } + + // Skip leading zeros so we don't parse as octal + while (**p == '0' && isdigit(*(*p + 1))) p++; + + // Parse number + char *end; + float value = strtod(*p, &end); + if (end == *p) { + parser.error = STAT_BAD_NUMBER_FORMAT; + return 0; + } + *p = end; // Point to next character after the word + + return value; +} + + +static float _parse_gcode_var(char **p) { + (*p)++; // Skip # + + if (isdigit(**p)) { + // TODO numbered parameters + parser.error = STAT_GCODE_NUM_PARAM_UNSUPPORTED; + + } else if (**p == '<') { + (*p)++; + + // Assigning vars is not supported so the '_' global prefix is optional + if (**p == '_') (*p)++; + + char *name = *p; + while (**p && **p != '>') (*p)++; + + if (**p != '>') parser.error = STAT_GCODE_UNTERMINATED_VAR; + else { + *(*p)++ = 0; // Null terminate + return vars_get_number(name); + } + } + + return 0; +} + + +static float _parse_gcode_func(char **p) { + // TODO LinuxCNC supports GCode functions: ATAN, ABS, ACOS, ASIN, COS, EXP, + // FIX, FUP, ROUND, LN, SIN, TAN & EXISTS. + // See http://linuxcnc.org/docs/html/gcode/overview.html#gcode:functions + parser.error = STAT_GCODE_FUNC_UNSUPPORTED; + return 0; +} + + +static int _op_precedence(op_t op) { + switch (op) { + case OP_INVALID: break; + case OP_MINUS: return 6; + case OP_EXP: return 5; + case OP_MUL: case OP_DIV: case OP_MOD: return 4; + case OP_ADD: case OP_SUB: return 3; + case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT: case OP_LE: + return 2; + case OP_AND: case OP_OR: case OP_XOR: return 1; + } + return 0; +} + + +static op_t _parse_gcode_op(char **_p) { + char *p = *_p; + op_t op = OP_INVALID; + + switch (toupper(p[0])) { + case '*': op = p[1] == '*' ? OP_EXP : OP_MUL; break; + case '/': op = OP_DIV; break; + + case 'M': + if (toupper(p[1]) == 'O' && toupper(p[1]) == 'O') op = OP_EXP; + break; + + case '+': op = OP_ADD; break; + case '-': op = OP_SUB; break; + + case 'E': if (toupper(p[1]) == 'Q') op = OP_EQ; break; + case 'N': if (toupper(p[1]) == 'E') op = OP_NE; break; + + case 'G': + if (toupper(p[1]) == 'T') op = OP_GT; + if (toupper(p[1]) == 'E') op = OP_GE; + break; + + case 'L': + if (toupper(p[1]) == 'T') op = OP_LT; + if (toupper(p[1]) == 'E') op = OP_LE; + break; + + case 'A': + if (toupper(p[1]) == 'N' && toupper(p[2]) == 'D') op = OP_AND; + break; + + case 'O': if (toupper(p[1]) == 'R') op = OP_OR; break; + + case 'X': + if (toupper(p[1]) == 'O' && toupper(p[2]) == 'R') op = OP_XOR; + break; + } + + // Advance pointer + switch (op) { + case OP_INVALID: break; + case OP_MINUS: case OP_MUL: case OP_DIV: case OP_ADD: + case OP_SUB: *_p += 1; break; + case OP_EXP: case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT: + case OP_LE: case OP_OR: *_p += 2; break; + case OP_MOD: case OP_AND: case OP_XOR: *_p += 3; break; + } + + return op; +} + + +static float _apply_binary(op_t op, float left, float right) { + switch (op) { + case OP_INVALID: case OP_MINUS: return 0; + + case OP_EXP: return pow(left, right); + + case OP_MUL: return left * right; + case OP_DIV: return left / right; + case OP_MOD: return fmod(left, right); + + case OP_ADD: return left + right; + case OP_SUB: return left - right; + + case OP_EQ: return left == right; + case OP_NE: return left != right; + case OP_GT: return left > right; + case OP_GE: return left >= right; + case OP_LT: return left > right; + case OP_LE: return left <= right; + + case OP_AND: return left && right; + case OP_OR: return left || right; + case OP_XOR: return (bool)left ^ (bool)right; + } + + return 0; +} + + +static void _val_push(float val) { + if (parser.valPtr < GCODE_MAX_VALUE_DEPTH) parser.vals[parser.valPtr++] = val; + else parser.error = STAT_EXPR_VALUE_STACK_OVERFLOW; +} + + +static float _val_pop() { + if (parser.valPtr) return parser.vals[--parser.valPtr]; + parser.error = STAT_EXPR_VALUE_STACK_UNDERFLOW; + return 0; +} + + +static bool _op_empty() {return !parser.opPtr;} + + +static void _op_push(op_t op) { + if (parser.opPtr < GCODE_MAX_OPERATOR_DEPTH) parser.ops[parser.opPtr++] = op; + else parser.error = STAT_EXPR_OP_STACK_OVERFLOW; +} + + +static op_t _op_pop() { + if (parser.opPtr) return parser.ops[--parser.opPtr]; + parser.error = STAT_EXPR_OP_STACK_UNDERFLOW; + return OP_INVALID; +} + + +static op_t _op_top() { + if (parser.opPtr) return parser.ops[parser.opPtr - 1]; + parser.error = STAT_EXPR_OP_STACK_UNDERFLOW; + return OP_INVALID; +} + + +static void _op_apply() { + op_t op = _op_pop(); + + if (op == OP_MINUS) _val_push(-_val_pop()); + + else { + float right = _val_pop(); + float left = _val_pop(); + + _val_push(_apply_binary(op, left, right)); + } +} + + +// Parse expressions with Dijkstra's Shunting Yard Algorithm +float parse_gcode_expression(char **p) { + bool unary = true; // Used to detect unary minus + + while (!parser.error && **p) { + if (_op_empty() && parser.valPtr == 1) break; // We're done + + switch (**p) { + case ' ': case '\n': case '\r': case '\t': (*p)++; break; + case '#': _val_push(_parse_gcode_var(p)); unary = false; break; + case '[': _op_push(OP_INVALID); (*p)++; unary = true; break; + + case ']': + (*p)++; + + while (!parser.error && _op_top() != OP_INVALID) + _op_apply(); + + _op_pop(); // Pop opening bracket + unary = false; + break; + + default: + if (isdigit(**p) || **p == '.') { + _val_push(_parse_gcode_number(p)); + unary = false; + + } else if (isalpha(**p)) { + _val_push(_parse_gcode_func(p)); + unary = false; + + } else { + op_t op = _parse_gcode_op(p); + + if (unary && op == OP_ADD) continue; // Ignore it + if (unary && op == OP_SUB) {_op_push(OP_MINUS); continue;} + + if (op == OP_INVALID) parser.error = STAT_INVALID_OR_MALFORMED_COMMAND; + else { + int precedence = _op_precedence(op); + + while (!parser.error && !_op_empty() && + precedence <= _op_precedence(_op_top())) + _op_apply(); + + _op_push(op); + unary = true; + } + } + } + } + + return _val_pop(); +} diff --git a/avr/src/gcode_expr.h b/avr/src/gcode_expr.h new file mode 100644 index 0000000..c449f52 --- /dev/null +++ b/avr/src/gcode_expr.h @@ -0,0 +1,32 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + + +float parse_gcode_expression(char **p); diff --git a/avr/src/gcode_parser.c b/avr/src/gcode_parser.c index 09589e2..642436b 100644 --- a/avr/src/gcode_parser.c +++ b/avr/src/gcode_parser.c @@ -28,10 +28,9 @@ #include "gcode_parser.h" +#include "gcode_expr.h" #include "machine.h" #include "plan/arc.h" -#include "probing.h" -#include "homing.h" #include "axis.h" #include "util.h" @@ -47,127 +46,43 @@ parser_t parser = {{0}}; #define SET_MODAL(m, parm, val) \ - {parser.gn.parm = val; parser.gf.parm = true; modals[m] += 1; break;} + {parser.gn.parm = val; parser.gf.parm = true; parser.modals[m] += 1; break;} #define SET_NON_MODAL(parm, val) \ {parser.gn.parm = val; parser.gf.parm = true; break;} #define EXEC_FUNC(f, parm) if (parser.gf.parm) f(parser.gn.parm) -static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block +// NOTE Nested comments are not allowed. E.g. (msg (hello)) +static char *_parse_gcode_comment(char *p) { + char *msg = 0; + p++; // Skip leading paren -/* Normalize a block (line) of gcode in place - * - * Normalization functions: - * - convert all letters to upper case - * - remove white space, control and other invalid characters - * - remove (erroneous) leading zeros that might be taken to mean Octal - * - identify and return start of comments and messages - * - signal if a block-delete character (/) was encountered in the first space - * - * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" - * - * Comment and message handling: - * - Comments field start with a '(' char or alternately a semicolon ';' - * - Comments and messages are not normalized - they are left alone - * - The 'MSG' specifier in comment can have mixed case but cannot cannot - * have embedded white spaces - * - Normalization returns true if there was a message to display, false - * otherwise - * - Comments always terminate the block - i.e. leading or embedded comments - * are not supported - * - Valid cases (examples) Notes: - * G0X10 - command only - no comment - * (comment text) - There is no command on this line - * G0X10 (comment text) - * G0X10 (comment text - It's OK to drop the trailing paren - * G0X10 ;comment text - It's OK to drop the trailing paren - * - * - Invalid cases (examples) Notes: - * G0X10 comment text - Comment with no separator - * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored - * (comment) G0X10 - leading comment. G0X10 will be ignored - * G0X10 # comment - invalid separator - * - * Returns: - * - com points to comment string or to 0 if no comment - * - msg points to message string or to 0 if no comment - * - block_delete_flag is set true if block delete encountered, false otherwise - */ - -static void _normalize_gcode_block(char *str, char **com, char **msg, - uint8_t *block_delete_flag) { - char *rd = str; // read pointer - char *wr = str; // write pointer - - // mark block deletes - *block_delete_flag = *rd == '/'; - - // normalize the command block & find the comment (if any) - for (; *wr; rd++) - if (!*rd) *wr = 0; - else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;} - else if (isalnum(*rd) || strchr("-.", *rd)) // all valid characters - *wr++ = toupper(*rd); - - // Perform Octal stripping - remove invalid leading zeros in number strings - rd = str; - while (*rd) { - if (*rd == '.') break; // don't strip past a decimal point - if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) { - wr = rd + 1; - while (*wr) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite - continue; - } + while (isspace(*p)) p++; // skip whitespace - rd++; + // Look for "(MSG" + if (tolower(*(p + 0)) == 'm' && + tolower(*(p + 1)) == 's' && + tolower(*(p + 2)) == 'g') { + p += 3; + while (isspace(*p)) p++; // skip whitespace + if (*p && *p != ')') msg = p; } - // process comments and messages - if (**com) { - rd = *com; - while (isspace(*rd)) rd++; // skip any leading spaces before "msg" + // Find end + while (*p && *p != ')') p++; + *p = 0; // Terminate string - if (tolower(*rd) == 'm' && tolower(*(rd + 1)) == 's' && - tolower(*(rd + 2)) == 'g') - *msg = rd + 3; + // Queue message + if (msg) mach_message(msg); - for (; *rd; rd++) - // 0 terminate on trailing parenthesis, if any - if (*rd == ')') *rd = 0; - } + return p; } -/* Get gcode word consisting of a letter and a value - * - * This function requires the Gcode string to be normalized. - * Normalization must remove any leading zeros or they will be converted to - * octal. G0X... is not interpreted as hexadecimal. This is trapped. - */ -static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { - if (!**pstr) return STAT_COMPLETE; // no more words to process - - // get letter part - if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND; - *letter = **pstr; - (*pstr)++; - - // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 - if (**pstr == '0' && *(*pstr + 1) == 'X') { - *value = 0; - (*pstr)++; - return STAT_OK; // pointer points to X - } - - // get-value general case - char *end; - *value = strtod(*pstr, &end); - // more robust test then checking for value == 0 - if (end == *pstr) return STAT_GCODE_COMMAND_UNSUPPORTED; - *pstr = end; // pointer points to next character after the word - - return STAT_OK; +static stat_t _parse_gcode_value(char **p, float *value) { + *value = parse_gcode_expression(p); + return parser.error; } @@ -195,12 +110,12 @@ static stat_t _validate_gcode_block() { // activity of the group 1 G-code is suspended for that line. The // axis word-using G-codes from group 0 are G10, G28, G30, and G92" - if (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1]) + if (parser.modals[MODAL_GROUP_G0] && parser.modals[MODAL_GROUP_G1]) return STAT_MODAL_GROUP_VIOLATION; -#if 0 // This check fails for arcs which may have offsets but no axis word +#if 0 // TODO This check fails for arcs which may have offsets but no axis word // look for commands that require an axis word to be present - if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1]) + if (parser.modals[MODAL_GROUP_G0] || parser.modals[MODAL_GROUP_G1]) if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING; #endif @@ -235,7 +150,7 @@ static stat_t _validate_gcode_block() { * 16. set path control mode (G61, G61.1, G64) * 17. set distance mode (G90, G91, G90.1, G91.1) * 18. set retract mode (G98, G99) - * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) + * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) // TODO update this * 19b. update system data (G10) * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 @@ -286,18 +201,9 @@ static stat_t _execute_gcode_block() { case NEXT_ACTION_GOTO_G30_POSITION: // G30 status = mach_goto_g30_position(parser.gn.target, parser.gf.target); break; - case NEXT_ACTION_SEARCH_HOME: // G28.2 - mach_homing_cycle_start(); - break; case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 mach_set_absolute_origin(parser.gn.target, parser.gf.target); break; - case NEXT_ACTION_HOMING_NO_SET: // G28.4 - mach_homing_cycle_start_no_set(); - break; - case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 - status = mach_probe(parser.gn.target, parser.gf.target); - break; case NEXT_ACTION_SET_COORD_DATA: mach_set_coord_offsets(parser.gn.parameter, parser.gn.target, parser.gf.target); @@ -336,7 +242,40 @@ static stat_t _execute_gcode_block() { parser.gn.arc_offset, parser.gf.arc_offset, parser.gn.arc_radius, parser.gf.arc_radius, parser.gn.parameter, parser.gf.parameter, - modals[MODAL_GROUP_G1], parser.gn.motion_mode); + parser.modals[MODAL_GROUP_G1], + parser.gn.motion_mode); + break; + case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR: // G38.2 + status = mach_probe(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_STRAIGHT_PROBE_CLOSE: // G38.3 + status = mach_probe(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR: // G38.4 + status = mach_probe(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_STRAIGHT_PROBE_OPEN: // G38.5 + status = mach_probe(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_SEEK_CLOSE_ERR: // G38.6 + status = mach_seek(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_SEEK_CLOSE: // G38.7 + status = mach_seek(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_SEEK_OPEN_ERR: // G38.8 + status = mach_seek(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); + break; + case MOTION_MODE_SEEK_OPEN: // G38.9 + status = mach_seek(parser.gn.target, parser.gf.target, + parser.gn.motion_mode); break; default: break; // Should not get here } @@ -357,218 +296,216 @@ static stat_t _execute_gcode_block() { } -/* Parses one line of 0 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 canceled - set back to units_per_minute mode - */ -static stat_t _parse_gcode_block(char *buf) { - char *pstr = buf; // persistent pointer 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(modals, 0, sizeof(modals)); // clear all parser values - memset(&parser.gf, 0, sizeof(gcode_flags_t)); // clear all next-state flags - memset(&parser.gn, 0, sizeof(gcode_state_t)); // clear all next-state values - - // get motion mode from previous block - parser.gn.motion_mode = mach_get_motion_mode(); - - // extract commands and parameters - while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { - switch (letter) { - case 'G': - switch ((uint8_t)value) { +/// Load the state values in gn (next model state) and set flags in gf (model +/// state flags). +static stat_t _process_gcode_word(char letter, float value) { + switch (letter) { + case 'G': + switch ((uint8_t)value) { + case 0: + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID); + case 1: + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_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, plane, PLANE_XY); + case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ); + case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ); + case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES); + case 21: SET_MODAL(MODAL_GROUP_G6, units, MILLIMETERS); + case 28: + switch (_point(value)) { case 0: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID); + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); case 1: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_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, plane, PLANE_XY); - case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ); - case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ); - case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES); - case 21: SET_MODAL(MODAL_GROUP_G6, units, 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); - case 4: SET_NON_MODAL(next_action, NEXT_ACTION_HOMING_NO_SET); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - 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_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 38: - switch (_point(value)) { - case 2: SET_NON_MODAL(next_action, NEXT_ACTION_STRAIGHT_PROBE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 40: break; // ignore cancel cutter radius compensation - case 49: break; // ignore cancel tool length offset comp. - case 53: SET_NON_MODAL(absolute_mode, 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_mode, PATH_EXACT_PATH); - case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 64: SET_MODAL(MODAL_GROUP_G13,path_mode, 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 90: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); - case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 91: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); - case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - 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_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE); - case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE); - // case 95: - // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE); - // case 96: // Spindle Constant Surface Speed (not currently supported) - case 97: break; // Spindle RPM mode (only mode curently supported) - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); + case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); + default: return STAT_GCODE_COMMAND_UNSUPPORTED; } break; - case 'M': - switch ((uint8_t)value) { + case 30: + switch (_point(value)) { case 0: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP); + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); case 1: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP); - case 60: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_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(tool_change, 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); // Also mist - 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_override_enable, true); - case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); - default: status = STAT_MCODE_COMMAND_UNSUPPORTED; + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); + default: return STAT_GCODE_COMMAND_UNSUPPORTED; } break; - case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value)); - case 'F': SET_NON_MODAL(feed_rate, value); - // used for dwell time, G10 coord select, rotations - case 'P': SET_NON_MODAL(parameter, value); - 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(line, (uint32_t)value); // line number - case 'L': break; // not used for anything - case 0: break; - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + case 38: + switch (_point(value)) { + case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR); + case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_STRAIGHT_PROBE_CLOSE); + case 4: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR); + case 5: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_STRAIGHT_PROBE_OPEN); + case 6: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_SEEK_CLOSE_ERR); + case 7: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_CLOSE); + case 8: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_SEEK_OPEN_ERR); + case 9: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_OPEN); + default: return STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 40: break; // ignore cancel cutter radius compensation + case 49: break; // ignore cancel tool length offset comp. + case 53: SET_NON_MODAL(absolute_mode, 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_mode, PATH_EXACT_PATH); + case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP); + default: return STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 64: SET_MODAL(MODAL_GROUP_G13,path_mode, 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 90: + switch (_point(value)) { + case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); + case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); + default: return STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 91: + switch (_point(value)) { + case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); + case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); + default: return STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + 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: return STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE); + case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE); + // case 95: + // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE); + // case 96: // Spindle Constant Surface Speed (not currently supported) + case 97: break; // Spindle RPM mode (only mode curently supported) + default: return STAT_GCODE_COMMAND_UNSUPPORTED; } + break; - if (status != STAT_OK) break; - } + case 'M': + switch ((uint8_t)value) { + case 0: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP); + case 1: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP); + case 60: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_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(tool_change, 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); // Also mist + 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_override_enable, true); + case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); + default: return STAT_MCODE_COMMAND_UNSUPPORTED; + } + break; - if (status != STAT_OK && status != STAT_COMPLETE) return status; - RITORNO(_validate_gcode_block()); + case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value)); + case 'F': SET_NON_MODAL(feed_rate, value); + // used for dwell time, G10 coord select, rotations + case 'P': SET_NON_MODAL(parameter, value); + 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(line, (uint32_t)value); // line number + case 'L': break; // not used for anything + case 0: break; + default: return STAT_GCODE_COMMAND_UNSUPPORTED; + } - return _execute_gcode_block(); // if successful execute the block + return STAT_OK; } /// Parse a block (line) of gcode /// Top level of gcode parser. Normalizes block and looks for special cases stat_t gc_gcode_parser(char *block) { - char *str = block; // gcode command or 0 string - char none = 0; - char *com = &none; // gcode comment or 0 string - char *msg = &none; // gcode message or 0 string - uint8_t block_delete_flag; - - _normalize_gcode_block(str, &com, &msg, &block_delete_flag); - - // Block delete omits the line if a / char is present in the first space - // For now this is unconditional and will always delete - if (block_delete_flag) return STAT_NOOP; - #ifdef DEBUG printf("GCODE: %s\n", block); #endif - // queue a "(MSG" response - if (*msg) mach_message(msg); // queue the message + // Delete block if it starts with / + if (*block == '/') return STAT_NOOP; + + // Set initial state for new block + // A number of implicit things happen when the gn struct is zeroed: + // - inverse feed rate mode is canceled + // - set back to units_per_minute mode + memset(&parser, 0, sizeof(parser)); // clear all parser values + + // get motion mode from previous block + parser.gn.motion_mode = mach_get_motion_mode(); + + // Parse words + for (char *p = block; *p;) { + switch (*p) { + case ' ': case '\t': case '\r': case '\n': p++; break; // Skip whitespace + case '(': p = _parse_gcode_comment(p); break; + case ';': *p = 0; break; // Comment + + default: { + char letter = toupper(*p++); + float value = 0; + if (!isalpha(letter)) return STAT_INVALID_OR_MALFORMED_COMMAND; + RITORNO(_parse_gcode_value(&p, &value)); + RITORNO(_process_gcode_word(letter, value)); + } + } + } + + RITORNO(_validate_gcode_block()); - return _parse_gcode_block(block); + return _execute_gcode_block(); } diff --git a/avr/src/gcode_parser.h b/avr/src/gcode_parser.h index 5e85936..dc880ef 100644 --- a/avr/src/gcode_parser.h +++ b/avr/src/gcode_parser.h @@ -55,9 +55,29 @@ typedef enum { // Used for detecting gcode errors. See NIST section 3.4 #define MODAL_GROUP_COUNT (MODAL_GROUP_M9 + 1) +typedef enum { + OP_INVALID, + OP_MINUS, + OP_EXP, + OP_MUL, OP_DIV, OP_MOD, + OP_ADD, OP_SUB, + OP_EQ, OP_NE, OP_GT,OP_GE, OP_LT, OP_LE, + OP_AND, OP_OR, OP_XOR, +} op_t; + + typedef struct { gcode_state_t gn; // gcode input values gcode_flags_t gf; // gcode input flags + + uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block + + op_t ops[GCODE_MAX_OPERATOR_DEPTH]; + float vals[GCODE_MAX_VALUE_DEPTH]; + int opPtr; + int valPtr; + + stat_t error; } parser_t; diff --git a/avr/src/gcode_state.h b/avr/src/gcode_state.h index 214469b..8c97677 100644 --- a/avr/src/gcode_state.h +++ b/avr/src/gcode_state.h @@ -40,42 +40,45 @@ * commands, whereas motion_mode_t persists across blocks as G modal group 1 */ -/// these are in order to optimized CASE statement typedef enum { - NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes) - NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle - NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set - NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle no coord setting - NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates - NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position - NEXT_ACTION_SET_G30_POSITION, // G30.1 - NEXT_ACTION_GOTO_G30_POSITION, // G30 - NEXT_ACTION_SET_COORD_DATA, // G10 - NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 - NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 - NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 - NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 - NEXT_ACTION_DWELL, // G4 - NEXT_ACTION_STRAIGHT_PROBE, // G38.2 + NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes) + NEXT_ACTION_DWELL, // G4 + NEXT_ACTION_SET_COORD_DATA, // G10 + NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position + NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates + NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set + NEXT_ACTION_GOTO_G30_POSITION, // G30 + NEXT_ACTION_SET_G30_POSITION, // G30.1 + NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 + NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 + NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 + NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 } next_action_t; -typedef enum { // G Modal Group 1 - MOTION_MODE_RAPID, // G0 - rapid - MOTION_MODE_FEED, // G1 - straight feed - MOTION_MODE_CW_ARC, // G2 - clockwise arc feed - MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed - MOTION_MODE_CANCEL_MOTION_MODE, // G80 - MOTION_MODE_STRAIGHT_PROBE, // G38.2 - MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling - MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell - MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling - MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping - MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out - MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out - MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring - MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out - MOTION_MODE_CANNED_CYCLE_89, // G89 - boring, dwell, feed out +typedef enum { // G Modal Group 1 + MOTION_MODE_RAPID, // G0 - rapid + MOTION_MODE_FEED, // G1 - straight feed + MOTION_MODE_CW_ARC, // G2 - clockwise arc feed + MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed + MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR, // G38.2 + MOTION_MODE_STRAIGHT_PROBE_CLOSE, // G38.3 + MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR, // G38.4 + MOTION_MODE_STRAIGHT_PROBE_OPEN, // G38.5 + MOTION_MODE_SEEK_CLOSE_ERR, // G38.6 + MOTION_MODE_SEEK_CLOSE, // G38.7 + MOTION_MODE_SEEK_OPEN_ERR, // G38.8 + MOTION_MODE_SEEK_OPEN, // G38.9 + MOTION_MODE_CANCEL_MOTION_MODE, // G80 + MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling + MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell + MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling + MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping + MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out + MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out + MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring + MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, man out + MOTION_MODE_CANNED_CYCLE_89, // G89 - boring, dwell, feed out } motion_mode_t; diff --git a/avr/src/home.c b/avr/src/home.c deleted file mode 100644 index 96886c8..0000000 --- a/avr/src/home.c +++ /dev/null @@ -1,176 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "home.h" - -#include "plan/runtime.h" -#include "plan/line.h" -#include "plan/state.h" -#include "plan/exec.h" -#include "axis.h" -#include "motor.h" -#include "util.h" -#include "config.h" - -#include -#include - - -typedef enum { - STATE_INIT, - STATE_HOMING, - STATE_STALLED, - STATE_BACKOFF, - STATE_DONE, -} home_state_t; - - -typedef struct { - bool homed[AXES]; - unsigned axis; - home_state_t state; - float velocity; - uint16_t microsteps; -} home_t; - -static home_t home; - - -void _stall_callback(int motor) { - if (home.velocity == mp_runtime_get_velocity()) { - mp_exec_abort(); - home.state = STATE_STALLED; - } -} - - -void _move_axis(float travel) { - float target[AXES]; - float *position = mp_runtime_get_position(); - copy_vector(target, position); - target[home.axis] += travel; - mp_aline(target, false, false, false, home.velocity, 1, -1); -} - - -void home_callback() { - if (mp_get_cycle() != CYCLE_HOMING || !mp_is_quiescent() || - !mp_queue_is_empty()) return; - - while (true) { - int motor = axis_get_motor(home.axis); - homing_mode_t mode = axis_get_homing_mode(home.axis); - int direction = - (mode == HOMING_STALL_MIN || mode == HOMING_SWITCH_MIN) ? -1 : 1; - float min = axis_get_travel_min(home.axis); - float max = axis_get_travel_max(home.axis); - - switch (home.state) { - case STATE_INIT: { - if (motor == -1 || mode == HOMING_DISABLED) { - home.state = STATE_DONE; - break; - } - - STATUS_INFO("Homing %c axis", axis_get_char(home.axis)); - - // Set axis not homed - home.homed[home.axis] = false; - - // Determine homing type: switch or stall - - // Configure driver, set stall callback and compute homing velocity - home.microsteps = motor_get_microsteps(motor); - motor_set_microsteps(motor, 8); - motor_set_stall_callback(motor, _stall_callback); - //home.velocity = axis_get_stall_homing_velocity(home.axis); - home.velocity = axis_get_search_velocity(home.axis); - - // Move in home direction - float travel = max - min; // TODO consider ramping distance - _move_axis(travel * direction); - - home.state = STATE_HOMING; - return; - } - - case STATE_HOMING: - case STATE_STALLED: - // Restore motor driver config - motor_set_microsteps(motor, home.microsteps); - motor_set_stall_callback(motor, 0); - - if (home.state == STATE_HOMING) { - STATUS_ERROR(STAT_HOMING_CYCLE_FAILED, "Failed to find %c axis home", - axis_get_char(home.axis)); - mp_set_cycle(CYCLE_MACHINING); - - } else { - STATUS_INFO("Backing off %c axis", axis_get_char(home.axis)); - mach_set_axis_position(home.axis, direction < 0 ? min : max); - _move_axis(axis_get_zero_backoff(home.axis) * direction * -1); - home.state = STATE_BACKOFF; - } - return; - - case STATE_BACKOFF: - STATUS_INFO("Homed %c axis", axis_get_char(home.axis)); - - // Set axis position & homed - mach_set_axis_position(home.axis, min); - home.homed[home.axis] = true; - home.state = STATE_DONE; - break; - - case STATE_DONE: - if (home.axis == AXIS_X) { - // Done - mp_set_cycle(CYCLE_MACHINING); - return; - } - - // Next axis - home.axis--; - home.state = STATE_INIT; - break; - } - } -} - - -uint8_t command_home(int argc, char *argv[]) { - if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) - return 0; - - // Init - memset(&home, 0, sizeof(home)); - home.axis = AXIS_Z; - - mp_set_cycle(CYCLE_HOMING); - - return 0; -} diff --git a/avr/src/home.h b/avr/src/home.h deleted file mode 100644 index f888cad..0000000 --- a/avr/src/home.h +++ /dev/null @@ -1,31 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -void home_init(); -void home_callback(); diff --git a/avr/src/homing.c b/avr/src/homing.c deleted file mode 100644 index 7c2f9d4..0000000 --- a/avr/src/homing.c +++ /dev/null @@ -1,408 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "homing.h" - -#include "axis.h" -#include "machine.h" -#include "switch.h" -#include "gcode_parser.h" -#include "report.h" - -#include "plan/planner.h" -#include "plan/runtime.h" -#include "plan/state.h" - - -typedef void (*homing_func_t)(int8_t axis); - -static void _homing_axis_start(int8_t axis); - - -typedef enum { - HOMING_NOT_HOMED, // machine is not homed - HOMING_HOMED, // machine is homed - HOMING_WAITING, // machine waiting to be homed -} homing_state_t; - - -/// persistent homing runtime variables -typedef struct { - homing_state_t state; // homing cycle sub-state machine - bool homed[AXES]; // individual axis homing flags - - // controls for homing cycle - int8_t axis; // axis currently being homed - - /// homing switch for current axis (index into switch flag table) - int8_t homing_switch; - int8_t limit_switch; // limit switch for current axis or -1 if none - - uint8_t homing_closed; // 0=open, 1=closed - uint8_t limit_closed; // 0=open, 1=closed - /// G28.4 flag. true = set coords to zero at the end of homing cycle - uint8_t set_coordinates; - homing_func_t func; // binding for callback function state machine - - // per-axis parameters - /// set to 1 for positive (max), -1 for negative (to min); - float direction; - float search_travel; // signed distance to travel in search - float search_velocity; // search speed as positive number - float latch_velocity; // latch speed as positive number - /// max distance to back off switch during latch phase - float latch_backoff; - /// distance to back off switch before setting zero - float zero_backoff; - /// maximum distance of switch clearing backoffs before erring out - float max_clear_backoff; - - // state saved from gcode model - uint8_t saved_units; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_mode; // G93,G94 global setting - float saved_feed_rate; // F setting - float saved_jerk; // saved and restored for each axis homed -} homing_t; - - -static homing_t hm = {0}; - - -// G28.2 homing cycle - -/* Homing works from a G28.2 according to the following writeup: - * - * https://github.com/synthetos - * /TinyG/wiki/Homing-and-Limits-Description-and-Operation - * - * How does this work? - * - * Homing is invoked using a G28.2 command with 1 or more axes specified in the - * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant) - * - * Homing is always run in the following order - for each enabled axis: - * Z,X,Y,A Note: B and C cannot be homed - * - * At the start of a homing cycle those switches configured for homing - * (or for homing and limits) are treated as homing switches (they are modal). - * - * After initialization the following sequence is run for each axis to be homed: - * - * 0. If a homing or limit switch is closed on invocation, clear the switch - * 1. Drive towards the homing switch at search velocity until switch is hit - * 2. Drive away from the homing switch at latch velocity until switch opens - * 3. Back off switch by the zero backoff distance and set zero for that axis - * - * Homing works as a state machine that is driven by registering a callback - * function at hm.func() for the next state to be run. Once the axis is - * initialized each callback basically does two things (1) start the move - * for the current function, and (2) register the next state with hm.func(). - * When a move is started it will either be interrupted if the homing switch - * changes state, This will cause the move to stop with a feedhold. The other - * thing that can happen is the move will run to its full length if no switch - * change is detected (hit or open), - * - * Once all moves for an axis are complete the next axis in the sequence is - * homed. - * - * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED - * When homing completes successfully this is set to HOMING_HOMED, otherwise it - * remains HOMING_NOT_HOMED. - * - * Notes: - * - * 1. When coding a cycle (like this one) you get to perform one queued - * move per entry into the continuation, then you must exit. - * - * 2. When coding a cycle (like this one) you must wait until - * the last move has actually been queued (or has finished) before - * declaring the cycle to be done. Otherwise there is a nasty race - * condition that will accept the next command before the position of - * the final move has been recorded in the Gcode model. - */ - - -/*** Return next axis in sequence based on axis in arg - * - * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis - * Returns next axis based on "axis" argument and if that axis is flagged for - * homing in the gf struct - * Returns -1 when all axes have been processed - * Returns -2 if no axes are specified (Gcode calling error) - * Homes Z first, then the rest in sequence - * - * Isolating this function facilitates implementing more complex and - * user-specified axis homing orders - */ -static int8_t _get_next_axis(int8_t axis) { - switch (axis) { - case -1: if (parser.gf.target[AXIS_Z]) return AXIS_Z; - case AXIS_Z: if (parser.gf.target[AXIS_X]) return AXIS_X; - case AXIS_X: if (parser.gf.target[AXIS_Y]) return AXIS_Y; - case AXIS_Y: if (parser.gf.target[AXIS_A]) return AXIS_A; - case AXIS_A: if (parser.gf.target[AXIS_B]) return AXIS_B; - case AXIS_B: if (parser.gf.target[AXIS_C]) return AXIS_C; - } - - return axis == -1 ? -2 : -1; // error or done -} - - -/// Helper to finalize homing, third part of return to home -static void _homing_finalize_exit() { - mp_flush_planner(); // should be stopped, but in case of switch closure - - // Restore saved machine state - mach_set_coord_system(hm.saved_coord_system); - mach_set_units(hm.saved_units); - mach_set_distance_mode(hm.saved_distance_mode); - mach_set_feed_mode(hm.saved_feed_mode); - mach_set_feed_rate(hm.saved_feed_rate); - mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle -} - - -static void _homing_error_exit(stat_t status) { - _homing_finalize_exit(); - status_error(status); -} - - -/// Execute moves -static void _homing_axis_move(int8_t axis, float target, float velocity) { - float vect[AXES] = {0}; - bool flags[AXES] = {0}; - - vect[axis] = target; - flags[axis] = true; - mach_set_feed_rate(velocity); - mp_flush_planner(); // don't use mp_request_flush() here - - stat_t status = mach_feed(vect, flags); - if (status) _homing_error_exit(status); -} - - -/// End homing cycle in progress -static void _homing_abort(int8_t axis) { - axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value - - // homing state remains HOMING_NOT_HOMED - _homing_error_exit(STAT_HOMING_CYCLE_FAILED); - - report_request(); -} - - -/// set zero and finish up -static void _homing_axis_set_zero(int8_t axis) { - if (hm.set_coordinates) { - mach_set_axis_position(axis, 0); - hm.homed[axis] = true; - - } else // do not set axis if in G28.4 cycle - mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); - - axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value - - hm.func = _homing_axis_start; -} - - -/// backoff to zero position -static void _homing_axis_zero_backoff(int8_t axis) { - _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); - hm.func = _homing_axis_set_zero; -} - - -/// Slow reverse until switch opens again -static void _homing_axis_latch(int8_t axis) { - // verify assumption that we arrived here because of homing switch closure - // rather than user-initiated feedhold or other disruption - if (!switch_is_active(hm.homing_switch)) hm.func = _homing_abort; - - else { - _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); - hm.func = _homing_axis_zero_backoff; - } -} - - -/// Fast search for switch, closes switch -static void _homing_axis_search(int8_t axis) { - // use the homing jerk for search onward - _homing_axis_move(axis, hm.search_travel, hm.search_velocity); - hm.func = _homing_axis_latch; -} - - -/// Initiate a clear to move off a switch that is thrown at the start -static void _homing_axis_clear(int8_t axis) { - // Handle an initial switch closure by backing off the closed switch - // NOTE: Relies on independent switches per axis (not shared) - - if (switch_is_active(hm.homing_switch)) - _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); - - else if (switch_is_active(hm.limit_switch)) - _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); - - hm.func = _homing_axis_search; -} - - -/// Get next axis, initialize variables, call the clear -static void _homing_axis_start(int8_t axis) { - // get the first or next axis - if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error - if (axis == -1) { // -1 is done - hm.state = HOMING_HOMED; - _homing_finalize_exit(); - return; - - } else if (axis == -2) // -2 is error - return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS); - } - - // clear the homed flag for axis to move w/o triggering soft limits - hm.homed[axis] = false; - - // trap axis mis-configurations - if (fp_ZERO(axis_get_search_velocity(axis))) - return _homing_error_exit(STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); - if (fp_ZERO(axis_get_latch_velocity(axis))) - return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); - if (axis_get_latch_backoff(axis) < 0) - return _homing_error_exit(STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); - - // calculate and test travel distance - float travel_distance = - fabs(axis_get_travel_max(axis) - axis_get_travel_min(axis)) + - axis_get_latch_backoff(axis); - if (fp_ZERO(travel_distance)) - return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); - - hm.axis = axis; // persist the axis - // search velocity is always positive - hm.search_velocity = fabs(axis_get_search_velocity(axis)); - // latch velocity is always positive - hm.latch_velocity = fabs(axis_get_latch_velocity(axis)); - - // determine which switch to use - bool min_enabled = switch_is_enabled(MIN_SWITCH(axis)); - bool max_enabled = switch_is_enabled(MAX_SWITCH(axis)); - - if (min_enabled) { - // setup parameters homing to the minimum switch - hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch - hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch - hm.search_travel = -travel_distance; // in negative direction - hm.latch_backoff = axis_get_latch_backoff(axis); // in positive direction - hm.zero_backoff = axis_get_zero_backoff(axis); - - } else if (max_enabled) { - // setup parameters for positive travel (homing to the maximum switch) - hm.homing_switch = MAX_SWITCH(axis); // max is homing switch - hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch - hm.search_travel = travel_distance; // in positive direction - hm.latch_backoff = -axis_get_latch_backoff(axis); // in negative direction - hm.zero_backoff = -axis_get_zero_backoff(axis); - - } else { - // if homing is disabled for the axis then skip to the next axis - hm.limit_switch = -1; // disable the limit switch parameter - hm.func = _homing_axis_start; - return; - } - - hm.saved_jerk = axis_get_jerk_max(axis); // save the max jerk value - hm.func = _homing_axis_clear; // start the clear -} - - -bool mach_is_homing() { - return mp_get_cycle() == CYCLE_HOMING; -} - - -void mach_set_not_homed() { - for (int axis = 0; axis < AXES; axis++) - mach_set_homed(axis, false); -} - - -bool mach_get_homed(int axis) { - return hm.homed[axis]; -} - - -void mach_set_homed(int axis, bool homed) { - // TODO save homed to EEPROM - hm.homed[axis] = homed; -} - - -/// G28.2 homing cycle using limit switches -void mach_homing_cycle_start() { - // save relevant non-axis parameters from Gcode model - hm.saved_units = mach_get_units(); - hm.saved_coord_system = mach_get_coord_system(); - hm.saved_distance_mode = mach_get_distance_mode(); - hm.saved_feed_mode = mach_get_feed_mode(); - hm.saved_feed_rate = mach_get_feed_rate(); - - // set working values - mach_set_units(MILLIMETERS); - mach_set_distance_mode(INCREMENTAL_MODE); - mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates - mach_set_feed_mode(UNITS_PER_MINUTE_MODE); - hm.set_coordinates = true; - - hm.axis = -1; // set to retrieve initial axis - hm.func = _homing_axis_start; // bind initial processing function - mp_set_cycle(CYCLE_HOMING); - hm.state = HOMING_NOT_HOMED; -} - - -void mach_homing_cycle_start_no_set() { - mach_homing_cycle_start(); - hm.set_coordinates = false; // don't update position variables at end of cycle -} - - -/// Main loop callback for running the homing cycle -void mach_homing_callback() { - if (mp_get_cycle() != CYCLE_HOMING || mp_get_state() != STATE_READY) return; - hm.func(hm.axis); // execute the current homing move -} diff --git a/avr/src/homing.h b/avr/src/homing.h deleted file mode 100644 index 6d527fd..0000000 --- a/avr/src/homing.h +++ /dev/null @@ -1,39 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include - - -bool mach_is_homing(); -void mach_set_not_homed(); -bool mach_get_homed(int axis); -void mach_set_homed(int axis, bool homed); -void mach_homing_cycle_start(); -void mach_homing_cycle_start_no_set(); -void mach_homing_callback(); diff --git a/avr/src/i2c.h b/avr/src/i2c.h index 2a74f81..d8f4ef0 100644 --- a/avr/src/i2c.h +++ b/avr/src/i2c.h @@ -43,7 +43,6 @@ typedef enum { I2C_STEP, I2C_FLUSH, I2C_REPORT, - I2C_HOME, I2C_REBOOT, I2C_ZERO, } i2c_cmd_t; diff --git a/avr/src/machine.c b/avr/src/machine.c index bf8a4fc..50e29ad 100644 --- a/avr/src/machine.c +++ b/avr/src/machine.c @@ -66,7 +66,6 @@ #include "gcode_parser.h" #include "spindle.h" #include "coolant.h" -#include "homing.h" #include "plan/planner.h" #include "plan/runtime.h" @@ -298,7 +297,9 @@ void mach_calc_target(float target[], const float values[], float radius = axis_get_radius(axis); if (radius) // Handle radius mode if radius is non-zero target[axis] += TO_MM(values[axis]) * 360 / (2 * M_PI * radius); + // For ABC axes no mm conversion - it's already in degrees + // TODO This should depend on the axis mode else if (AXIS_Z < axis) target[axis] += values[axis]; else target[axis] += TO_MM(values[axis]); } @@ -310,7 +311,7 @@ void mach_calc_target(float target[], const float values[], * Must be called with target properly set in GM struct. Best done * after mach_calc_target(). * - * Tests for soft limit for any homed axis if min and max are + * Tests for soft limit for any axis if min and max are * different values. You can set min and max to 0,0 to disable soft * limits for an axis. Also will not test a min or a max if the value * is < -1000000 (negative one million). This allows a single end to @@ -318,8 +319,6 @@ void mach_calc_target(float target[], const float values[], */ stat_t mach_test_soft_limits(float target[]) { for (int axis = 0; axis < AXES; axis++) { - if (!axis_get_homed(axis)) continue; // don't test axes that arent homed - float min = axis_get_travel_min(axis); float max = axis_get_travel_max(axis); @@ -412,7 +411,7 @@ void mach_set_coord_system(coord_system_t cs) { /* Set the position of a single axis in the model, planner and runtime * * This command sets an axis/axes to a position provided as an argument. - * This is useful for setting origins for homing, probing, and other operations. + * This is useful for setting origins for probing, and other operations. * * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! * @@ -422,8 +421,8 @@ void mach_set_coord_system(coord_system_t cs) { * because the planned / running moves have a different reference * frame than the one you are now going to set. These functions should * only be called during initialization sequences and during cycles - * (such as homing cycles) when you know there are no more moves in - * the planner and that all motion has stopped. + * when you know there are no more moves in the planner and that all motion + * has stopped. */ void mach_set_axis_position(unsigned axis, float position) { //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT); @@ -462,10 +461,7 @@ static stat_t _exec_absolute_origin(mp_buffer_t *bf) { const float *flags = bf->unit; for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) { - mp_runtime_set_axis_position(axis, origin[axis]); - mach_set_homed(axis, true); // G28.3 is not considered homed until here - } + if (flags[axis]) mp_runtime_set_axis_position(axis, origin[axis]); mp_runtime_set_steps_from_position(); @@ -482,8 +478,7 @@ static stat_t _exec_absolute_origin(mp_buffer_t *bf) { * This is a 2 step process. The model and planner contexts are set * immediately, the runtime command is queued and synchronized with * the planner queue. This includes the runtime position and the step - * recording done by the encoders. At that point any axis that is set - * is also marked as homed. + * recording done by the encoders. */ void mach_set_absolute_origin(float origin[], bool flags[]) { float value[AXES]; @@ -526,26 +521,51 @@ void mach_reset_origin_offsets() { /// G92.2 -void mach_suspend_origin_offsets() { - mach.origin_offset_enable = false; -} +void mach_suspend_origin_offsets() {mach.origin_offset_enable = false;} /// G92.3 -void mach_resume_origin_offsets() { - mach.origin_offset_enable = true; -} +void mach_resume_origin_offsets() {mach.origin_offset_enable = true;} + + +stat_t mach_plan_line(float target[], switch_id_t sw) { + buffer_flags_t flags = 0; + switch (mach_get_motion_mode()) { + case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR: + case MOTION_MODE_SEEK_CLOSE_ERR: + flags |= BUFFER_SEEK_CLOSE | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP; + break; -stat_t mach_plan_line(float target[]) { - return mp_aline(target, mach_is_rapid(), mach_is_inverse_time_mode(), - mach_is_exact_stop(), mach.gm.feed_rate, + case MOTION_MODE_STRAIGHT_PROBE_CLOSE: + case MOTION_MODE_SEEK_CLOSE: + flags |= BUFFER_SEEK_CLOSE | BUFFER_EXACT_STOP; + break; + + case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR: + case MOTION_MODE_SEEK_OPEN_ERR: + flags |= BUFFER_SEEK_OPEN | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP; + break; + + case MOTION_MODE_STRAIGHT_PROBE_OPEN: + case MOTION_MODE_SEEK_OPEN: + flags |= BUFFER_SEEK_OPEN | BUFFER_EXACT_STOP; + break; + + default: break; + } + + if (mach_is_rapid()) flags |= BUFFER_RAPID; + if (mach_is_inverse_time_mode()) flags |= BUFFER_INVERSE_TIME; + if (mach_is_exact_stop()) flags |= BUFFER_EXACT_STOP; + + return mp_aline(target, flags, sw, mach.gm.feed_rate, mach_get_feed_override(), mach_get_line()); } // Free Space Motion (4.3.4) -static stat_t _feed(float values[], bool flags[]) { +static stat_t _feed(float values[], bool flags[], switch_id_t sw) { float target[AXES]; mach_calc_target(target, values, flags); @@ -555,7 +575,7 @@ static stat_t _feed(float values[], bool flags[]) { // prep and plan the move mach_update_work_offsets(); // update resolved offsets - mach_plan_line(target); + mach_plan_line(target, sw); copy_vector(mach.position, target); // update model position return status; @@ -564,8 +584,8 @@ static stat_t _feed(float values[], bool flags[]) { /// G0 linear rapid stat_t mach_rapid(float values[], bool flags[]) { - mach.gm.motion_mode = MOTION_MODE_RAPID; - return _feed(values, flags); + mach_set_motion_mode(MOTION_MODE_RAPID); + return _feed(values, flags, 0); } @@ -603,6 +623,41 @@ stat_t mach_goto_g30_position(float target[], bool flags[]) { } +stat_t mach_probe(float values[], bool flags[], motion_mode_t mode) { + mach_set_motion_mode(mode); + return _feed(values, flags, SW_PROBE); +} + + +stat_t mach_seek(float values[], bool flags[], motion_mode_t mode) { + mach_set_motion_mode(mode); + + float target[AXES]; + mach_calc_target(target, values, flags); + + switch_id_t sw = SW_PROBE; + + for (int axis = 0; axis < AXES; axis++) + if (flags[axis]) { + if (!axis_is_enabled(axis)) return STAT_SEEK_AXIS_DISABLED; + if (sw != SW_PROBE) return STAT_SEEK_MULTIPLE_AXES; + if (fp_EQ(target[axis], mach.position[axis])) return STAT_SEEK_ZERO_MOVE; + + bool min = target[axis] < mach.position[axis]; + switch (axis) { + case AXIS_X: sw = min ? SW_MIN_X : SW_MAX_X; break; + case AXIS_Y: sw = min ? SW_MIN_Y : SW_MAX_Y; break; + case AXIS_Z: sw = min ? SW_MIN_Z : SW_MAX_Z; break; + case AXIS_A: sw = min ? SW_MIN_A : SW_MAX_A; break; + } + } + + if (sw == SW_PROBE) return STAT_SEEK_MISSING_AXIS; + + return _feed(values, flags, sw); +} + + // Machining Attributes (4.3.5) /// F parameter @@ -630,13 +685,7 @@ void mach_set_path_mode(path_mode_t mode) { } -// Machining Functions (4.3.6) See arc.c - -/// G4, P parameter (seconds) -stat_t mach_dwell(float seconds) { - return mp_dwell(seconds, mach_get_line()); -} - +// Machining Functions (4.3.6) see also arc.c /// G1 stat_t mach_feed(float values[], bool flags[]) { @@ -645,11 +694,15 @@ stat_t mach_feed(float values[], bool flags[]) { (mach.gm.feed_mode == INVERSE_TIME_MODE && !parser.gf.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - mach.gm.motion_mode = MOTION_MODE_FEED; - return _feed(values, flags); + mach_set_motion_mode(MOTION_MODE_FEED); + return _feed(values, flags, 0); } +/// G4, P parameter (seconds) +stat_t mach_dwell(float seconds) {return mp_dwell(seconds, mach_get_line());} + + // Spindle Functions (4.3.7) see spindle.c, spindle.h // Tool Functions (4.3.8) diff --git a/avr/src/machine.h b/avr/src/machine.h index d3b09a2..c053441 100644 --- a/avr/src/machine.h +++ b/avr/src/machine.h @@ -32,6 +32,7 @@ #include "config.h" #include "status.h" #include "gcode_state.h" +#include "switch.h" #define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) @@ -97,12 +98,14 @@ void mach_suspend_origin_offsets(); void mach_resume_origin_offsets(); // Free Space Motion (4.3.4) -stat_t mach_plan_line(float target[]); +stat_t mach_plan_line(float target[], switch_id_t sw); stat_t mach_rapid(float target[], bool flags[]); void mach_set_g28_position(); stat_t mach_goto_g28_position(float target[], bool flags[]); void mach_set_g30_position(); stat_t mach_goto_g30_position(float target[], bool flags[]); +stat_t mach_probe(float target[], bool flags[], motion_mode_t mode); +stat_t mach_seek(float target[], bool flags[], motion_mode_t mode); // Machining Attributes (4.3.5) void mach_set_feed_rate(float feed_rate); diff --git a/avr/src/main.c b/avr/src/main.c index 9443960..1066f6b 100644 --- a/avr/src/main.c +++ b/avr/src/main.c @@ -39,9 +39,6 @@ #include "report.h" #include "command.h" #include "estop.h" -#include "probing.h" -#include "homing.h" -#include "home.h" #include "i2c.h" #include "pgmspace.h" @@ -86,9 +83,6 @@ int main() { if (!estop_triggered()) { mp_state_callback(); mach_arc_callback(); // arc generation runs - home_callback(); - //mach_homing_callback(); // G28.2 continuation TODO - mach_probe_callback(); // G38.2 continuation } command_callback(); // process next command report_callback(); // report changes diff --git a/avr/src/messages.def b/avr/src/messages.def index 1260926..ca4a85a 100644 --- a/avr/src/messages.def +++ b/avr/src/messages.def @@ -80,6 +80,13 @@ STAT_MSG(ARC_RADIUS_OUT_OF_TOLERANCE, "Arc radius arc out of tolerance") STAT_MSG(ARC_ENDPOINT_IS_STARTING_POINT, "Arc end point is starting point") STAT_MSG(ARC_OFFSETS_MISSING_FOR_PLANE, "arc offsets missing for plane") STAT_MSG(P_WORD_IS_NOT_A_POSITIVE_INTEGER, "P word is not a positive integer") +STAT_MSG(EXPR_VALUE_STACK_OVERFLOW, "Expression parser value overflow") +STAT_MSG(EXPR_VALUE_STACK_UNDERFLOW, "Expression parser value underflow") +STAT_MSG(EXPR_OP_STACK_OVERFLOW, "Expression parser operator overflow") +STAT_MSG(EXPR_OP_STACK_UNDERFLOW, "Expression parser operator underflow") +STAT_MSG(GCODE_FUNC_UNSUPPORTED, "GCode function call unsupported") +STAT_MSG(GCODE_NUM_PARAM_UNSUPPORTED, "GCode numbered parameters unsupported") +STAT_MSG(GCODE_UNTERMINATED_VAR, "GCode variable reference untermiated") // Errors and warnings STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length") @@ -110,5 +117,12 @@ STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION, STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination") STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe") +// Seeking +STAT_MSG(SEEK_MULTIPLE_AXES, "Seek error - multiple axes specified") +STAT_MSG(SEEK_MISSING_AXIS, "Seek error - no axis specified") +STAT_MSG(SEEK_AXIS_DISABLED, "Seek error - specified axis is disabled") +STAT_MSG(SEEK_ZERO_MOVE, "Seek error - axis move too short or zero") +STAT_MSG(SEEK_SWTICH_NOT_FOUND, "Seek error - move end before switch change") + // End of stats marker STAT_MSG(MAX, "") diff --git a/avr/src/motor.c b/avr/src/motor.c index 901139a..7419bc2 100644 --- a/avr/src/motor.c +++ b/avr/src/motor.c @@ -185,22 +185,6 @@ void motor_set_axis(int motor, uint8_t axis) { } -void motor_set_stall_callback(int motor, stall_callback_t cb) { - drv8711_set_stall_callback(motor, cb); -} - - -/// @return computed homing velocity -float motor_get_stall_homing_velocity(int motor) { - // Compute velocity: - // velocity = travel_rev * step_angle * 60 / (SMPLTH * mstep * 360 * 2) - // SMPLTH = 50us = 0.00005s - // mstep = 8 - return motors[motor].travel_rev * motors[motor].step_angle * 1667 / - motors[motor].microsteps; -} - - float motor_get_steps_per_unit(int motor) {return motors[motor].steps_per_unit;} uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;} diff --git a/avr/src/motor.h b/avr/src/motor.h index a4f5bda..ce568b4 100644 --- a/avr/src/motor.h +++ b/avr/src/motor.h @@ -42,15 +42,10 @@ typedef enum { } motor_power_mode_t; -typedef void (*stall_callback_t)(int motor); - - void motor_init(); bool motor_is_enabled(int motor); int motor_get_axis(int motor); -void motor_set_stall_callback(int motor, stall_callback_t cb); -float motor_get_stall_homing_velocity(int motor); float motor_get_steps_per_unit(int motor); uint16_t motor_get_microsteps(int motor); void motor_set_microsteps(int motor, uint16_t microsteps); diff --git a/avr/src/plan/arc.c b/avr/src/plan/arc.c index e389ed9..7f6d382 100644 --- a/avr/src/plan/arc.c +++ b/avr/src/plan/arc.c @@ -492,7 +492,7 @@ void mach_arc_callback() { } // run the line - mach_plan_line(arc.position); + mach_plan_line(arc.position, 0); if (!--arc.segments) arc.running = false; } diff --git a/avr/src/plan/buffer.c b/avr/src/plan/buffer.c index 74e4301..9943d21 100644 --- a/avr/src/plan/buffer.c +++ b/avr/src/plan/buffer.c @@ -199,8 +199,10 @@ void mp_buffer_print(const mp_buffer_t *bf) { "\"delta_vmax\":%0.2f," "\"jerk\":%0.2f," "\"cbrt_jerk\":%0.2f" - "}", bf->ts, bf->line, bf->state, bf->replannable ? "true" : "false", - bf->hold ? "true" : "false", bf->value, bf->target[0], bf->target[1], + "}", bf->ts, bf->line, bf->state, + (bf->flags & BUFFER_REPLANNABLE) ? "true" : "false", + (bf->flags & BUFFER_HOLD) ? "true" : "false", + bf->value, bf->target[0], bf->target[1], bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2], bf->unit[3], bf->length, bf->head_length, bf->body_length, bf->tail_length, bf->entry_velocity, bf->cruise_velocity, diff --git a/avr/src/plan/buffer.h b/avr/src/plan/buffer.h index c962132..96efd16 100644 --- a/avr/src/plan/buffer.h +++ b/avr/src/plan/buffer.h @@ -42,6 +42,18 @@ typedef enum { } buffer_state_t; +typedef enum { + BUFFER_REPLANNABLE = 1 << 0, + BUFFER_HOLD = 1 << 1, + BUFFER_SEEK_CLOSE = 1 << 2, + BUFFER_SEEK_OPEN = 1 << 3, + BUFFER_SEEK_ERROR = 1 << 4, + BUFFER_RAPID = 1 << 5, + BUFFER_INVERSE_TIME = 1 << 6, + BUFFER_EXACT_STOP = 1 << 7, +} buffer_flags_t; + + // Callbacks struct mp_buffer_t; typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf); @@ -56,8 +68,8 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes buffer_cb_t cb; // callback to buffer exec function buffer_state_t state; // buffer state - bool replannable; // true if move can be re-planned - bool hold; // hold at the start of this block + buffer_flags_t flags; // buffer flags + switch_id_t sw; // Switch to seek float value; // used in dwell and other callbacks diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index 0b6da2b..dc19f4a 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -209,7 +209,7 @@ static void _plan_hold() { // Distance to brake to zero from braking_velocity, bf is OK to use here float braking_length = mp_get_target_length(braking_velocity, 0, bf->jerk); - // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing. + // Hack to prevent Case 2 moves for perfect-fit decels. if (available_length < braking_length && fp_ZERO(bf->exit_velocity)) braking_length = available_length; @@ -362,6 +362,13 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { if (status != STAT_OK) return status; } + // If seeking, end move when switch is in target state. + if ((bf->flags & BUFFER_SEEK_CLOSE && !switch_is_active(bf->sw)) || + (bf->flags & BUFFER_SEEK_OPEN && switch_is_active(bf->sw))) { + mp_runtime_set_velocity(0); + return STAT_NOOP; + } + // Plan holds if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold(); @@ -372,8 +379,16 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { case SECTION_TAIL: status = _exec_aline_tail(); break; } - // Set runtime velocity on exit - if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity); + // Exiting + if (status != STAT_EAGAIN) { + // Set runtime velocity on exit + mp_runtime_set_velocity(ex.exit_velocity); + + // If seeking, switch was not found. Signal error if necessary. + if ((bf->flags & (BUFFER_SEEK_CLOSE | BUFFER_SEEK_OPEN)) && + (bf->flags & BUFFER_SEEK_ERROR)) + return STAT_SEEK_SWTICH_NOT_FOUND; + } return status; } @@ -403,7 +418,7 @@ stat_t mp_exec_move() { // Take control of buffer bf->state = BUFFER_INIT; - bf->replannable = false; + bf->flags &= ~BUFFER_REPLANNABLE; // Update runtime mp_runtime_set_line(bf->line); @@ -425,7 +440,8 @@ stat_t mp_exec_move() { if (status != STAT_EAGAIN) { // Signal that we've encountered a stopping point if (fp_ZERO(mp_runtime_get_velocity()) && - (mp_get_state() == STATE_STOPPING || bf->hold)) mp_state_holding(); + (mp_get_state() == STATE_STOPPING || (bf->flags & BUFFER_HOLD))) + mp_state_holding(); // Handle buffer restarts and deallocation if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW; @@ -434,7 +450,7 @@ stat_t mp_exec_move() { // the new buffer has not started because the current one is still // being run by the steppers. Planning can overwrite the new buffer. // See notes above. - mp_buffer_next(bf)->replannable = false; + mp_buffer_next(bf)->flags &= ~BUFFER_REPLANNABLE; mp_queue_pop(); // Release buffer diff --git a/avr/src/plan/line.c b/avr/src/plan/line.c index 03c1ade..989f2d6 100644 --- a/avr/src/plan/line.c +++ b/avr/src/plan/line.c @@ -254,7 +254,7 @@ static void _calc_max_velocities(mp_buffer_t *bf, float move_time, // Zero out exact stop cases if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0; - else bf->replannable = true; + else bf->flags |= BUFFER_REPLANNABLE; } @@ -366,12 +366,13 @@ static float _calc_move_time(const float axis_length[], * @param reed_rate is in minutes when @param inverse_time is true. * See mach_set_feed_rate() */ -stat_t mp_aline(const float target[], bool rapid, bool inverse_time, - bool exact_stop, float feed_rate, float feed_override, - int32_t line) { +stat_t mp_aline(const float target[], buffer_flags_t flags, switch_id_t sw, + float feed_rate, float feed_override, int32_t line) { DEBUG_CALL("(%f, %f, %f, %f), %s, %s, %s, %f, %f, %d", target[0], target[1], - target[2], target[3], rapid ? "true" : "false", - inverse_time ? "true" : "false", exact_stop ? "true" : "false", + target[2], target[3], + (flags & BUFFER_RAPID) ? "true" : "false", + (flags & BUFFER_INVERSE_TIME) ? "true" : "false", + (flags & BUFFER_EXACT_STOP) ? "true" : "false", feed_rate, feed_override, line); // Compute axis and move lengths @@ -404,12 +405,15 @@ stat_t mp_aline(const float target[], bool rapid, bool inverse_time, _calc_and_cache_jerk_values(bf); // Compute move time and velocities - float time = _calc_move_time(axis_length, axis_square, rapid, inverse_time, - feed_rate, feed_override); - _calc_max_velocities(bf, time, exact_stop); + float time = _calc_move_time(axis_length, axis_square, flags & BUFFER_RAPID, + flags & BUFFER_INVERSE_TIME, feed_rate, + feed_override); + _calc_max_velocities(bf, time, flags & BUFFER_EXACT_STOP); // Note, the following lines must remain in order. - bf->line = line; // Planner needs then when planning steps + bf->line = line; // Planner needs this when planning steps + bf->flags = flags; // Move flags + bf->sw = sw; // Seek switche mp_plan(bf); // Plan block list mp_set_position(target); // Set planner position before committing buffer mp_queue_push(mp_exec_aline, line); // After position update diff --git a/avr/src/plan/line.h b/avr/src/plan/line.h index 84d1be6..e1f3b35 100644 --- a/avr/src/plan/line.h +++ b/avr/src/plan/line.h @@ -28,12 +28,12 @@ #pragma once #include "status.h" +#include "buffer.h" #include #include -stat_t mp_aline(const float target[], bool rapid, bool inverse_time, - bool exact_stop, float feed_rate, float feed_override, - int32_t line); +stat_t mp_aline(const float target[], buffer_flags_t flags, switch_id_t sw, + float feed_rate, float feed_override, int32_t line); int mp_find_jerk_axis(const float axis_square[]); diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c index 3a21889..02dafdb 100644 --- a/avr/src/plan/planner.c +++ b/avr/src/plan/planner.c @@ -449,7 +449,7 @@ void mp_print_queue(mp_buffer_t *bf) { "%0.2f,%0.2f,%0.2f,%0.2f," "%0.2f,%0.2f,%0.2f,%0.2f," "%0.2f,%0.2f,%0.2f\"}\n"), - i++, bp->replannable, (intptr_t)bp->cb, + i++, bp->flags & BUFFER_REPLANNABLE, (intptr_t)bp->cb, bp->length, bp->head_length, bp->body_length, bp->tail_length, bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity, bp->braking_velocity, @@ -483,8 +483,7 @@ void mp_print_queue(mp_buffer_t *bf) { * Variables that must be provided in the mp_buffer_t that will be processed: * * bl (function arg) - end of block list (last block in time) - * bl->replannable - start of block list set by last FALSE value - * [Note 1] + * bl->flags - replanable, hold, probe, etc [Note 1] * bl->length - provides block length * bl->entry_vmax - used during forward planning to set entry velocity * bl->cruise_vmax - used during forward planning to set cruise velocity @@ -494,7 +493,7 @@ void mp_print_queue(mp_buffer_t *bf) { * * Variables that will be set during processing: * - * bl->replannable - set if the block becomes optimally planned + * bl->flags - replanable * bl->braking_velocity - set during backward planning * bl->entry_velocity - set during forward planning * bl->cruise_velocity - set during forward planning @@ -512,9 +511,10 @@ void mp_print_queue(mp_buffer_t *bf) { * * Notes: * - * [1] Whether or not a block is planned is controlled by the bl->replannable - * setting. Replan flags are checked during the backwards pass. They prune - * the replan list to include only the latest blocks that require planning. + * [1] Whether or not a block is planned is controlled by the bl->flags + * BUFFER_REPLANNABLE bit. Replan flags are checked during the backwards + * pass. They prune the replan list to include only the latest blocks that + * require planning. * * In normal operation, the first block (currently running block) is not * replanned, but may be for feedholds and feed overrides. In these cases, @@ -531,7 +531,7 @@ void mp_plan(mp_buffer_t *bl) { // By the end bp points to the buffer before the first block. mp_buffer_t *next = bp; while ((bp = mp_buffer_prev(bp)) != bl) { - if (!bp->replannable) break; + if (!(bp->flags & BUFFER_REPLANNABLE)) break; bp->braking_velocity = min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; @@ -552,18 +552,18 @@ void mp_plan(mp_buffer_t *bl) { if (mp.plan_steps && bp->line != next->line) { bp->exit_velocity = 0; - bp->hold = true; + bp->flags |= BUFFER_HOLD; - } else bp->hold = false; + } else bp->flags &= ~BUFFER_HOLD; mp_calculate_trapezoid(bp); // Test for optimally planned trapezoids by checking exit conditions if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || fp_EQ(bp->exit_velocity, next->entry_vmax)) || - (!prev->replannable && + (!(prev->flags & BUFFER_REPLANNABLE) && fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) - bp->replannable = false; + bp->flags &= ~BUFFER_REPLANNABLE; prev = bp; } @@ -588,7 +588,7 @@ void mp_replan_all() { // Mark all blocks replanable while (true) { - bp->replannable = true; + bp->flags |= BUFFER_REPLANNABLE; mp_buffer_t *next = mp_buffer_next(bp); if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around bp = next; @@ -606,7 +606,7 @@ void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY; copy_vector(bp->unit, bp->prev->unit); - bp->replannable = true; + bp->flags |= BUFFER_REPLANNABLE; mp_queue_push(cb, line); } diff --git a/avr/src/probing.c b/avr/src/probing.c deleted file mode 100644 index 21ff1cb..0000000 --- a/avr/src/probing.c +++ /dev/null @@ -1,215 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "machine.h" -#include "axis.h" -#include "spindle.h" -#include "switch.h" -#include "util.h" -#include "pgmspace.h" - -#include "plan/planner.h" -#include "plan/runtime.h" -#include "plan/state.h" - -#include -#include -#include -#include - - -#define MINIMUM_PROBE_TRAVEL 0.254 - - -typedef enum { - PROBE_FAILED, // probe reached endpoint without triggering - PROBE_SUCCEEDED, // probe was triggered, pb.results has position - PROBE_WAITING, // probe is waiting to be started -} probe_state_t; - - -typedef struct { - probe_state_t state; - float results[AXES]; // probing results - - void (*func)(); // binding for callback function state machine - - // state saved from gcode model - uint8_t saved_distance_mode; // G90, G91 global setting - uint8_t saved_coord_system; // G54 - G59 setting - - // probe destination - float start_position[AXES]; - float target[AXES]; - bool flags[AXES]; -} probing_t; - - -static probing_t pb = {0}; - - -/* Note: When coding a cycle (like this one) you get to perform one - * queued move per entry into the continuation, then you must exit. - * - * Another Note: When coding a cycle (like this one) you must wait - * until the last move has actually been queued (or has finished) - * before declaring the cycle to be done. Otherwise there is a nasty - * race condition that will accept the next command before the position - * of the final move has been recorded in the Gcode model. - */ - - -static void _probe_restore_settings() { - // we should be stopped now, but in case of switch closure - mp_flush_planner(); - - // restore coordinate system and distance mode - mach_set_coord_system(pb.saved_coord_system); - mach_set_distance_mode(pb.saved_distance_mode); - - // update the model with actual position - mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle -} - - -static void _probing_error_exit(stat_t status) { - _probe_restore_settings(); // clean up and exit - status_error(status); -} - - -static void _probing_finish() { - bool closed = switch_is_active(SW_PROBE); - pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED; - - for (int axis = 0; axis < AXES; axis++) { - // if we got here because of a feed hold keep the model position correct - mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); - - // store the probe results - pb.results[axis] = mach_get_axis_position(axis); - } - - _probe_restore_settings(); -} - - -static void _probing_start() { - // initial probe state, don't probe if we're already contacted! - bool closed = switch_is_active(SW_PROBE); - - if (!closed) { - stat_t status = mach_feed(pb.target, pb.flags); - if (status) return _probing_error_exit(status); - } - - pb.func = _probing_finish; -} - - -/* G38.2 homing cycle using limit switches - * - * These initializations are required before starting the probing cycle. - * They must be done after the planner has exhasted all current CYCLE moves as - * they affect the runtime (specifically the switch modes). Side effects would - * include limit switches initiating probe actions instead of just killing - * movement - */ -static void _probing_init() { - // NOTE: it is *not* an error condition for the probe not to trigger. - // it is an error for the limit or homing switches to fire, or for some other - // configuration error. - pb.state = PROBE_FAILED; - mp_set_cycle(CYCLE_PROBING); - - // initialize the axes - for (int axis = 0; axis < AXES; axis++) - pb.start_position[axis] = mach_get_axis_position(axis); - - // error if the probe target is too close to the current position - if (axis_get_vector_length(pb.start_position, pb.target) < - MINIMUM_PROBE_TRAVEL) - _probing_error_exit(STAT_PROBE_INVALID_DEST); - - // error if the probe target requires a move along the A/B/C axes - for (int axis = 0; axis < AXES; axis++) - if (fp_NE(pb.start_position[axis], pb.target[axis])) - _probing_error_exit(STAT_MOVE_DURING_PROBE); - - // probe in absolute machine coords - pb.saved_coord_system = mach_get_coord_system(); - pb.saved_distance_mode = mach_get_distance_mode(); - mach_set_distance_mode(ABSOLUTE_MODE); - mach_set_coord_system(ABSOLUTE_COORDS); - - mach_set_spindle_mode(SPINDLE_OFF); - - // start the move - pb.func = _probing_start; -} - - -bool mach_is_probing() { - return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING; -} - - -/// G38.2 homing cycle using limit switches -stat_t mach_probe(float target[], bool flags[]) { - // trap zero feed rate condition - if (mach_get_feed_mode() != INVERSE_TIME_MODE && - fp_ZERO(mach_get_feed_rate())) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // trap no axes specified - if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z]) - return STAT_GCODE_AXIS_IS_MISSING; - - // set probe move endpoint - copy_vector(pb.target, target); // set probe move endpoint - copy_vector(pb.flags, flags); // set axes involved on the move - clear_vector(pb.results); // clear the old probe position. - // NOTE: relying on pb.results will not detect a probe to (0, 0, 0). - - // wait until planner queue empties before completing initialization - pb.state = PROBE_WAITING; - pb.func = _probing_init; // bind probing initialization function - - return STAT_OK; -} - - -/// main loop callback for running the homing cycle -void mach_probe_callback() { - // sync to planner move ends - if (!mach_is_probing() || mp_get_state() != STATE_READY) return; - - pb.func(); // execute the current homing move -} diff --git a/avr/src/probing.h b/avr/src/probing.h deleted file mode 100644 index 3bb5ef7..0000000 --- a/avr/src/probing.h +++ /dev/null @@ -1,37 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - - -bool mach_is_probing(); -stat_t mach_probe(float target[], bool flags[]); -void mach_probe_callback(); diff --git a/avr/src/switch.c b/avr/src/switch.c index 4fb4dce..91a29fa 100644 --- a/avr/src/switch.c +++ b/avr/src/switch.c @@ -147,7 +147,7 @@ void switch_rtc_callback() { // state is either lockout or deglitching if (++s->count == SW_LOCKOUT_TICKS) { - PORT(s->pin)->INT0MASK |= BM(s->pin); // Renable INT0 + PORT(s->pin)->INT0MASK |= BM(s->pin); // Reenable INT0 bool state = _read_state(s); s->debounce = SW_IDLE; @@ -201,23 +201,28 @@ void switch_set_callback(int index, switch_callback_t cb) { // Var callbacks -uint8_t get_min_switch(int index) {return switch_get_type(MIN_SWITCH(index));} +uint8_t get_min_sw_mode(int index) {return switch_get_type(MIN_SWITCH(index));} -void set_min_switch(int index, uint8_t value) { +void set_min_sw_mode(int index, uint8_t value) { switch_set_type(MIN_SWITCH(index), value); } -uint8_t get_max_switch(int index) {return switch_get_type(MAX_SWITCH(index));} +uint8_t get_max_sw_mode(int index) {return switch_get_type(MAX_SWITCH(index));} -void set_max_switch(int index, uint8_t value) { +void set_max_sw_mode(int index, uint8_t value) { switch_set_type(MAX_SWITCH(index), value); } -uint8_t get_estop_switch() {return switch_get_type(8);} -void set_estop_switch(uint8_t value) {switch_set_type(8, value);} -uint8_t get_probe_switch() {return switch_get_type(9);} -void set_probe_switch(uint8_t value) {switch_set_type(9, value);} +uint8_t get_estop_mode() {return switch_get_type(SW_ESTOP);} +void set_estop_mode(uint8_t value) {switch_set_type(SW_ESTOP, value);} +uint8_t get_probe_mode() {return switch_get_type(SW_PROBE);} +void set_probe_mode(uint8_t value) {switch_set_type(SW_PROBE, value);} + +bool get_min_switch(int index) {return switch_is_active(MIN_SWITCH(index));} +bool get_max_switch(int index) {return switch_is_active(MAX_SWITCH(index));} +bool get_estop_switch() {return switch_is_active(SW_ESTOP);} +bool get_probe_switch() {return switch_is_active(SW_PROBE);} diff --git a/avr/src/vars.c b/avr/src/vars.c index 05d273d..c05a476 100644 --- a/avr/src/vars.c +++ b/avr/src/vars.c @@ -31,6 +31,7 @@ #include "status.h" #include "hardware.h" #include "config.h" +#include "axis.h" #include "pgmspace.h" #include @@ -73,11 +74,12 @@ static void var_print_string(string s) {printf_P(PSTR("\"%s\""), s);} // Program string static void var_print_pstring(pstring s) {printf_P(PSTR("\"%"PRPSTR"\""), s);} static const char *var_parse_pstring(const char *value) {return value;} +static float var_pstring_to_float(pstring s) {return 0;} // Flags -static void var_print_flags_t(uint16_t x) { - extern void print_status_flags(uint16_t x); +static void var_print_flags_t(flags_t x) { + extern void print_status_flags(flags_t x); print_status_flags(x); } @@ -113,9 +115,8 @@ static void var_print_float(float x) { } -static float var_parse_float(const char *value) { - return strtod(value, 0); -} +static float var_parse_float(const char *value) {return strtod(value, 0);} +static float var_float_to_float(float x) {return x;} // Bool @@ -127,11 +128,14 @@ bool var_parse_bool(const char *value) { return !strcasecmp(value, "true") || var_parse_float(value); } +static float var_bool_to_float(bool x) {return x;} + // Char #if 0 static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');} static char var_parse_char(const char *value) {return value[1];} +static float var_char_to_float(char x) {return x;} #endif @@ -142,9 +146,8 @@ static void var_print_int8_t(int8_t x) { } -static int8_t var_parse_int8_t(const char *value) { - return strtol(value, 0, 0); -} +static int8_t var_parse_int8_t(const char *value) {return strtol(value, 0, 0);} +static float var_int8_t_to_float(int8_t x) {return x;} #endif // uint8 @@ -155,6 +158,8 @@ static uint8_t var_parse_uint8_t(const char *value) { return strtol(value, 0, 0); } +static float var_uint8_t_to_float(uint8_t x) {return x;} + // unit16 static void var_print_uint16_t(uint16_t x) { @@ -167,10 +172,11 @@ static uint16_t var_parse_uint16_t(const char *value) { } +static float var_uint16_t_to_float(uint16_t x) {return x;} + + // int32 -static void var_print_int32_t(uint32_t x) { - printf_P(PSTR("%"PRIi32), x); -} +static void var_print_int32_t(int32_t x) {printf_P(PSTR("%"PRIi32), x);} // Ensure no code is used more than once @@ -301,45 +307,42 @@ void vars_report_var(const char *code, bool enable) { } -int vars_find(const char *name) { - uint8_t i = 0; - uint8_t n = 0; - unsigned len = strlen(name); +static char *_resolve_name(const char *_name) { + unsigned len = strlen(_name); - if (!len) return -1; + if (!len || 4 < len) return 0; -#define VAR(NAME, CODE, TYPE, INDEX, ...) \ - if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ - IF(INDEX) \ - (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ - if (INDEX <= i) return -1); \ - return n; \ - } \ - n++; + static char name[4]; + strcpy(name, _name); -#include "vars.def" -#undef VAR + // Handle axis to motor mapping + if (2 < len && name[1] == '.') { + int axis = axis_get_id(name[0]); + if (axis < 0) return 0; + int motor = axis_get_motor(axis); + if (motor < 0) return 0; - return -1; -} + name[0] = MOTORS_LABEL[motor]; + for (int i = 1; _name[i]; i++) + name[i] = _name[i + 1]; + } + return name; +} -bool vars_print(const char *name) { - uint8_t i; - unsigned len = strlen(name); - if (!len) return false; +bool vars_print(const char *_name) { + char *name = _resolve_name(_name); + if (!name) return false; + int i; #define VAR(NAME, CODE, TYPE, INDEX, ...) \ if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ IF(INDEX) \ (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ if (INDEX <= i) return false); \ \ - putchar('{'); \ - printf_P \ - (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \ - IF(INDEX)(INDEX##_LABEL[i],) #CODE); \ + printf("{\"%s\":", _name); \ var_print_##TYPE(get_##NAME(IF(INDEX)(i))); \ putchar('}'); \ \ @@ -353,12 +356,11 @@ bool vars_print(const char *name) { } -bool vars_set(const char *name, const char *value) { - uint8_t i; - unsigned len = strlen(name); - - if (!len) return false; +bool vars_set(const char *_name, const char *value) { + char *name = _resolve_name(_name); + if (!name) return false; + int i; #define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ IF(SET) \ (if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ @@ -379,6 +381,29 @@ bool vars_set(const char *name, const char *value) { } +float vars_get_number(const char *_name) { + char *name = _resolve_name(_name); + if (!name) return 0; + + int i; +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + IF(SET) \ + (if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ + if (INDEX <= i) return 0); \ + \ + TYPE x = get_##NAME(IF(INDEX)(i)); \ + return var_##TYPE##_to_float(x); \ + }) \ + +#include "vars.def" +#undef VAR + + return 0; +} + + int vars_parser(char *vars) { if (!*vars || *vars != '{') return STAT_OK; vars++; diff --git a/avr/src/vars.def b/avr/src/vars.def index 15e88a2..f8878f1 100644 --- a/avr/src/vars.def +++ b/avr/src/vars.def @@ -55,10 +55,15 @@ VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero") // Switches VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit") VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit") -VAR(min_switch, ls, uint8_t, MOTORS, 1, 1, "Minimum switch mode") -VAR(max_switch, xs, uint8_t, MOTORS, 1, 1, "Maximum switch mode") -VAR(estop_switch, et, uint8_t, 0, 1, 1, "Estop switch mode") -VAR(probe_switch, pt, uint8_t, 0, 1, 1, "Probe switch mode") +VAR(min_sw_mode, ls, uint8_t, MOTORS, 1, 1, "Minimum switch mode") +VAR(max_sw_mode, xs, uint8_t, MOTORS, 1, 1, "Maximum switch mode") +VAR(estop_mode, et, uint8_t, 0, 1, 1, "Estop switch mode") +VAR(probe_mode, pt, uint8_t, 0, 1, 1, "Probe switch mode") + +VAR(min_switch, lw, bool, MOTORS, 0, 1, "Minimum switch state") +VAR(max_switch, xw, bool, MOTORS, 0, 1, "Maximum switch state") +VAR(estop_switch, ew, bool, 0, 0, 1, "Estop switch state") +VAR(probe_switch, pw, bool, 0, 0, 1, "Probe switch state") // Homing VAR(homing_mode, ho, uint8_t, MOTORS, 1, 1, "Homing type") diff --git a/avr/src/vars.h b/avr/src/vars.h index fda234f..4ad37b7 100644 --- a/avr/src/vars.h +++ b/avr/src/vars.h @@ -38,8 +38,8 @@ void vars_init(); void vars_report(bool full); void vars_report_all(bool enable); void vars_report_var(const char *code, bool enable); -int vars_find(const char *name); bool vars_print(const char *name); bool vars_set(const char *name, const char *value); +float vars_get_number(const char *name); int vars_parser(char *vars); void vars_print_help();