Sergey Pastor / grbl1

grbl/gcode.c@0:8f0d870509fe, 2017-09-04 (annotated)

Committer:
Sergunb
Date:
Mon Sep 04 12:04:13 2017 +0000
Revision:
0:8f0d870509fe
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sergunb 0:8f0d870509fe 1 /*
Sergunb 0:8f0d870509fe 2 gcode.c - rs274/ngc parser.
Sergunb 0:8f0d870509fe 3 Part of Grbl
Sergunb 0:8f0d870509fe 4
Sergunb 0:8f0d870509fe 5 Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
Sergunb 0:8f0d870509fe 6 Copyright (c) 2009-2011 Simen Svale Skogsrud
Sergunb 0:8f0d870509fe 7
Sergunb 0:8f0d870509fe 8 Grbl is free software: you can redistribute it and/or modify
Sergunb 0:8f0d870509fe 9 it under the terms of the GNU General Public License as published by
Sergunb 0:8f0d870509fe 10 the Free Software Foundation, either version 3 of the License, or
Sergunb 0:8f0d870509fe 11 (at your option) any later version.
Sergunb 0:8f0d870509fe 12
Sergunb 0:8f0d870509fe 13 Grbl is distributed in the hope that it will be useful,
Sergunb 0:8f0d870509fe 14 but WITHOUT ANY WARRANTY; without even the implied warranty of
Sergunb 0:8f0d870509fe 15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
Sergunb 0:8f0d870509fe 16 GNU General Public License for more details.
Sergunb 0:8f0d870509fe 17
Sergunb 0:8f0d870509fe 18 You should have received a copy of the GNU General Public License
Sergunb 0:8f0d870509fe 19 along with Grbl. If not, see <http://www.gnu.org/licenses/>.
Sergunb 0:8f0d870509fe 20 */
Sergunb 0:8f0d870509fe 21
Sergunb 0:8f0d870509fe 22 #include "grbl.h"
Sergunb 0:8f0d870509fe 23
Sergunb 0:8f0d870509fe 24 // NOTE: Max line number is defined by the g-code standard to be 99999. It seems to be an
Sergunb 0:8f0d870509fe 25 // arbitrary value, and some GUIs may require more. So we increased it based on a max safe
Sergunb 0:8f0d870509fe 26 // value when converting a float (7.2 digit precision)s to an integer.
Sergunb 0:8f0d870509fe 27 #define MAX_LINE_NUMBER 10000000
Sergunb 0:8f0d870509fe 28 #define MAX_TOOL_NUMBER 255 // Limited by max unsigned 8-bit value
Sergunb 0:8f0d870509fe 29
Sergunb 0:8f0d870509fe 30 #define AXIS_COMMAND_NONE 0
Sergunb 0:8f0d870509fe 31 #define AXIS_COMMAND_NON_MODAL 1
Sergunb 0:8f0d870509fe 32 #define AXIS_COMMAND_MOTION_MODE 2
Sergunb 0:8f0d870509fe 33 #define AXIS_COMMAND_TOOL_LENGTH_OFFSET 3 // *Undefined but required
Sergunb 0:8f0d870509fe 34
Sergunb 0:8f0d870509fe 35 // Declare gc extern struct
Sergunb 0:8f0d870509fe 36 parser_state_t gc_state;
Sergunb 0:8f0d870509fe 37 parser_block_t gc_block;
Sergunb 0:8f0d870509fe 38
Sergunb 0:8f0d870509fe 39 #define FAIL(status) return(status);
Sergunb 0:8f0d870509fe 40
Sergunb 0:8f0d870509fe 41
Sergunb 0:8f0d870509fe 42 void gc_init()
Sergunb 0:8f0d870509fe 43 {
Sergunb 0:8f0d870509fe 44 memset(&gc_state, 0, sizeof(parser_state_t));
Sergunb 0:8f0d870509fe 45
Sergunb 0:8f0d870509fe 46 // Load default G54 coordinate system.
Sergunb 0:8f0d870509fe 47 if (!(settings_read_coord_data(gc_state.modal.coord_select,gc_state.coord_system))) {
Sergunb 0:8f0d870509fe 48 report_status_message(STATUS_SETTING_READ_FAIL);
Sergunb 0:8f0d870509fe 49 }
Sergunb 0:8f0d870509fe 50 }
Sergunb 0:8f0d870509fe 51
Sergunb 0:8f0d870509fe 52
Sergunb 0:8f0d870509fe 53 // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard
Sergunb 0:8f0d870509fe 54 // limit pull-off routines.
Sergunb 0:8f0d870509fe 55 void gc_sync_position()
Sergunb 0:8f0d870509fe 56 {
Sergunb 0:8f0d870509fe 57 system_convert_array_steps_to_mpos(gc_state.position,sys_position);
Sergunb 0:8f0d870509fe 58 }
Sergunb 0:8f0d870509fe 59
Sergunb 0:8f0d870509fe 60
Sergunb 0:8f0d870509fe 61 // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
Sergunb 0:8f0d870509fe 62 // characters and signed floating point values (no whitespace). Comments and block delete
Sergunb 0:8f0d870509fe 63 // characters have been removed. In this function, all units and positions are converted and
Sergunb 0:8f0d870509fe 64 // exported to grbl's internal functions in terms of (mm, mm/min) and absolute machine
Sergunb 0:8f0d870509fe 65 // coordinates, respectively.
Sergunb 0:8f0d870509fe 66 uint8_t gc_execute_line(char *line)
Sergunb 0:8f0d870509fe 67 {
Sergunb 0:8f0d870509fe 68 /* -------------------------------------------------------------------------------------
Sergunb 0:8f0d870509fe 69 STEP 1: Initialize parser block struct and copy current g-code state modes. The parser
Sergunb 0:8f0d870509fe 70 updates these modes and commands as the block line is parser and will only be used and
Sergunb 0:8f0d870509fe 71 executed after successful error-checking. The parser block struct also contains a block
Sergunb 0:8f0d870509fe 72 values struct, word tracking variables, and a non-modal commands tracker for the new
Sergunb 0:8f0d870509fe 73 block. This struct contains all of the necessary information to execute the block. */
Sergunb 0:8f0d870509fe 74
Sergunb 0:8f0d870509fe 75 memset(&gc_block, 0, sizeof(parser_block_t)); // Initialize the parser block struct.
Sergunb 0:8f0d870509fe 76 memcpy(&gc_block.modal,&gc_state.modal,sizeof(gc_modal_t)); // Copy current modes
Sergunb 0:8f0d870509fe 77
Sergunb 0:8f0d870509fe 78 uint8_t axis_command = AXIS_COMMAND_NONE;
Sergunb 0:8f0d870509fe 79 uint8_t axis_0, axis_1, axis_linear;
Sergunb 0:8f0d870509fe 80 uint8_t coord_select = 0; // Tracks G10 P coordinate selection for execution
Sergunb 0:8f0d870509fe 81
Sergunb 0:8f0d870509fe 82 // Initialize bitflag tracking variables for axis indices compatible operations.
Sergunb 0:8f0d870509fe 83 uint8_t axis_words = 0; // XYZ tracking
Sergunb 0:8f0d870509fe 84 uint8_t ijk_words = 0; // IJK tracking
Sergunb 0:8f0d870509fe 85
Sergunb 0:8f0d870509fe 86 // Initialize command and value words and parser flags variables.
Sergunb 0:8f0d870509fe 87 uint16_t command_words = 0; // Tracks G and M command words. Also used for modal group violations.
Sergunb 0:8f0d870509fe 88 uint16_t value_words = 0; // Tracks value words.
Sergunb 0:8f0d870509fe 89 uint8_t gc_parser_flags = GC_PARSER_NONE;
Sergunb 0:8f0d870509fe 90
Sergunb 0:8f0d870509fe 91 // Determine if the line is a jogging motion or a normal g-code block.
Sergunb 0:8f0d870509fe 92 if (line[0] == '$') { // NOTE: `$J=` already parsed when passed to this function.
Sergunb 0:8f0d870509fe 93 // Set G1 and G94 enforced modes to ensure accurate error checks.
Sergunb 0:8f0d870509fe 94 gc_parser_flags |= GC_PARSER_JOG_MOTION;
Sergunb 0:8f0d870509fe 95 gc_block.modal.motion = MOTION_MODE_LINEAR;
Sergunb 0:8f0d870509fe 96 gc_block.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN;
Sergunb 0:8f0d870509fe 97 #ifdef USE_LINE_NUMBERS
Sergunb 0:8f0d870509fe 98 gc_block.values.n = JOG_LINE_NUMBER; // Initialize default line number reported during jog.
Sergunb 0:8f0d870509fe 99 #endif
Sergunb 0:8f0d870509fe 100 }
Sergunb 0:8f0d870509fe 101
Sergunb 0:8f0d870509fe 102 /* -------------------------------------------------------------------------------------
Sergunb 0:8f0d870509fe 103 STEP 2: Import all g-code words in the block line. A g-code word is a letter followed by
Sergunb 0:8f0d870509fe 104 a number, which can either be a 'G'/'M' command or sets/assigns a command value. Also,
Sergunb 0:8f0d870509fe 105 perform initial error-checks for command word modal group violations, for any repeated
Sergunb 0:8f0d870509fe 106 words, and for negative values set for the value words F, N, P, T, and S. */
Sergunb 0:8f0d870509fe 107
Sergunb 0:8f0d870509fe 108 uint8_t word_bit; // Bit-value for assigning tracking variables
Sergunb 0:8f0d870509fe 109 uint8_t char_counter;
Sergunb 0:8f0d870509fe 110 char letter;
Sergunb 0:8f0d870509fe 111 float value;
Sergunb 0:8f0d870509fe 112 uint8_t int_value = 0;
Sergunb 0:8f0d870509fe 113 uint16_t mantissa = 0;
Sergunb 0:8f0d870509fe 114 if (gc_parser_flags & GC_PARSER_JOG_MOTION) { char_counter = 3; } // Start parsing after `$J=`
Sergunb 0:8f0d870509fe 115 else { char_counter = 0; }
Sergunb 0:8f0d870509fe 116
Sergunb 0:8f0d870509fe 117 while (line[char_counter] != 0) { // Loop until no more g-code words in line.
Sergunb 0:8f0d870509fe 118
Sergunb 0:8f0d870509fe 119 // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out.
Sergunb 0:8f0d870509fe 120 letter = line[char_counter];
Sergunb 0:8f0d870509fe 121 if((letter < 'A') || (letter > 'Z')) { FAIL(STATUS_EXPECTED_COMMAND_LETTER); } // [Expected word letter]
Sergunb 0:8f0d870509fe 122 char_counter++;
Sergunb 0:8f0d870509fe 123 if (!read_float(line, &char_counter, &value)) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // [Expected word value]
Sergunb 0:8f0d870509fe 124
Sergunb 0:8f0d870509fe 125 // Convert values to smaller uint8 significand and mantissa values for parsing this word.
Sergunb 0:8f0d870509fe 126 // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more
Sergunb 0:8f0d870509fe 127 // accurate than the NIST gcode requirement of x10 when used for commands, but not quite
Sergunb 0:8f0d870509fe 128 // accurate enough for value words that require integers to within 0.0001. This should be
Sergunb 0:8f0d870509fe 129 // a good enough comprimise and catch most all non-integer errors. To make it compliant,
Sergunb 0:8f0d870509fe 130 // we would simply need to change the mantissa to int16, but this add compiled flash space.
Sergunb 0:8f0d870509fe 131 // Maybe update this later.
Sergunb 0:8f0d870509fe 132 int_value = truncf(value);
Sergunb 0:8f0d870509fe 133 mantissa = (uint16_t)lroundf(100 * (value - int_value)); // Compute mantissa for Gxx.x commands.
Sergunb 0:8f0d870509fe 134 // NOTE: Rounding must be used to catch small floating point errors.
Sergunb 0:8f0d870509fe 135
Sergunb 0:8f0d870509fe 136 // Check if the g-code word is supported or errors due to modal group violations or has
Sergunb 0:8f0d870509fe 137 // been repeated in the g-code block. If ok, update the command or record its value.
Sergunb 0:8f0d870509fe 138 switch(letter) {
Sergunb 0:8f0d870509fe 139
Sergunb 0:8f0d870509fe 140 /* 'G' and 'M' Command Words: Parse commands and check for modal group violations.
Sergunb 0:8f0d870509fe 141 NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
Sergunb 0:8f0d870509fe 142
Sergunb 0:8f0d870509fe 143 case 'G':
Sergunb 0:8f0d870509fe 144 // Determine 'G' command and its modal group
Sergunb 0:8f0d870509fe 145 switch(int_value) {
Sergunb 0:8f0d870509fe 146 case 10: case 28: case 30: case 92:
Sergunb 0:8f0d870509fe 147 // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block.
Sergunb 0:8f0d870509fe 148 // * G43.1 is also an axis command but is not explicitly defined this way.
Sergunb 0:8f0d870509fe 149 if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1
Sergunb 0:8f0d870509fe 150 if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict]
Sergunb 0:8f0d870509fe 151 axis_command = AXIS_COMMAND_NON_MODAL;
Sergunb 0:8f0d870509fe 152 }
Sergunb 0:8f0d870509fe 153 // No break. Continues to next line.
Sergunb 0:8f0d870509fe 154 case 4: case 53:
Sergunb 0:8f0d870509fe 155 word_bit = MODAL_GROUP_G0;
Sergunb 0:8f0d870509fe 156 gc_block.non_modal_command = int_value;
Sergunb 0:8f0d870509fe 157 if ((int_value == 28) || (int_value == 30) || (int_value == 92)) {
Sergunb 0:8f0d870509fe 158 if (!((mantissa == 0) || (mantissa == 10))) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); }
Sergunb 0:8f0d870509fe 159 gc_block.non_modal_command += mantissa;
Sergunb 0:8f0d870509fe 160 mantissa = 0; // Set to zero to indicate valid non-integer G command.
Sergunb 0:8f0d870509fe 161 }
Sergunb 0:8f0d870509fe 162 break;
Sergunb 0:8f0d870509fe 163 case 0: case 1: case 2: case 3: case 38:
Sergunb 0:8f0d870509fe 164 // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block.
Sergunb 0:8f0d870509fe 165 // * G43.1 is also an axis command but is not explicitly defined this way.
Sergunb 0:8f0d870509fe 166 if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict]
Sergunb 0:8f0d870509fe 167 axis_command = AXIS_COMMAND_MOTION_MODE;
Sergunb 0:8f0d870509fe 168 // No break. Continues to next line.
Sergunb 0:8f0d870509fe 169 case 80:
Sergunb 0:8f0d870509fe 170 word_bit = MODAL_GROUP_G1;
Sergunb 0:8f0d870509fe 171 gc_block.modal.motion = int_value;
Sergunb 0:8f0d870509fe 172 if (int_value == 38){
Sergunb 0:8f0d870509fe 173 if (!((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50))) {
Sergunb 0:8f0d870509fe 174 FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G38.x command]
Sergunb 0:8f0d870509fe 175 }
Sergunb 0:8f0d870509fe 176 gc_block.modal.motion += (mantissa/10)+100;
Sergunb 0:8f0d870509fe 177 mantissa = 0; // Set to zero to indicate valid non-integer G command.
Sergunb 0:8f0d870509fe 178 }
Sergunb 0:8f0d870509fe 179 break;
Sergunb 0:8f0d870509fe 180 case 17: case 18: case 19:
Sergunb 0:8f0d870509fe 181 word_bit = MODAL_GROUP_G2;
Sergunb 0:8f0d870509fe 182 gc_block.modal.plane_select = int_value - 17;
Sergunb 0:8f0d870509fe 183 break;
Sergunb 0:8f0d870509fe 184 case 90: case 91:
Sergunb 0:8f0d870509fe 185 if (mantissa == 0) {
Sergunb 0:8f0d870509fe 186 word_bit = MODAL_GROUP_G3;
Sergunb 0:8f0d870509fe 187 gc_block.modal.distance = int_value - 90;
Sergunb 0:8f0d870509fe 188 } else {
Sergunb 0:8f0d870509fe 189 word_bit = MODAL_GROUP_G4;
Sergunb 0:8f0d870509fe 190 if ((mantissa != 10) || (int_value == 90)) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G90.1 not supported]
Sergunb 0:8f0d870509fe 191 mantissa = 0; // Set to zero to indicate valid non-integer G command.
Sergunb 0:8f0d870509fe 192 // Otherwise, arc IJK incremental mode is default. G91.1 does nothing.
Sergunb 0:8f0d870509fe 193 }
Sergunb 0:8f0d870509fe 194 break;
Sergunb 0:8f0d870509fe 195 case 93: case 94:
Sergunb 0:8f0d870509fe 196 word_bit = MODAL_GROUP_G5;
Sergunb 0:8f0d870509fe 197 gc_block.modal.feed_rate = 94 - int_value;
Sergunb 0:8f0d870509fe 198 break;
Sergunb 0:8f0d870509fe 199 case 20: case 21:
Sergunb 0:8f0d870509fe 200 word_bit = MODAL_GROUP_G6;
Sergunb 0:8f0d870509fe 201 gc_block.modal.units = 21 - int_value;
Sergunb 0:8f0d870509fe 202 break;
Sergunb 0:8f0d870509fe 203 case 40:
Sergunb 0:8f0d870509fe 204 word_bit = MODAL_GROUP_G7;
Sergunb 0:8f0d870509fe 205 // NOTE: Not required since cutter radius compensation is always disabled. Only here
Sergunb 0:8f0d870509fe 206 // to support G40 commands that often appear in g-code program headers to setup defaults.
Sergunb 0:8f0d870509fe 207 // gc_block.modal.cutter_comp = CUTTER_COMP_DISABLE; // G40
Sergunb 0:8f0d870509fe 208 break;
Sergunb 0:8f0d870509fe 209 case 43: case 49:
Sergunb 0:8f0d870509fe 210 word_bit = MODAL_GROUP_G8;
Sergunb 0:8f0d870509fe 211 // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed,
Sergunb 0:8f0d870509fe 212 // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49
Sergunb 0:8f0d870509fe 213 // all are explicit axis commands, regardless if they require axis words or not.
Sergunb 0:8f0d870509fe 214 if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] }
Sergunb 0:8f0d870509fe 215 axis_command = AXIS_COMMAND_TOOL_LENGTH_OFFSET;
Sergunb 0:8f0d870509fe 216 if (int_value == 49) { // G49
Sergunb 0:8f0d870509fe 217 gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_CANCEL;
Sergunb 0:8f0d870509fe 218 } else if (mantissa == 10) { // G43.1
Sergunb 0:8f0d870509fe 219 gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC;
Sergunb 0:8f0d870509fe 220 } else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported G43.x command]
Sergunb 0:8f0d870509fe 221 mantissa = 0; // Set to zero to indicate valid non-integer G command.
Sergunb 0:8f0d870509fe 222 break;
Sergunb 0:8f0d870509fe 223 case 54: case 55: case 56: case 57: case 58: case 59:
Sergunb 0:8f0d870509fe 224 // NOTE: G59.x are not supported. (But their int_values would be 60, 61, and 62.)
Sergunb 0:8f0d870509fe 225 word_bit = MODAL_GROUP_G12;
Sergunb 0:8f0d870509fe 226 gc_block.modal.coord_select = int_value - 54; // Shift to array indexing.
Sergunb 0:8f0d870509fe 227 break;
Sergunb 0:8f0d870509fe 228 case 61:
Sergunb 0:8f0d870509fe 229 word_bit = MODAL_GROUP_G13;
Sergunb 0:8f0d870509fe 230 if (mantissa != 0) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G61.1 not supported]
Sergunb 0:8f0d870509fe 231 // gc_block.modal.control = CONTROL_MODE_EXACT_PATH; // G61
Sergunb 0:8f0d870509fe 232 break;
Sergunb 0:8f0d870509fe 233 default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G command]
Sergunb 0:8f0d870509fe 234 }
Sergunb 0:8f0d870509fe 235 if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [Unsupported or invalid Gxx.x command]
Sergunb 0:8f0d870509fe 236 // Check for more than one command per modal group violations in the current block
Sergunb 0:8f0d870509fe 237 // NOTE: Variable 'word_bit' is always assigned, if the command is valid.
Sergunb 0:8f0d870509fe 238 if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); }
Sergunb 0:8f0d870509fe 239 command_words |= bit(word_bit);
Sergunb 0:8f0d870509fe 240 break;
Sergunb 0:8f0d870509fe 241
Sergunb 0:8f0d870509fe 242 case 'M':
Sergunb 0:8f0d870509fe 243
Sergunb 0:8f0d870509fe 244 // Determine 'M' command and its modal group
Sergunb 0:8f0d870509fe 245 if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [No Mxx.x commands]
Sergunb 0:8f0d870509fe 246 switch(int_value) {
Sergunb 0:8f0d870509fe 247 case 0: case 1: case 2: case 30:
Sergunb 0:8f0d870509fe 248 word_bit = MODAL_GROUP_M4;
Sergunb 0:8f0d870509fe 249 switch(int_value) {
Sergunb 0:8f0d870509fe 250 case 0: gc_block.modal.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
Sergunb 0:8f0d870509fe 251 case 1: break; // Optional stop not supported. Ignore.
Sergunb 0:8f0d870509fe 252 default: gc_block.modal.program_flow = int_value; // Program end and reset
Sergunb 0:8f0d870509fe 253 }
Sergunb 0:8f0d870509fe 254 break;
Sergunb 0:8f0d870509fe 255 case 3: case 4: case 5:
Sergunb 0:8f0d870509fe 256 word_bit = MODAL_GROUP_M7;
Sergunb 0:8f0d870509fe 257 switch(int_value) {
Sergunb 0:8f0d870509fe 258 case 3: gc_block.modal.spindle = SPINDLE_ENABLE_CW; break;
Sergunb 0:8f0d870509fe 259 case 4: gc_block.modal.spindle = SPINDLE_ENABLE_CCW; break;
Sergunb 0:8f0d870509fe 260 case 5: gc_block.modal.spindle = SPINDLE_DISABLE; break;
Sergunb 0:8f0d870509fe 261 }
Sergunb 0:8f0d870509fe 262 break;
Sergunb 0:8f0d870509fe 263 #ifdef ENABLE_M7
Sergunb 0:8f0d870509fe 264 case 7: case 8: case 9:
Sergunb 0:8f0d870509fe 265 #else
Sergunb 0:8f0d870509fe 266 case 8: case 9:
Sergunb 0:8f0d870509fe 267 #endif
Sergunb 0:8f0d870509fe 268 word_bit = MODAL_GROUP_M8;
Sergunb 0:8f0d870509fe 269 switch(int_value) {
Sergunb 0:8f0d870509fe 270 #ifdef ENABLE_M7
Sergunb 0:8f0d870509fe 271 case 7: gc_block.modal.coolant = COOLANT_MIST_ENABLE; break;
Sergunb 0:8f0d870509fe 272 #endif
Sergunb 0:8f0d870509fe 273 case 8: gc_block.modal.coolant = COOLANT_FLOOD_ENABLE; break;
Sergunb 0:8f0d870509fe 274 case 9: gc_block.modal.coolant = COOLANT_DISABLE; break;
Sergunb 0:8f0d870509fe 275 }
Sergunb 0:8f0d870509fe 276 break;
Sergunb 0:8f0d870509fe 277 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
Sergunb 0:8f0d870509fe 278 case 56:
Sergunb 0:8f0d870509fe 279 word_bit = MODAL_GROUP_M9;
Sergunb 0:8f0d870509fe 280 gc_block.modal.override = OVERRIDE_PARKING_MOTION;
Sergunb 0:8f0d870509fe 281 break;
Sergunb 0:8f0d870509fe 282 #endif
Sergunb 0:8f0d870509fe 283 default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command]
Sergunb 0:8f0d870509fe 284 }
Sergunb 0:8f0d870509fe 285
Sergunb 0:8f0d870509fe 286 // Check for more than one command per modal group violations in the current block
Sergunb 0:8f0d870509fe 287 // NOTE: Variable 'word_bit' is always assigned, if the command is valid.
Sergunb 0:8f0d870509fe 288 if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); }
Sergunb 0:8f0d870509fe 289 command_words |= bit(word_bit);
Sergunb 0:8f0d870509fe 290 break;
Sergunb 0:8f0d870509fe 291
Sergunb 0:8f0d870509fe 292 // NOTE: All remaining letters assign values.
Sergunb 0:8f0d870509fe 293 default:
Sergunb 0:8f0d870509fe 294
Sergunb 0:8f0d870509fe 295 /* Non-Command Words: This initial parsing phase only checks for repeats of the remaining
Sergunb 0:8f0d870509fe 296 legal g-code words and stores their value. Error-checking is performed later since some
Sergunb 0:8f0d870509fe 297 words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */
Sergunb 0:8f0d870509fe 298 switch(letter){
Sergunb 0:8f0d870509fe 299 // case 'A': // Not supported
Sergunb 0:8f0d870509fe 300 // case 'B': // Not supported
Sergunb 0:8f0d870509fe 301 // case 'C': // Not supported
Sergunb 0:8f0d870509fe 302 // case 'D': // Not supported
Sergunb 0:8f0d870509fe 303 case 'F': word_bit = WORD_F; gc_block.values.f = value; break;
Sergunb 0:8f0d870509fe 304 // case 'H': // Not supported
Sergunb 0:8f0d870509fe 305 case 'I': word_bit = WORD_I; gc_block.values.ijk[X_AXIS] = value; ijk_words |= (1<<X_AXIS); break;
Sergunb 0:8f0d870509fe 306 case 'J': word_bit = WORD_J; gc_block.values.ijk[Y_AXIS] = value; ijk_words |= (1<<Y_AXIS); break;
Sergunb 0:8f0d870509fe 307 case 'K': word_bit = WORD_K; gc_block.values.ijk[Z_AXIS] = value; ijk_words |= (1<<Z_AXIS); break;
Sergunb 0:8f0d870509fe 308 case 'L': word_bit = WORD_L; gc_block.values.l = int_value; break;
Sergunb 0:8f0d870509fe 309 case 'N': word_bit = WORD_N; gc_block.values.n = truncf(value); break;
Sergunb 0:8f0d870509fe 310 case 'P': word_bit = WORD_P; gc_block.values.p = value; break;
Sergunb 0:8f0d870509fe 311 // NOTE: For certain commands, P value must be an integer, but none of these commands are supported.
Sergunb 0:8f0d870509fe 312 // case 'Q': // Not supported
Sergunb 0:8f0d870509fe 313 case 'R': word_bit = WORD_R; gc_block.values.r = value; break;
Sergunb 0:8f0d870509fe 314 case 'S': word_bit = WORD_S; gc_block.values.s = value; break;
Sergunb 0:8f0d870509fe 315 case 'T': word_bit = WORD_T;
Sergunb 0:8f0d870509fe 316 if (value > MAX_TOOL_NUMBER) { FAIL(STATUS_GCODE_MAX_VALUE_EXCEEDED); }
Sergunb 0:8f0d870509fe 317 gc_block.values.t = int_value;
Sergunb 0:8f0d870509fe 318 break;
Sergunb 0:8f0d870509fe 319 case 'X': word_bit = WORD_X; gc_block.values.xyz[X_AXIS] = value; axis_words |= (1<<X_AXIS); break;
Sergunb 0:8f0d870509fe 320 case 'Y': word_bit = WORD_Y; gc_block.values.xyz[Y_AXIS] = value; axis_words |= (1<<Y_AXIS); break;
Sergunb 0:8f0d870509fe 321 case 'Z': word_bit = WORD_Z; gc_block.values.xyz[Z_AXIS] = value; axis_words |= (1<<Z_AXIS); break;
Sergunb 0:8f0d870509fe 322 default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND);
Sergunb 0:8f0d870509fe 323 }
Sergunb 0:8f0d870509fe 324
Sergunb 0:8f0d870509fe 325 // NOTE: Variable 'word_bit' is always assigned, if the non-command letter is valid.
Sergunb 0:8f0d870509fe 326 if (bit_istrue(value_words,bit(word_bit))) { FAIL(STATUS_GCODE_WORD_REPEATED); } // [Word repeated]
Sergunb 0:8f0d870509fe 327 // Check for invalid negative values for words F, N, P, T, and S.
Sergunb 0:8f0d870509fe 328 // NOTE: Negative value check is done here simply for code-efficiency.
Sergunb 0:8f0d870509fe 329 if ( bit(word_bit) & (bit(WORD_F)|bit(WORD_N)|bit(WORD_P)|bit(WORD_T)|bit(WORD_S)) ) {
Sergunb 0:8f0d870509fe 330 if (value < 0.0) { FAIL(STATUS_NEGATIVE_VALUE); } // [Word value cannot be negative]
Sergunb 0:8f0d870509fe 331 }
Sergunb 0:8f0d870509fe 332 value_words |= bit(word_bit); // Flag to indicate parameter assigned.
Sergunb 0:8f0d870509fe 333
Sergunb 0:8f0d870509fe 334 }
Sergunb 0:8f0d870509fe 335 }
Sergunb 0:8f0d870509fe 336 // Parsing complete!
Sergunb 0:8f0d870509fe 337
Sergunb 0:8f0d870509fe 338
Sergunb 0:8f0d870509fe 339 /* -------------------------------------------------------------------------------------
Sergunb 0:8f0d870509fe 340 STEP 3: Error-check all commands and values passed in this block. This step ensures all of
Sergunb 0:8f0d870509fe 341 the commands are valid for execution and follows the NIST standard as closely as possible.
Sergunb 0:8f0d870509fe 342 If an error is found, all commands and values in this block are dumped and will not update
Sergunb 0:8f0d870509fe 343 the active system g-code modes. If the block is ok, the active system g-code modes will be
Sergunb 0:8f0d870509fe 344 updated based on the commands of this block, and signal for it to be executed.
Sergunb 0:8f0d870509fe 345
Sergunb 0:8f0d870509fe 346 Also, we have to pre-convert all of the values passed based on the modes set by the parsed
Sergunb 0:8f0d870509fe 347 block. There are a number of error-checks that require target information that can only be
Sergunb 0:8f0d870509fe 348 accurately calculated if we convert these values in conjunction with the error-checking.
Sergunb 0:8f0d870509fe 349 This relegates the next execution step as only updating the system g-code modes and
Sergunb 0:8f0d870509fe 350 performing the programmed actions in order. The execution step should not require any
Sergunb 0:8f0d870509fe 351 conversion calculations and would only require minimal checks necessary to execute.
Sergunb 0:8f0d870509fe 352 */
Sergunb 0:8f0d870509fe 353
Sergunb 0:8f0d870509fe 354 /* NOTE: At this point, the g-code block has been parsed and the block line can be freed.
Sergunb 0:8f0d870509fe 355 NOTE: It's also possible, at some future point, to break up STEP 2, to allow piece-wise
Sergunb 0:8f0d870509fe 356 parsing of the block on a per-word basis, rather than the entire block. This could remove
Sergunb 0:8f0d870509fe 357 the need for maintaining a large string variable for the entire block and free up some memory.
Sergunb 0:8f0d870509fe 358 To do this, this would simply need to retain all of the data in STEP 1, such as the new block
Sergunb 0:8f0d870509fe 359 data struct, the modal group and value bitflag tracking variables, and axis array indices
Sergunb 0:8f0d870509fe 360 compatible variables. This data contains all of the information necessary to error-check the
Sergunb 0:8f0d870509fe 361 new g-code block when the EOL character is received. However, this would break Grbl's startup
Sergunb 0:8f0d870509fe 362 lines in how it currently works and would require some refactoring to make it compatible.
Sergunb 0:8f0d870509fe 363 */
Sergunb 0:8f0d870509fe 364
Sergunb 0:8f0d870509fe 365 // [0. Non-specific/common error-checks and miscellaneous setup]:
Sergunb 0:8f0d870509fe 366
Sergunb 0:8f0d870509fe 367 // Determine implicit axis command conditions. Axis words have been passed, but no explicit axis
Sergunb 0:8f0d870509fe 368 // command has been sent. If so, set axis command to current motion mode.
Sergunb 0:8f0d870509fe 369 if (axis_words) {
Sergunb 0:8f0d870509fe 370 if (!axis_command) { axis_command = AXIS_COMMAND_MOTION_MODE; } // Assign implicit motion-mode
Sergunb 0:8f0d870509fe 371 }
Sergunb 0:8f0d870509fe 372
Sergunb 0:8f0d870509fe 373 // Check for valid line number N value.
Sergunb 0:8f0d870509fe 374 if (bit_istrue(value_words,bit(WORD_N))) {
Sergunb 0:8f0d870509fe 375 // Line number value cannot be less than zero (done) or greater than max line number.
Sergunb 0:8f0d870509fe 376 if (gc_block.values.n > MAX_LINE_NUMBER) { FAIL(STATUS_GCODE_INVALID_LINE_NUMBER); } // [Exceeds max line number]
Sergunb 0:8f0d870509fe 377 }
Sergunb 0:8f0d870509fe 378 // bit_false(value_words,bit(WORD_N)); // NOTE: Single-meaning value word. Set at end of error-checking.
Sergunb 0:8f0d870509fe 379
Sergunb 0:8f0d870509fe 380 // Track for unused words at the end of error-checking.
Sergunb 0:8f0d870509fe 381 // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because
Sergunb 0:8f0d870509fe 382 // they are always used when present. This was done to save a few bytes of flash. For clarity, the
Sergunb 0:8f0d870509fe 383 // single-meaning value words may be removed as they are used. Also, axis words are treated in the
Sergunb 0:8f0d870509fe 384 // same way. If there is an explicit/implicit axis command, XYZ words are always used and are
Sergunb 0:8f0d870509fe 385 // are removed at the end of error-checking.
Sergunb 0:8f0d870509fe 386
Sergunb 0:8f0d870509fe 387 // [1. Comments ]: MSG's NOT SUPPORTED. Comment handling performed by protocol.
Sergunb 0:8f0d870509fe 388
Sergunb 0:8f0d870509fe 389 // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate
Sergunb 0:8f0d870509fe 390 // is not defined after switching to G94 from G93.
Sergunb 0:8f0d870509fe 391 // NOTE: For jogging, ignore prior feed rate mode. Enforce G94 and check for required F word.
Sergunb 0:8f0d870509fe 392 if (gc_parser_flags & GC_PARSER_JOG_MOTION) {
Sergunb 0:8f0d870509fe 393 if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); }
Sergunb 0:8f0d870509fe 394 if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.f *= MM_PER_INCH; }
Sergunb 0:8f0d870509fe 395 } else {
Sergunb 0:8f0d870509fe 396 if (gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93
Sergunb 0:8f0d870509fe 397 // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here.
Sergunb 0:8f0d870509fe 398 if (axis_command == AXIS_COMMAND_MOTION_MODE) {
Sergunb 0:8f0d870509fe 399 if ((gc_block.modal.motion != MOTION_MODE_NONE) && (gc_block.modal.motion != MOTION_MODE_SEEK)) {
Sergunb 0:8f0d870509fe 400 if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [F word missing]
Sergunb 0:8f0d870509fe 401 }
Sergunb 0:8f0d870509fe 402 }
Sergunb 0:8f0d870509fe 403 // NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would
Sergunb 0:8f0d870509fe 404 // accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each
Sergunb 0:8f0d870509fe 405 // inverse time block, since the commands that use this value already perform undefined checks. This would
Sergunb 0:8f0d870509fe 406 // also allow other commands, following this switch, to execute and not error out needlessly. This code is
Sergunb 0:8f0d870509fe 407 // combined with the above feed rate mode and the below set feed rate error-checking.
Sergunb 0:8f0d870509fe 408
Sergunb 0:8f0d870509fe 409 // [3. Set feed rate ]: F is negative (done.)
Sergunb 0:8f0d870509fe 410 // - In inverse time mode: Always implicitly zero the feed rate value before and after block completion.
Sergunb 0:8f0d870509fe 411 // NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word
Sergunb 0:8f0d870509fe 412 // value in the block. If no F word is passed with a motion command that requires a feed rate, this will error
Sergunb 0:8f0d870509fe 413 // out in the motion modes error-checking. However, if no F word is passed with NO motion command that requires
Sergunb 0:8f0d870509fe 414 // a feed rate, we simply move on and the state feed rate value gets updated to zero and remains undefined.
Sergunb 0:8f0d870509fe 415 } else { // = G94
Sergunb 0:8f0d870509fe 416 // - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value.
Sergunb 0:8f0d870509fe 417 if (gc_state.modal.feed_rate == FEED_RATE_MODE_UNITS_PER_MIN) { // Last state is also G94
Sergunb 0:8f0d870509fe 418 if (bit_istrue(value_words,bit(WORD_F))) {
Sergunb 0:8f0d870509fe 419 if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.f *= MM_PER_INCH; }
Sergunb 0:8f0d870509fe 420 } else {
Sergunb 0:8f0d870509fe 421 gc_block.values.f = gc_state.feed_rate; // Push last state feed rate
Sergunb 0:8f0d870509fe 422 }
Sergunb 0:8f0d870509fe 423 } // Else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word value.
Sergunb 0:8f0d870509fe 424 }
Sergunb 0:8f0d870509fe 425 }
Sergunb 0:8f0d870509fe 426 // bit_false(value_words,bit(WORD_F)); // NOTE: Single-meaning value word. Set at end of error-checking.
Sergunb 0:8f0d870509fe 427
Sergunb 0:8f0d870509fe 428 // [4. Set spindle speed ]: S is negative (done.)
Sergunb 0:8f0d870509fe 429 if (bit_isfalse(value_words,bit(WORD_S))) { gc_block.values.s = gc_state.spindle_speed; }
Sergunb 0:8f0d870509fe 430 // bit_false(value_words,bit(WORD_S)); // NOTE: Single-meaning value word. Set at end of error-checking.
Sergunb 0:8f0d870509fe 431
Sergunb 0:8f0d870509fe 432 // [5. Select tool ]: NOT SUPPORTED. Only tracks value. T is negative (done.) Not an integer. Greater than max tool value.
Sergunb 0:8f0d870509fe 433 // bit_false(value_words,bit(WORD_T)); // NOTE: Single-meaning value word. Set at end of error-checking.
Sergunb 0:8f0d870509fe 434
Sergunb 0:8f0d870509fe 435 // [6. Change tool ]: N/A
Sergunb 0:8f0d870509fe 436 // [7. Spindle control ]: N/A
Sergunb 0:8f0d870509fe 437 // [8. Coolant control ]: N/A
Sergunb 0:8f0d870509fe 438 // [9. Override control ]: Not supported except for a Grbl-only parking motion override control.
Sergunb 0:8f0d870509fe 439 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
Sergunb 0:8f0d870509fe 440 if (bit_istrue(command_words, bit(MODAL_GROUP_M9))) { // Already set as enabled in parser.
Sergunb 0:8f0d870509fe 441 if (bit_istrue(value_words, bit(WORD_P))) {
Sergunb 0:8f0d870509fe 442 if (gc_block.values.p == 0.0f) { gc_block.modal.override = OVERRIDE_DISABLED; }
Sergunb 0:8f0d870509fe 443 bit_false(value_words, bit(WORD_P));
Sergunb 0:8f0d870509fe 444 }
Sergunb 0:8f0d870509fe 445 }
Sergunb 0:8f0d870509fe 446 #endif
Sergunb 0:8f0d870509fe 447
Sergunb 0:8f0d870509fe 448 // [10. Dwell ]: P value missing. P is negative (done.) NOTE: See below.
Sergunb 0:8f0d870509fe 449 if (gc_block.non_modal_command == NON_MODAL_DWELL) {
Sergunb 0:8f0d870509fe 450 if (bit_isfalse(value_words,bit(WORD_P))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); } // [P word missing]
Sergunb 0:8f0d870509fe 451 bit_false(value_words,bit(WORD_P));
Sergunb 0:8f0d870509fe 452 }
Sergunb 0:8f0d870509fe 453
Sergunb 0:8f0d870509fe 454 // [11. Set active plane ]: N/A
Sergunb 0:8f0d870509fe 455 switch (gc_block.modal.plane_select) {
Sergunb 0:8f0d870509fe 456 case PLANE_SELECT_XY:
Sergunb 0:8f0d870509fe 457 axis_0 = X_AXIS;
Sergunb 0:8f0d870509fe 458 axis_1 = Y_AXIS;
Sergunb 0:8f0d870509fe 459 axis_linear = Z_AXIS;
Sergunb 0:8f0d870509fe 460 break;
Sergunb 0:8f0d870509fe 461 case PLANE_SELECT_ZX:
Sergunb 0:8f0d870509fe 462 axis_0 = Z_AXIS;
Sergunb 0:8f0d870509fe 463 axis_1 = X_AXIS;
Sergunb 0:8f0d870509fe 464 axis_linear = Y_AXIS;
Sergunb 0:8f0d870509fe 465 break;
Sergunb 0:8f0d870509fe 466 default: // case PLANE_SELECT_YZ:
Sergunb 0:8f0d870509fe 467 axis_0 = Y_AXIS;
Sergunb 0:8f0d870509fe 468 axis_1 = Z_AXIS;
Sergunb 0:8f0d870509fe 469 axis_linear = X_AXIS;
Sergunb 0:8f0d870509fe 470 }
Sergunb 0:8f0d870509fe 471
Sergunb 0:8f0d870509fe 472 // [12. Set length units ]: N/A
Sergunb 0:8f0d870509fe 473 // Pre-convert XYZ coordinate values to millimeters, if applicable.
Sergunb 0:8f0d870509fe 474 uint8_t idx;
Sergunb 0:8f0d870509fe 475 if (gc_block.modal.units == UNITS_MODE_INCHES) {
Sergunb 0:8f0d870509fe 476 for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
Sergunb 0:8f0d870509fe 477 if (bit_istrue(axis_words,bit(idx)) ) {
Sergunb 0:8f0d870509fe 478 gc_block.values.xyz[idx] *= MM_PER_INCH;
Sergunb 0:8f0d870509fe 479 }
Sergunb 0:8f0d870509fe 480 }
Sergunb 0:8f0d870509fe 481 }
Sergunb 0:8f0d870509fe 482
Sergunb 0:8f0d870509fe 483 // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED. Error, if enabled while G53 is active.
Sergunb 0:8f0d870509fe 484 // [G40 Errors]: G2/3 arc is programmed after a G40. The linear move after disabling is less than tool diameter.
Sergunb 0:8f0d870509fe 485 // NOTE: Since cutter radius compensation is never enabled, these G40 errors don't apply. Grbl supports G40
Sergunb 0:8f0d870509fe 486 // only for the purpose to not error when G40 is sent with a g-code program header to setup the default modes.
Sergunb 0:8f0d870509fe 487
Sergunb 0:8f0d870509fe 488 // [14. Cutter length compensation ]: G43 NOT SUPPORTED, but G43.1 and G49 are.
Sergunb 0:8f0d870509fe 489 // [G43.1 Errors]: Motion command in same line.
Sergunb 0:8f0d870509fe 490 // NOTE: Although not explicitly stated so, G43.1 should be applied to only one valid
Sergunb 0:8f0d870509fe 491 // axis that is configured (in config.h). There should be an error if the configured axis
Sergunb 0:8f0d870509fe 492 // is absent or if any of the other axis words are present.
Sergunb 0:8f0d870509fe 493 if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates called in block.
Sergunb 0:8f0d870509fe 494 if (gc_block.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) {
Sergunb 0:8f0d870509fe 495 if (axis_words ^ (1<<TOOL_LENGTH_OFFSET_AXIS)) { FAIL(STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR); }
Sergunb 0:8f0d870509fe 496 }
Sergunb 0:8f0d870509fe 497 }
Sergunb 0:8f0d870509fe 498
Sergunb 0:8f0d870509fe 499 // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active.
Sergunb 0:8f0d870509fe 500 // TODO: An EEPROM read of the coordinate data may require a buffer sync when the cycle
Sergunb 0:8f0d870509fe 501 // is active. The read pauses the processor temporarily and may cause a rare crash. For
Sergunb 0:8f0d870509fe 502 // future versions on processors with enough memory, all coordinate data should be stored
Sergunb 0:8f0d870509fe 503 // in memory and written to EEPROM only when there is not a cycle active.
Sergunb 0:8f0d870509fe 504 float block_coord_system[N_AXIS];
Sergunb 0:8f0d870509fe 505 memcpy(block_coord_system,gc_state.coord_system,sizeof(gc_state.coord_system));
Sergunb 0:8f0d870509fe 506 if ( bit_istrue(command_words,bit(MODAL_GROUP_G12)) ) { // Check if called in block
Sergunb 0:8f0d870509fe 507 if (gc_block.modal.coord_select > N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys]
Sergunb 0:8f0d870509fe 508 if (gc_state.modal.coord_select != gc_block.modal.coord_select) {
Sergunb 0:8f0d870509fe 509 if (!(settings_read_coord_data(gc_block.modal.coord_select,block_coord_system))) { FAIL(STATUS_SETTING_READ_FAIL); }
Sergunb 0:8f0d870509fe 510 }
Sergunb 0:8f0d870509fe 511 }
Sergunb 0:8f0d870509fe 512
Sergunb 0:8f0d870509fe 513 // [16. Set path control mode ]: N/A. Only G61. G61.1 and G64 NOT SUPPORTED.
Sergunb 0:8f0d870509fe 514 // [17. Set distance mode ]: N/A. Only G91.1. G90.1 NOT SUPPORTED.
Sergunb 0:8f0d870509fe 515 // [18. Set retract mode ]: NOT SUPPORTED.
Sergunb 0:8f0d870509fe 516
Sergunb 0:8f0d870509fe 517 // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets.
Sergunb 0:8f0d870509fe 518 // NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these
Sergunb 0:8f0d870509fe 519 // commands all treat axis words differently. G10 as absolute offsets or computes current position as
Sergunb 0:8f0d870509fe 520 // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes
Sergunb 0:8f0d870509fe 521 // all the current coordinate system and G92 offsets.
Sergunb 0:8f0d870509fe 522 switch (gc_block.non_modal_command) {
Sergunb 0:8f0d870509fe 523 case NON_MODAL_SET_COORDINATE_DATA:
Sergunb 0:8f0d870509fe 524 // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.)
Sergunb 0:8f0d870509fe 525 // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing.
Sergunb 0:8f0d870509fe 526 // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing.
Sergunb 0:8f0d870509fe 527 if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS) }; // [No axis words]
Sergunb 0:8f0d870509fe 528 if (bit_isfalse(value_words,((1<<WORD_P)|(1<<WORD_L)))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); } // [P/L word missing]
Sergunb 0:8f0d870509fe 529 coord_select = truncf(gc_block.values.p); // Convert p value to int.
Sergunb 0:8f0d870509fe 530 if (coord_select > N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys]
Sergunb 0:8f0d870509fe 531 if (gc_block.values.l != 20) {
Sergunb 0:8f0d870509fe 532 if (gc_block.values.l == 2) {
Sergunb 0:8f0d870509fe 533 if (bit_istrue(value_words,bit(WORD_R))) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G10 L2 R not supported]
Sergunb 0:8f0d870509fe 534 } else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported L]
Sergunb 0:8f0d870509fe 535 }
Sergunb 0:8f0d870509fe 536 bit_false(value_words,(bit(WORD_L)|bit(WORD_P)));
Sergunb 0:8f0d870509fe 537
Sergunb 0:8f0d870509fe 538 // Determine coordinate system to change and try to load from EEPROM.
Sergunb 0:8f0d870509fe 539 if (coord_select > 0) { coord_select--; } // Adjust P1-P6 index to EEPROM coordinate data indexing.
Sergunb 0:8f0d870509fe 540 else { coord_select = gc_block.modal.coord_select; } // Index P0 as the active coordinate system
Sergunb 0:8f0d870509fe 541
Sergunb 0:8f0d870509fe 542 // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command.
Sergunb 0:8f0d870509fe 543 if (!settings_read_coord_data(coord_select,gc_block.values.ijk)) { FAIL(STATUS_SETTING_READ_FAIL); } // [EEPROM read fail]
Sergunb 0:8f0d870509fe 544
Sergunb 0:8f0d870509fe 545 // Pre-calculate the coordinate data changes.
Sergunb 0:8f0d870509fe 546 for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
Sergunb 0:8f0d870509fe 547 // Update axes defined only in block. Always in machine coordinates. Can change non-active system.
Sergunb 0:8f0d870509fe 548 if (bit_istrue(axis_words,bit(idx)) ) {
Sergunb 0:8f0d870509fe 549 if (gc_block.values.l == 20) {
Sergunb 0:8f0d870509fe 550 // L20: Update coordinate system axis at current position (with modifiers) with programmed value
Sergunb 0:8f0d870509fe 551 // WPos = MPos - WCS - G92 - TLO -> WCS = MPos - G92 - TLO - WPos
Sergunb 0:8f0d870509fe 552 gc_block.values.ijk[idx] = gc_state.position[idx]-gc_state.coord_offset[idx]-gc_block.values.xyz[idx];
Sergunb 0:8f0d870509fe 553 if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.ijk[idx] -= gc_state.tool_length_offset; }
Sergunb 0:8f0d870509fe 554 } else {
Sergunb 0:8f0d870509fe 555 // L2: Update coordinate system axis to programmed value.
Sergunb 0:8f0d870509fe 556 gc_block.values.ijk[idx] = gc_block.values.xyz[idx];
Sergunb 0:8f0d870509fe 557 }
Sergunb 0:8f0d870509fe 558 } // Else, keep current stored value.
Sergunb 0:8f0d870509fe 559 }
Sergunb 0:8f0d870509fe 560 break;
Sergunb 0:8f0d870509fe 561 case NON_MODAL_SET_COORDINATE_OFFSET:
Sergunb 0:8f0d870509fe 562 // [G92 Errors]: No axis words.
Sergunb 0:8f0d870509fe 563 if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words]
Sergunb 0:8f0d870509fe 564
Sergunb 0:8f0d870509fe 565 // Update axes defined only in block. Offsets current system to defined value. Does not update when
Sergunb 0:8f0d870509fe 566 // active coordinate system is selected, but is still active unless G92.1 disables it.
Sergunb 0:8f0d870509fe 567 for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
Sergunb 0:8f0d870509fe 568 if (bit_istrue(axis_words,bit(idx)) ) {
Sergunb 0:8f0d870509fe 569 // WPos = MPos - WCS - G92 - TLO -> G92 = MPos - WCS - TLO - WPos
Sergunb 0:8f0d870509fe 570 gc_block.values.xyz[idx] = gc_state.position[idx]-block_coord_system[idx]-gc_block.values.xyz[idx];
Sergunb 0:8f0d870509fe 571 if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] -= gc_state.tool_length_offset; }
Sergunb 0:8f0d870509fe 572 } else {
Sergunb 0:8f0d870509fe 573 gc_block.values.xyz[idx] = gc_state.coord_offset[idx];
Sergunb 0:8f0d870509fe 574 }
Sergunb 0:8f0d870509fe 575 }
Sergunb 0:8f0d870509fe 576 break;
Sergunb 0:8f0d870509fe 577
Sergunb 0:8f0d870509fe 578 default:
Sergunb 0:8f0d870509fe 579
Sergunb 0:8f0d870509fe 580 // At this point, the rest of the explicit axis commands treat the axis values as the traditional
Sergunb 0:8f0d870509fe 581 // target position with the coordinate system offsets, G92 offsets, absolute override, and distance
Sergunb 0:8f0d870509fe 582 // modes applied. This includes the motion mode commands. We can now pre-compute the target position.
Sergunb 0:8f0d870509fe 583 // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
Sergunb 0:8f0d870509fe 584 if (axis_command != AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // TLO block any axis command.
Sergunb 0:8f0d870509fe 585 if (axis_words) {
Sergunb 0:8f0d870509fe 586 for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
Sergunb 0:8f0d870509fe 587 if ( bit_isfalse(axis_words,bit(idx)) ) {
Sergunb 0:8f0d870509fe 588 gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position.
Sergunb 0:8f0d870509fe 589 } else {
Sergunb 0:8f0d870509fe 590 // Update specified value according to distance mode or ignore if absolute override is active.
Sergunb 0:8f0d870509fe 591 // NOTE: G53 is never active with G28/30 since they are in the same modal group.
Sergunb 0:8f0d870509fe 592 if (gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) {
Sergunb 0:8f0d870509fe 593 // Apply coordinate offsets based on distance mode.
Sergunb 0:8f0d870509fe 594 if (gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) {
Sergunb 0:8f0d870509fe 595 gc_block.values.xyz[idx] += block_coord_system[idx] + gc_state.coord_offset[idx];
Sergunb 0:8f0d870509fe 596 if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] += gc_state.tool_length_offset; }
Sergunb 0:8f0d870509fe 597 } else { // Incremental mode
Sergunb 0:8f0d870509fe 598 gc_block.values.xyz[idx] += gc_state.position[idx];
Sergunb 0:8f0d870509fe 599 }
Sergunb 0:8f0d870509fe 600 }
Sergunb 0:8f0d870509fe 601 }
Sergunb 0:8f0d870509fe 602 }
Sergunb 0:8f0d870509fe 603 }
Sergunb 0:8f0d870509fe 604 }
Sergunb 0:8f0d870509fe 605
Sergunb 0:8f0d870509fe 606 // Check remaining non-modal commands for errors.
Sergunb 0:8f0d870509fe 607 switch (gc_block.non_modal_command) {
Sergunb 0:8f0d870509fe 608 case NON_MODAL_GO_HOME_0: // G28
Sergunb 0:8f0d870509fe 609 case NON_MODAL_GO_HOME_1: // G30
Sergunb 0:8f0d870509fe 610 // [G28/30 Errors]: Cutter compensation is enabled.
Sergunb 0:8f0d870509fe 611 // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
Sergunb 0:8f0d870509fe 612 // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command.
Sergunb 0:8f0d870509fe 613 if (gc_block.non_modal_command == NON_MODAL_GO_HOME_0) {
Sergunb 0:8f0d870509fe 614 if (!settings_read_coord_data(SETTING_INDEX_G28,gc_block.values.ijk)) { FAIL(STATUS_SETTING_READ_FAIL); }
Sergunb 0:8f0d870509fe 615 } else { // == NON_MODAL_GO_HOME_1
Sergunb 0:8f0d870509fe 616 if (!settings_read_coord_data(SETTING_INDEX_G30,gc_block.values.ijk)) { FAIL(STATUS_SETTING_READ_FAIL); }
Sergunb 0:8f0d870509fe 617 }
Sergunb 0:8f0d870509fe 618 if (axis_words) {
Sergunb 0:8f0d870509fe 619 // Move only the axes specified in secondary move.
Sergunb 0:8f0d870509fe 620 for (idx=0; idx<N_AXIS; idx++) {
Sergunb 0:8f0d870509fe 621 if (!(axis_words & (1<<idx))) { gc_block.values.ijk[idx] = gc_state.position[idx]; }
Sergunb 0:8f0d870509fe 622 }
Sergunb 0:8f0d870509fe 623 } else {
Sergunb 0:8f0d870509fe 624 axis_command = AXIS_COMMAND_NONE; // Set to none if no intermediate motion.
Sergunb 0:8f0d870509fe 625 }
Sergunb 0:8f0d870509fe 626 break;
Sergunb 0:8f0d870509fe 627 case NON_MODAL_SET_HOME_0: // G28.1
Sergunb 0:8f0d870509fe 628 case NON_MODAL_SET_HOME_1: // G30.1
Sergunb 0:8f0d870509fe 629 // [G28.1/30.1 Errors]: Cutter compensation is enabled.
Sergunb 0:8f0d870509fe 630 // NOTE: If axis words are passed here, they are interpreted as an implicit motion mode.
Sergunb 0:8f0d870509fe 631 break;
Sergunb 0:8f0d870509fe 632 case NON_MODAL_RESET_COORDINATE_OFFSET:
Sergunb 0:8f0d870509fe 633 // NOTE: If axis words are passed here, they are interpreted as an implicit motion mode.
Sergunb 0:8f0d870509fe 634 break;
Sergunb 0:8f0d870509fe 635 case NON_MODAL_ABSOLUTE_OVERRIDE:
Sergunb 0:8f0d870509fe 636 // [G53 Errors]: G0 and G1 are not active. Cutter compensation is enabled.
Sergunb 0:8f0d870509fe 637 // NOTE: All explicit axis word commands are in this modal group. So no implicit check necessary.
Sergunb 0:8f0d870509fe 638 if (!(gc_block.modal.motion == MOTION_MODE_SEEK || gc_block.modal.motion == MOTION_MODE_LINEAR)) {
Sergunb 0:8f0d870509fe 639 FAIL(STATUS_GCODE_G53_INVALID_MOTION_MODE); // [G53 G0/1 not active]
Sergunb 0:8f0d870509fe 640 }
Sergunb 0:8f0d870509fe 641 break;
Sergunb 0:8f0d870509fe 642 }
Sergunb 0:8f0d870509fe 643 }
Sergunb 0:8f0d870509fe 644
Sergunb 0:8f0d870509fe 645 // [20. Motion modes ]:
Sergunb 0:8f0d870509fe 646 if (gc_block.modal.motion == MOTION_MODE_NONE) {
Sergunb 0:8f0d870509fe 647 // [G80 Errors]: Axis word are programmed while G80 is active.
Sergunb 0:8f0d870509fe 648 // NOTE: Even non-modal commands or TLO that use axis words will throw this strict error.
Sergunb 0:8f0d870509fe 649 if (axis_words) { FAIL(STATUS_GCODE_AXIS_WORDS_EXIST); } // [No axis words allowed]
Sergunb 0:8f0d870509fe 650
Sergunb 0:8f0d870509fe 651 // Check remaining motion modes, if axis word are implicit (exist and not used by G10/28/30/92), or
Sergunb 0:8f0d870509fe 652 // was explicitly commanded in the g-code block.
Sergunb 0:8f0d870509fe 653 } else if ( axis_command == AXIS_COMMAND_MOTION_MODE ) {
Sergunb 0:8f0d870509fe 654
Sergunb 0:8f0d870509fe 655 if (gc_block.modal.motion == MOTION_MODE_SEEK) {
Sergunb 0:8f0d870509fe 656 // [G0 Errors]: Axis letter not configured or without real value (done.)
Sergunb 0:8f0d870509fe 657 // Axis words are optional. If missing, set axis command flag to ignore execution.
Sergunb 0:8f0d870509fe 658 if (!axis_words) { axis_command = AXIS_COMMAND_NONE; }
Sergunb 0:8f0d870509fe 659
Sergunb 0:8f0d870509fe 660 // All remaining motion modes (all but G0 and G80), require a valid feed rate value. In units per mm mode,
Sergunb 0:8f0d870509fe 661 // the value must be positive. In inverse time mode, a positive value must be passed with each block.
Sergunb 0:8f0d870509fe 662 } else {
Sergunb 0:8f0d870509fe 663 // Check if feed rate is defined for the motion modes that require it.
Sergunb 0:8f0d870509fe 664 if (gc_block.values.f == 0.0f) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [Feed rate undefined]
Sergunb 0:8f0d870509fe 665
Sergunb 0:8f0d870509fe 666 switch (gc_block.modal.motion) {
Sergunb 0:8f0d870509fe 667 case MOTION_MODE_LINEAR:
Sergunb 0:8f0d870509fe 668 // [G1 Errors]: Feed rate undefined. Axis letter not configured or without real value.
Sergunb 0:8f0d870509fe 669 // Axis words are optional. If missing, set axis command flag to ignore execution.
Sergunb 0:8f0d870509fe 670 if (!axis_words) { axis_command = AXIS_COMMAND_NONE; }
Sergunb 0:8f0d870509fe 671
Sergunb 0:8f0d870509fe 672 break;
Sergunb 0:8f0d870509fe 673 case MOTION_MODE_CW_ARC:
Sergunb 0:8f0d870509fe 674 gc_parser_flags |= GC_PARSER_ARC_IS_CLOCKWISE; // No break intentional.
Sergunb 0:8f0d870509fe 675 case MOTION_MODE_CCW_ARC:
Sergunb 0:8f0d870509fe 676 // [G2/3 Errors All-Modes]: Feed rate undefined.
Sergunb 0:8f0d870509fe 677 // [G2/3 Radius-Mode Errors]: No axis words in selected plane. Target point is same as current.
Sergunb 0:8f0d870509fe 678 // [G2/3 Offset-Mode Errors]: No axis words and/or offsets in selected plane. The radius to the current
Sergunb 0:8f0d870509fe 679 // point and the radius to the target point differs more than 0.002mm (EMC def. 0.5mm OR 0.005mm and 0.1% radius).
Sergunb 0:8f0d870509fe 680 // [G2/3 Full-Circle-Mode Errors]: NOT SUPPORTED. Axis words exist. No offsets programmed. P must be an integer.
Sergunb 0:8f0d870509fe 681 // NOTE: Both radius and offsets are required for arc tracing and are pre-computed with the error-checking.
Sergunb 0:8f0d870509fe 682
Sergunb 0:8f0d870509fe 683 if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words]
Sergunb 0:8f0d870509fe 684 if (!(axis_words & (bit(axis_0)|bit(axis_1)))) { FAIL(STATUS_GCODE_NO_AXIS_WORDS_IN_PLANE); } // [No axis words in plane]
Sergunb 0:8f0d870509fe 685
Sergunb 0:8f0d870509fe 686 // Calculate the change in position along each selected axis
Sergunb 0:8f0d870509fe 687 float x,y;
Sergunb 0:8f0d870509fe 688 x = gc_block.values.xyz[axis_0]-gc_state.position[axis_0]; // Delta x between current position and target
Sergunb 0:8f0d870509fe 689 y = gc_block.values.xyz[axis_1]-gc_state.position[axis_1]; // Delta y between current position and target
Sergunb 0:8f0d870509fe 690
Sergunb 0:8f0d870509fe 691 if (value_words & bit(WORD_R)) { // Arc Radius Mode
Sergunb 0:8f0d870509fe 692 bit_false(value_words,bit(WORD_R));
Sergunb 0:8f0d870509fe 693 if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Invalid target]
Sergunb 0:8f0d870509fe 694
Sergunb 0:8f0d870509fe 695 // Convert radius value to proper units.
Sergunb 0:8f0d870509fe 696 if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.r *= MM_PER_INCH; }
Sergunb 0:8f0d870509fe 697 /* We need to calculate the center of the circle that has the designated radius and passes
Sergunb 0:8f0d870509fe 698 through both the current position and the target position. This method calculates the following
Sergunb 0:8f0d870509fe 699 set of equations where [x,y] is the vector from current to target position, d == magnitude of
Sergunb 0:8f0d870509fe 700 that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
Sergunb 0:8f0d870509fe 701 the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
Sergunb 0:8f0d870509fe 702 length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
Sergunb 0:8f0d870509fe 703 [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
Sergunb 0:8f0d870509fe 704
Sergunb 0:8f0d870509fe 705 d^2 == x^2 + y^2
Sergunb 0:8f0d870509fe 706 h^2 == r^2 - (d/2)^2
Sergunb 0:8f0d870509fe 707 i == x/2 - y/d*h
Sergunb 0:8f0d870509fe 708 j == y/2 + x/d*h
Sergunb 0:8f0d870509fe 709
Sergunb 0:8f0d870509fe 710 O <- [i,j]
Sergunb 0:8f0d870509fe 711 - |
Sergunb 0:8f0d870509fe 712 r - |
Sergunb 0:8f0d870509fe 713 - |
Sergunb 0:8f0d870509fe 714 - | h
Sergunb 0:8f0d870509fe 715 - |
Sergunb 0:8f0d870509fe 716 [0,0] -> C -----------------+--------------- T <- [x,y]
Sergunb 0:8f0d870509fe 717 | <------ d/2 ---->|
Sergunb 0:8f0d870509fe 718
Sergunb 0:8f0d870509fe 719 C - Current position
Sergunb 0:8f0d870509fe 720 T - Target position
Sergunb 0:8f0d870509fe 721 O - center of circle that pass through both C and T
Sergunb 0:8f0d870509fe 722 d - distance from C to T
Sergunb 0:8f0d870509fe 723 r - designated radius
Sergunb 0:8f0d870509fe 724 h - distance from center of CT to O
Sergunb 0:8f0d870509fe 725
Sergunb 0:8f0d870509fe 726 Expanding the equations:
Sergunb 0:8f0d870509fe 727
Sergunb 0:8f0d870509fe 728 d -> sqrt(x^2 + y^2)
Sergunb 0:8f0d870509fe 729 h -> sqrt(4 * r^2 - x^2 - y^2)/2
Sergunb 0:8f0d870509fe 730 i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Sergunb 0:8f0d870509fe 731 j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Sergunb 0:8f0d870509fe 732
Sergunb 0:8f0d870509fe 733 Which can be written:
Sergunb 0:8f0d870509fe 734
Sergunb 0:8f0d870509fe 735 i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Sergunb 0:8f0d870509fe 736 j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Sergunb 0:8f0d870509fe 737
Sergunb 0:8f0d870509fe 738 Which we for size and speed reasons optimize to:
Sergunb 0:8f0d870509fe 739
Sergunb 0:8f0d870509fe 740 h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
Sergunb 0:8f0d870509fe 741 i = (x - (y * h_x2_div_d))/2
Sergunb 0:8f0d870509fe 742 j = (y + (x * h_x2_div_d))/2
Sergunb 0:8f0d870509fe 743 */
Sergunb 0:8f0d870509fe 744
Sergunb 0:8f0d870509fe 745 // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
Sergunb 0:8f0d870509fe 746 // than d. If so, the sqrt of a negative number is complex and error out.
Sergunb 0:8f0d870509fe 747 float h_x2_div_d = 4.0f * gc_block.values.r*gc_block.values.r - x*x - y*y;
Sergunb 0:8f0d870509fe 748
Sergunb 0:8f0d870509fe 749 if (h_x2_div_d < 0) { FAIL(STATUS_GCODE_ARC_RADIUS_ERROR); } // [Arc radius error]
Sergunb 0:8f0d870509fe 750
Sergunb 0:8f0d870509fe 751 // Finish computing h_x2_div_d.
Sergunb 0:8f0d870509fe 752 h_x2_div_d = -sqrtf(h_x2_div_d)/hypot_f(x,y); // == -(h * 2 / d)
Sergunb 0:8f0d870509fe 753 // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
Sergunb 0:8f0d870509fe 754 if (gc_block.modal.motion == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
Sergunb 0:8f0d870509fe 755
Sergunb 0:8f0d870509fe 756 /* The counter clockwise circle lies to the left of the target direction. When offset is positive,
Sergunb 0:8f0d870509fe 757 the left hand circle will be generated - when it is negative the right hand circle is generated.
Sergunb 0:8f0d870509fe 758
Sergunb 0:8f0d870509fe 759 T <-- Target position
Sergunb 0:8f0d870509fe 760
Sergunb 0:8f0d870509fe 761 ^
Sergunb 0:8f0d870509fe 762 Clockwise circles with this center | Clockwise circles with this center will have
Sergunb 0:8f0d870509fe 763 will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
Sergunb 0:8f0d870509fe 764 \ | /
Sergunb 0:8f0d870509fe 765 center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
Sergunb 0:8f0d870509fe 766 |
Sergunb 0:8f0d870509fe 767 |
Sergunb 0:8f0d870509fe 768
Sergunb 0:8f0d870509fe 769 C <-- Current position
Sergunb 0:8f0d870509fe 770 */
Sergunb 0:8f0d870509fe 771 // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
Sergunb 0:8f0d870509fe 772 // even though it is advised against ever generating such circles in a single line of g-code. By
Sergunb 0:8f0d870509fe 773 // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
Sergunb 0:8f0d870509fe 774 // travel and thus we get the unadvisably long arcs as prescribed.
Sergunb 0:8f0d870509fe 775 if (gc_block.values.r < 0) {
Sergunb 0:8f0d870509fe 776 h_x2_div_d = -h_x2_div_d;
Sergunb 0:8f0d870509fe 777 gc_block.values.r = -gc_block.values.r; // Finished with r. Set to positive for mc_arc
Sergunb 0:8f0d870509fe 778 }
Sergunb 0:8f0d870509fe 779 // Complete the operation by calculating the actual center of the arc
Sergunb 0:8f0d870509fe 780 gc_block.values.ijk[axis_0] = 0.5f*(x-(y*h_x2_div_d));
Sergunb 0:8f0d870509fe 781 gc_block.values.ijk[axis_1] = 0.5f*(y+(x*h_x2_div_d));
Sergunb 0:8f0d870509fe 782
Sergunb 0:8f0d870509fe 783 } else { // Arc Center Format Offset Mode
Sergunb 0:8f0d870509fe 784 if (!(ijk_words & (bit(axis_0)|bit(axis_1)))) { FAIL(STATUS_GCODE_NO_OFFSETS_IN_PLANE); } // [No offsets in plane]
Sergunb 0:8f0d870509fe 785 bit_false(value_words,(bit(WORD_I)|bit(WORD_J)|bit(WORD_K)));
Sergunb 0:8f0d870509fe 786
Sergunb 0:8f0d870509fe 787 // Convert IJK values to proper units.
Sergunb 0:8f0d870509fe 788 if (gc_block.modal.units == UNITS_MODE_INCHES) {
Sergunb 0:8f0d870509fe 789 for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
Sergunb 0:8f0d870509fe 790 if (ijk_words & bit(idx)) { gc_block.values.ijk[idx] *= MM_PER_INCH; }
Sergunb 0:8f0d870509fe 791 }
Sergunb 0:8f0d870509fe 792 }
Sergunb 0:8f0d870509fe 793
Sergunb 0:8f0d870509fe 794 // Arc radius from center to target
Sergunb 0:8f0d870509fe 795 x -= gc_block.values.ijk[axis_0]; // Delta x between circle center and target
Sergunb 0:8f0d870509fe 796 y -= gc_block.values.ijk[axis_1]; // Delta y between circle center and target
Sergunb 0:8f0d870509fe 797 float target_r = hypot_f(x,y);
Sergunb 0:8f0d870509fe 798
Sergunb 0:8f0d870509fe 799 // Compute arc radius for mc_arc. Defined from current location to center.
Sergunb 0:8f0d870509fe 800 gc_block.values.r = hypot_f(gc_block.values.ijk[axis_0], gc_block.values.ijk[axis_1]);
Sergunb 0:8f0d870509fe 801
Sergunb 0:8f0d870509fe 802 // Compute difference between current location and target radii for final error-checks.
Sergunb 0:8f0d870509fe 803 float delta_r = fabsf(target_r-gc_block.values.r);
Sergunb 0:8f0d870509fe 804 if (delta_r > 0.005f) {
Sergunb 0:8f0d870509fe 805 if (delta_r > 0.5f) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] > 0.5mm
Sergunb 0:8f0d870509fe 806 if (delta_r > (0.001f*gc_block.values.r)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] > 0.005mm AND 0.1% radius
Sergunb 0:8f0d870509fe 807 }
Sergunb 0:8f0d870509fe 808 }
Sergunb 0:8f0d870509fe 809 break;
Sergunb 0:8f0d870509fe 810 case MOTION_MODE_PROBE_TOWARD_NO_ERROR: case MOTION_MODE_PROBE_AWAY_NO_ERROR:
Sergunb 0:8f0d870509fe 811 gc_parser_flags |= GC_PARSER_PROBE_IS_NO_ERROR; // No break intentional.
Sergunb 0:8f0d870509fe 812 case MOTION_MODE_PROBE_TOWARD: case MOTION_MODE_PROBE_AWAY:
Sergunb 0:8f0d870509fe 813 if ((gc_block.modal.motion == MOTION_MODE_PROBE_AWAY) ||
Sergunb 0:8f0d870509fe 814 (gc_block.modal.motion == MOTION_MODE_PROBE_AWAY_NO_ERROR)) { gc_parser_flags |= GC_PARSER_PROBE_IS_AWAY; }
Sergunb 0:8f0d870509fe 815 // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate
Sergunb 0:8f0d870509fe 816 // is undefined. Probe is triggered. NOTE: Probe check moved to probe cycle. Instead of returning
Sergunb 0:8f0d870509fe 817 // an error, it issues an alarm to prevent further motion to the probe. It's also done there to
Sergunb 0:8f0d870509fe 818 // allow the planner buffer to empty and move off the probe trigger before another probing cycle.
Sergunb 0:8f0d870509fe 819 if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words]
Sergunb 0:8f0d870509fe 820 if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Invalid target]
Sergunb 0:8f0d870509fe 821 break;
Sergunb 0:8f0d870509fe 822 }
Sergunb 0:8f0d870509fe 823 }
Sergunb 0:8f0d870509fe 824 }
Sergunb 0:8f0d870509fe 825
Sergunb 0:8f0d870509fe 826 // [21. Program flow ]: No error checks required.
Sergunb 0:8f0d870509fe 827
Sergunb 0:8f0d870509fe 828 // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc
Sergunb 0:8f0d870509fe 829 // radius mode, or axis words that aren't used in the block.
Sergunb 0:8f0d870509fe 830 if (gc_parser_flags & GC_PARSER_JOG_MOTION) {
Sergunb 0:8f0d870509fe 831 // Jogging only uses the F feed rate and XYZ value words. N is valid, but S and T are invalid.
Sergunb 0:8f0d870509fe 832 bit_false(value_words, (bit(WORD_N) | bit(WORD_F)));
Sergunb 0:8f0d870509fe 833 } else {
Sergunb 0:8f0d870509fe 834 bit_false(value_words, (bit(WORD_N) | bit(WORD_F) | bit(WORD_S) | bit(WORD_T))); // Remove single-meaning value words.
Sergunb 0:8f0d870509fe 835 }
Sergunb 0:8f0d870509fe 836 if (axis_command) { bit_false(value_words,(bit(WORD_X)|bit(WORD_Y)|bit(WORD_Z))); } // Remove axis words.
Sergunb 0:8f0d870509fe 837 if (value_words) { FAIL(STATUS_GCODE_UNUSED_WORDS); } // [Unused words]
Sergunb 0:8f0d870509fe 838
Sergunb 0:8f0d870509fe 839 /* -------------------------------------------------------------------------------------
Sergunb 0:8f0d870509fe 840 STEP 4: EXECUTE!!
Sergunb 0:8f0d870509fe 841 Assumes that all error-checking has been completed and no failure modes exist. We just
Sergunb 0:8f0d870509fe 842 need to update the state and execute the block according to the order-of-execution.
Sergunb 0:8f0d870509fe 843 */
Sergunb 0:8f0d870509fe 844
Sergunb 0:8f0d870509fe 845 // Initialize planner data struct for motion blocks.
Sergunb 0:8f0d870509fe 846 plan_line_data_t plan_data;
Sergunb 0:8f0d870509fe 847 plan_line_data_t *pl_data = &plan_data;
Sergunb 0:8f0d870509fe 848 memset(pl_data,0,sizeof(plan_line_data_t)); // Zero pl_data struct
Sergunb 0:8f0d870509fe 849
Sergunb 0:8f0d870509fe 850 // Intercept jog commands and complete error checking for valid jog commands and execute.
Sergunb 0:8f0d870509fe 851 // NOTE: G-code parser state is not updated, except the position to ensure sequential jog
Sergunb 0:8f0d870509fe 852 // targets are computed correctly. The final parser position after a jog is updated in
Sergunb 0:8f0d870509fe 853 // protocol_execute_realtime() when jogging completes or is canceled.
Sergunb 0:8f0d870509fe 854 if (gc_parser_flags & GC_PARSER_JOG_MOTION) {
Sergunb 0:8f0d870509fe 855 // Only distance and unit modal commands and G53 absolute override command are allowed.
Sergunb 0:8f0d870509fe 856 // NOTE: Feed rate word and axis word checks have already been performed in STEP 3.
Sergunb 0:8f0d870509fe 857 if (command_words & ~(bit(MODAL_GROUP_G3) | bit(MODAL_GROUP_G6 | bit(MODAL_GROUP_G0)))) { FAIL(STATUS_INVALID_JOG_COMMAND) };
Sergunb 0:8f0d870509fe 858 if (!(gc_block.non_modal_command == NON_MODAL_ABSOLUTE_OVERRIDE || gc_block.non_modal_command == NON_MODAL_NO_ACTION)) { FAIL(STATUS_INVALID_JOG_COMMAND); }
Sergunb 0:8f0d870509fe 859
Sergunb 0:8f0d870509fe 860 // Initialize planner data to current spindle and coolant modal state.
Sergunb 0:8f0d870509fe 861 pl_data->spindle_speed = gc_state.spindle_speed;
Sergunb 0:8f0d870509fe 862 plan_data.condition = (gc_state.modal.spindle | gc_state.modal.coolant);
Sergunb 0:8f0d870509fe 863
Sergunb 0:8f0d870509fe 864 uint8_t status = jog_execute(&plan_data, &gc_block);
Sergunb 0:8f0d870509fe 865 if (status == STATUS_OK) { memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); }
Sergunb 0:8f0d870509fe 866 return(status);
Sergunb 0:8f0d870509fe 867 }
Sergunb 0:8f0d870509fe 868
Sergunb 0:8f0d870509fe 869 // If in laser mode, setup laser power based on current and past parser conditions.
Sergunb 0:8f0d870509fe 870 if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
Sergunb 0:8f0d870509fe 871 if (!((gc_block.modal.motion == MOTION_MODE_LINEAR) || (gc_block.modal.motion == MOTION_MODE_CW_ARC)
Sergunb 0:8f0d870509fe 872 || (gc_block.modal.motion == MOTION_MODE_CCW_ARC))) {
Sergunb 0:8f0d870509fe 873 gc_parser_flags |= GC_PARSER_LASER_DISABLE;
Sergunb 0:8f0d870509fe 874 }
Sergunb 0:8f0d870509fe 875
Sergunb 0:8f0d870509fe 876 // Any motion mode with axis words is allowed to be passed from a spindle speed update.
Sergunb 0:8f0d870509fe 877 // NOTE: G1 and G0 without axis words sets axis_command to none. G28/30 are intentionally omitted.
Sergunb 0:8f0d870509fe 878 // TODO: Check sync conditions for M3 enabled motions that don't enter the planner. (zero length).
Sergunb 0:8f0d870509fe 879 if (axis_words && (axis_command == AXIS_COMMAND_MOTION_MODE)) {
Sergunb 0:8f0d870509fe 880 gc_parser_flags |= GC_PARSER_LASER_ISMOTION;
Sergunb 0:8f0d870509fe 881 }
Sergunb 0:8f0d870509fe 882 else {
Sergunb 0:8f0d870509fe 883 // M3 constant power laser requires planner syncs to update the laser when changing between
Sergunb 0:8f0d870509fe 884 // a G1/2/3 motion mode state and vice versa when there is no motion in the line.
Sergunb 0:8f0d870509fe 885 if (gc_state.modal.spindle == SPINDLE_ENABLE_CW) {
Sergunb 0:8f0d870509fe 886 if ((gc_state.modal.motion == MOTION_MODE_LINEAR) || (gc_state.modal.motion == MOTION_MODE_CW_ARC)
Sergunb 0:8f0d870509fe 887 || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) {
Sergunb 0:8f0d870509fe 888 if (bit_istrue(gc_parser_flags, GC_PARSER_LASER_DISABLE)) {
Sergunb 0:8f0d870509fe 889 gc_parser_flags |= GC_PARSER_LASER_FORCE_SYNC; // Change from G1/2/3 motion mode.
Sergunb 0:8f0d870509fe 890 }
Sergunb 0:8f0d870509fe 891 }
Sergunb 0:8f0d870509fe 892 else {
Sergunb 0:8f0d870509fe 893 // When changing to a G1 motion mode without axis words from a non-G1/2/3 motion mode.
Sergunb 0:8f0d870509fe 894 if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_DISABLE)) {
Sergunb 0:8f0d870509fe 895 gc_parser_flags |= GC_PARSER_LASER_FORCE_SYNC;
Sergunb 0:8f0d870509fe 896 }
Sergunb 0:8f0d870509fe 897 }
Sergunb 0:8f0d870509fe 898 }
Sergunb 0:8f0d870509fe 899 }
Sergunb 0:8f0d870509fe 900 }
Sergunb 0:8f0d870509fe 901
Sergunb 0:8f0d870509fe 902 // [0. Non-specific/common error-checks and miscellaneous setup]:
Sergunb 0:8f0d870509fe 903 // NOTE: If no line number is present, the value is zero.
Sergunb 0:8f0d870509fe 904 gc_state.line_number = gc_block.values.n;
Sergunb 0:8f0d870509fe 905 #ifdef USE_LINE_NUMBERS
Sergunb 0:8f0d870509fe 906 pl_data->line_number = gc_state.line_number; // Record data for planner use.
Sergunb 0:8f0d870509fe 907 #endif
Sergunb 0:8f0d870509fe 908
Sergunb 0:8f0d870509fe 909 // [1. Comments feedback ]: NOT SUPPORTED
Sergunb 0:8f0d870509fe 910
Sergunb 0:8f0d870509fe 911 // [2. Set feed rate mode ]:
Sergunb 0:8f0d870509fe 912 gc_state.modal.feed_rate = gc_block.modal.feed_rate;
Sergunb 0:8f0d870509fe 913 if (gc_state.modal.feed_rate) { pl_data->condition |= PL_COND_FLAG_INVERSE_TIME; } // Set condition flag for planner use.
Sergunb 0:8f0d870509fe 914
Sergunb 0:8f0d870509fe 915 // [3. Set feed rate ]:
Sergunb 0:8f0d870509fe 916 gc_state.feed_rate = gc_block.values.f; // Always copy this value. See feed rate error-checking.
Sergunb 0:8f0d870509fe 917 pl_data->feed_rate = gc_state.feed_rate; // Record data for planner use.
Sergunb 0:8f0d870509fe 918
Sergunb 0:8f0d870509fe 919 // [4. Set spindle speed ]:
Sergunb 0:8f0d870509fe 920 if ((gc_state.spindle_speed != gc_block.values.s) || bit_istrue(gc_parser_flags, GC_PARSER_LASER_FORCE_SYNC)) {
Sergunb 0:8f0d870509fe 921 if (gc_state.modal.spindle != SPINDLE_DISABLE) {
Sergunb 0:8f0d870509fe 922 #ifdef VARIABLE_SPINDLE
Sergunb 0:8f0d870509fe 923 if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_ISMOTION)) {
Sergunb 0:8f0d870509fe 924 if (bit_istrue(gc_parser_flags, GC_PARSER_LASER_DISABLE)) {
Sergunb 0:8f0d870509fe 925 spindle_sync(gc_state.modal.spindle, 0.0);
Sergunb 0:8f0d870509fe 926 }
Sergunb 0:8f0d870509fe 927 else { spindle_sync(gc_state.modal.spindle, gc_block.values.s); }
Sergunb 0:8f0d870509fe 928 }
Sergunb 0:8f0d870509fe 929 #else
Sergunb 0:8f0d870509fe 930 spindle_sync(gc_state.modal.spindle, 0.0);
Sergunb 0:8f0d870509fe 931 #endif
Sergunb 0:8f0d870509fe 932 }
Sergunb 0:8f0d870509fe 933 gc_state.spindle_speed = gc_block.values.s; // Update spindle speed state.
Sergunb 0:8f0d870509fe 934 }
Sergunb 0:8f0d870509fe 935 // NOTE: Pass zero spindle speed for all restricted laser motions.
Sergunb 0:8f0d870509fe 936 if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_DISABLE)) {
Sergunb 0:8f0d870509fe 937 pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use.
Sergunb 0:8f0d870509fe 938 } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already.
Sergunb 0:8f0d870509fe 939
Sergunb 0:8f0d870509fe 940 // [5. Select tool ]: NOT SUPPORTED. Only tracks tool value.
Sergunb 0:8f0d870509fe 941 gc_state.tool = gc_block.values.t;
Sergunb 0:8f0d870509fe 942
Sergunb 0:8f0d870509fe 943 // [6. Change tool ]: NOT SUPPORTED
Sergunb 0:8f0d870509fe 944
Sergunb 0:8f0d870509fe 945 // [7. Spindle control ]:
Sergunb 0:8f0d870509fe 946 if (gc_state.modal.spindle != gc_block.modal.spindle) {
Sergunb 0:8f0d870509fe 947 // Update spindle control and apply spindle speed when enabling it in this block.
Sergunb 0:8f0d870509fe 948 // NOTE: All spindle state changes are synced, even in laser mode. Also, pl_data,
Sergunb 0:8f0d870509fe 949 // rather than gc_state, is used to manage laser state for non-laser motions.
Sergunb 0:8f0d870509fe 950 spindle_sync(gc_block.modal.spindle, pl_data->spindle_speed);
Sergunb 0:8f0d870509fe 951 gc_state.modal.spindle = gc_block.modal.spindle;
Sergunb 0:8f0d870509fe 952 }
Sergunb 0:8f0d870509fe 953 pl_data->condition |= gc_state.modal.spindle; // Set condition flag for planner use.
Sergunb 0:8f0d870509fe 954
Sergunb 0:8f0d870509fe 955 // [8. Coolant control ]:
Sergunb 0:8f0d870509fe 956 if (gc_state.modal.coolant != gc_block.modal.coolant) {
Sergunb 0:8f0d870509fe 957 // NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states
Sergunb 0:8f0d870509fe 958 // can exist at the same time, while coolant disable clears all states.
Sergunb 0:8f0d870509fe 959 coolant_sync(gc_block.modal.coolant);
Sergunb 0:8f0d870509fe 960 if (gc_block.modal.coolant == COOLANT_DISABLE) { gc_state.modal.coolant = COOLANT_DISABLE; }
Sergunb 0:8f0d870509fe 961 else { gc_state.modal.coolant |= gc_block.modal.coolant; }
Sergunb 0:8f0d870509fe 962 }
Sergunb 0:8f0d870509fe 963 pl_data->condition |= gc_state.modal.coolant; // Set condition flag for planner use.
Sergunb 0:8f0d870509fe 964
Sergunb 0:8f0d870509fe 965 // [9. Override control ]: NOT SUPPORTED. Always enabled. Except for a Grbl-only parking control.
Sergunb 0:8f0d870509fe 966 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
Sergunb 0:8f0d870509fe 967 if (gc_state.modal.override != gc_block.modal.override) {
Sergunb 0:8f0d870509fe 968 gc_state.modal.override = gc_block.modal.override;
Sergunb 0:8f0d870509fe 969 mc_override_ctrl_update(gc_state.modal.override);
Sergunb 0:8f0d870509fe 970 }
Sergunb 0:8f0d870509fe 971 #endif
Sergunb 0:8f0d870509fe 972
Sergunb 0:8f0d870509fe 973 // [10. Dwell ]:
Sergunb 0:8f0d870509fe 974 if (gc_block.non_modal_command == NON_MODAL_DWELL) { mc_dwell(gc_block.values.p); }
Sergunb 0:8f0d870509fe 975
Sergunb 0:8f0d870509fe 976 // [11. Set active plane ]:
Sergunb 0:8f0d870509fe 977 gc_state.modal.plane_select = gc_block.modal.plane_select;
Sergunb 0:8f0d870509fe 978
Sergunb 0:8f0d870509fe 979 // [12. Set length units ]:
Sergunb 0:8f0d870509fe 980 gc_state.modal.units = gc_block.modal.units;
Sergunb 0:8f0d870509fe 981
Sergunb 0:8f0d870509fe 982 // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED
Sergunb 0:8f0d870509fe 983 // gc_state.modal.cutter_comp = gc_block.modal.cutter_comp; // NOTE: Not needed since always disabled.
Sergunb 0:8f0d870509fe 984
Sergunb 0:8f0d870509fe 985 // [14. Cutter length compensation ]: G43.1 and G49 supported. G43 NOT SUPPORTED.
Sergunb 0:8f0d870509fe 986 // NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms
Sergunb 0:8f0d870509fe 987 // of execution. The error-checking step would simply load the offset value into the correct
Sergunb 0:8f0d870509fe 988 // axis of the block XYZ value array.
Sergunb 0:8f0d870509fe 989 if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates a change.
Sergunb 0:8f0d870509fe 990 gc_state.modal.tool_length = gc_block.modal.tool_length;
Sergunb 0:8f0d870509fe 991 if (gc_state.modal.tool_length == TOOL_LENGTH_OFFSET_CANCEL) { // G49
Sergunb 0:8f0d870509fe 992 gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS] = 0.0f;
Sergunb 0:8f0d870509fe 993 } // else G43.1
Sergunb 0:8f0d870509fe 994 if ( gc_state.tool_length_offset != gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS] ) {
Sergunb 0:8f0d870509fe 995 gc_state.tool_length_offset = gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS];
Sergunb 0:8f0d870509fe 996 system_flag_wco_change();
Sergunb 0:8f0d870509fe 997 }
Sergunb 0:8f0d870509fe 998 }
Sergunb 0:8f0d870509fe 999
Sergunb 0:8f0d870509fe 1000 // [15. Coordinate system selection ]:
Sergunb 0:8f0d870509fe 1001 if (gc_state.modal.coord_select != gc_block.modal.coord_select) {
Sergunb 0:8f0d870509fe 1002 gc_state.modal.coord_select = gc_block.modal.coord_select;
Sergunb 0:8f0d870509fe 1003 memcpy(gc_state.coord_system,block_coord_system,N_AXIS*sizeof(float));
Sergunb 0:8f0d870509fe 1004 system_flag_wco_change();
Sergunb 0:8f0d870509fe 1005 }
Sergunb 0:8f0d870509fe 1006
Sergunb 0:8f0d870509fe 1007 // [16. Set path control mode ]: G61.1/G64 NOT SUPPORTED
Sergunb 0:8f0d870509fe 1008 // gc_state.modal.control = gc_block.modal.control; // NOTE: Always default.
Sergunb 0:8f0d870509fe 1009
Sergunb 0:8f0d870509fe 1010 // [17. Set distance mode ]:
Sergunb 0:8f0d870509fe 1011 gc_state.modal.distance = gc_block.modal.distance;
Sergunb 0:8f0d870509fe 1012
Sergunb 0:8f0d870509fe 1013 // [18. Set retract mode ]: NOT SUPPORTED
Sergunb 0:8f0d870509fe 1014
Sergunb 0:8f0d870509fe 1015 // [19. Go to predefined position, Set G10, or Set axis offsets ]:
Sergunb 0:8f0d870509fe 1016 switch(gc_block.non_modal_command) {
Sergunb 0:8f0d870509fe 1017 case NON_MODAL_SET_COORDINATE_DATA:
Sergunb 0:8f0d870509fe 1018 settings_write_coord_data(coord_select,gc_block.values.ijk);
Sergunb 0:8f0d870509fe 1019 // Update system coordinate system if currently active.
Sergunb 0:8f0d870509fe 1020 if (gc_state.modal.coord_select == coord_select) {
Sergunb 0:8f0d870509fe 1021 memcpy(gc_state.coord_system,gc_block.values.ijk,N_AXIS*sizeof(float));
Sergunb 0:8f0d870509fe 1022 system_flag_wco_change();
Sergunb 0:8f0d870509fe 1023 }
Sergunb 0:8f0d870509fe 1024 break;
Sergunb 0:8f0d870509fe 1025 case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
Sergunb 0:8f0d870509fe 1026 // Move to intermediate position before going home. Obeys current coordinate system and offsets
Sergunb 0:8f0d870509fe 1027 // and absolute and incremental modes.
Sergunb 0:8f0d870509fe 1028 pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag.
Sergunb 0:8f0d870509fe 1029 if (axis_command) { mc_line(gc_block.values.xyz, pl_data); }
Sergunb 0:8f0d870509fe 1030 mc_line(gc_block.values.ijk, pl_data);
Sergunb 0:8f0d870509fe 1031 memcpy(gc_state.position, gc_block.values.ijk, N_AXIS*sizeof(float));
Sergunb 0:8f0d870509fe 1032 break;
Sergunb 0:8f0d870509fe 1033 case NON_MODAL_SET_HOME_0:
Sergunb 0:8f0d870509fe 1034 settings_write_coord_data(SETTING_INDEX_G28,gc_state.position);
Sergunb 0:8f0d870509fe 1035 break;
Sergunb 0:8f0d870509fe 1036 case NON_MODAL_SET_HOME_1:
Sergunb 0:8f0d870509fe 1037 settings_write_coord_data(SETTING_INDEX_G30,gc_state.position);
Sergunb 0:8f0d870509fe 1038 break;
Sergunb 0:8f0d870509fe 1039 case NON_MODAL_SET_COORDINATE_OFFSET:
Sergunb 0:8f0d870509fe 1040 memcpy(gc_state.coord_offset,gc_block.values.xyz,sizeof(gc_block.values.xyz));
Sergunb 0:8f0d870509fe 1041 system_flag_wco_change();
Sergunb 0:8f0d870509fe 1042 break;
Sergunb 0:8f0d870509fe 1043 case NON_MODAL_RESET_COORDINATE_OFFSET:
Sergunb 0:8f0d870509fe 1044 clear_vector(gc_state.coord_offset); // Disable G92 offsets by zeroing offset vector.
Sergunb 0:8f0d870509fe 1045 system_flag_wco_change();
Sergunb 0:8f0d870509fe 1046 break;
Sergunb 0:8f0d870509fe 1047 }
Sergunb 0:8f0d870509fe 1048
Sergunb 0:8f0d870509fe 1049
Sergunb 0:8f0d870509fe 1050 // [20. Motion modes ]:
Sergunb 0:8f0d870509fe 1051 // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
Sergunb 0:8f0d870509fe 1052 // Enter motion modes only if there are axis words or a motion mode command word in the block.
Sergunb 0:8f0d870509fe 1053 gc_state.modal.motion = gc_block.modal.motion;
Sergunb 0:8f0d870509fe 1054 if (gc_state.modal.motion != MOTION_MODE_NONE) {
Sergunb 0:8f0d870509fe 1055 if (axis_command == AXIS_COMMAND_MOTION_MODE) {
Sergunb 0:8f0d870509fe 1056 uint8_t gc_update_pos = GC_UPDATE_POS_TARGET;
Sergunb 0:8f0d870509fe 1057 if (gc_state.modal.motion == MOTION_MODE_LINEAR) {
Sergunb 0:8f0d870509fe 1058 mc_line(gc_block.values.xyz, pl_data);
Sergunb 0:8f0d870509fe 1059 } else if (gc_state.modal.motion == MOTION_MODE_SEEK) {
Sergunb 0:8f0d870509fe 1060 pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag.
Sergunb 0:8f0d870509fe 1061 mc_line(gc_block.values.xyz, pl_data);
Sergunb 0:8f0d870509fe 1062 } else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) {
Sergunb 0:8f0d870509fe 1063 mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r,
Sergunb 0:8f0d870509fe 1064 axis_0, axis_1, axis_linear, bit_istrue(gc_parser_flags, GC_PARSER_ARC_IS_CLOCKWISE));
Sergunb 0:8f0d870509fe 1065 } else {
Sergunb 0:8f0d870509fe 1066 // NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So
Sergunb 0:8f0d870509fe 1067 // upon a successful probing cycle, the machine position and the returned value should be the same.
Sergunb 0:8f0d870509fe 1068 #ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES
Sergunb 0:8f0d870509fe 1069 pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE;
Sergunb 0:8f0d870509fe 1070 #endif
Sergunb 0:8f0d870509fe 1071 gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags);
Sergunb 0:8f0d870509fe 1072 }
Sergunb 0:8f0d870509fe 1073
Sergunb 0:8f0d870509fe 1074 // As far as the parser is concerned, the position is now == target. In reality the
Sergunb 0:8f0d870509fe 1075 // motion control system might still be processing the action and the real tool position
Sergunb 0:8f0d870509fe 1076 // in any intermediate location.
Sergunb 0:8f0d870509fe 1077 if (gc_update_pos == GC_UPDATE_POS_TARGET) {
Sergunb 0:8f0d870509fe 1078 memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); // gc_state.position[] = gc_block.values.xyz[]
Sergunb 0:8f0d870509fe 1079 } else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) {
Sergunb 0:8f0d870509fe 1080 gc_sync_position(); // gc_state.position[] = sys_position
Sergunb 0:8f0d870509fe 1081 } // == GC_UPDATE_POS_NONE
Sergunb 0:8f0d870509fe 1082 }
Sergunb 0:8f0d870509fe 1083
Sergunb 0:8f0d870509fe 1084 }
Sergunb 0:8f0d870509fe 1085
Sergunb 0:8f0d870509fe 1086 // [21. Program flow ]:
Sergunb 0:8f0d870509fe 1087 // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may
Sergunb 0:8f0d870509fe 1088 // refill and can only be resumed by the cycle start run-time command.
Sergunb 0:8f0d870509fe 1089 gc_state.modal.program_flow = gc_block.modal.program_flow;
Sergunb 0:8f0d870509fe 1090 if (gc_state.modal.program_flow) {
Sergunb 0:8f0d870509fe 1091 protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on.
Sergunb 0:8f0d870509fe 1092 if (gc_state.modal.program_flow == PROGRAM_FLOW_PAUSED) {
Sergunb 0:8f0d870509fe 1093 if (sys.state != STATE_CHECK_MODE) {
Sergunb 0:8f0d870509fe 1094 system_set_exec_state_flag(EXEC_FEED_HOLD); // Use feed hold for program pause.
Sergunb 0:8f0d870509fe 1095 protocol_execute_realtime(); // Execute suspend.
Sergunb 0:8f0d870509fe 1096 }
Sergunb 0:8f0d870509fe 1097 } else { // == PROGRAM_FLOW_COMPLETED
Sergunb 0:8f0d870509fe 1098 // Upon program complete, only a subset of g-codes reset to certain defaults, according to
Sergunb 0:8f0d870509fe 1099 // LinuxCNC's program end descriptions and testing. Only modal groups [G-code 1,2,3,5,7,12]
Sergunb 0:8f0d870509fe 1100 // and [M-code 7,8,9] reset to [G1,G17,G90,G94,G40,G54,M5,M9,M48]. The remaining modal groups
Sergunb 0:8f0d870509fe 1101 // [G-code 4,6,8,10,13,14,15] and [M-code 4,5,6] and the modal words [F,S,T,H] do not reset.
Sergunb 0:8f0d870509fe 1102 gc_state.modal.motion = MOTION_MODE_LINEAR;
Sergunb 0:8f0d870509fe 1103 gc_state.modal.plane_select = PLANE_SELECT_XY;
Sergunb 0:8f0d870509fe 1104 gc_state.modal.distance = DISTANCE_MODE_ABSOLUTE;
Sergunb 0:8f0d870509fe 1105 gc_state.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN;
Sergunb 0:8f0d870509fe 1106 // gc_state.modal.cutter_comp = CUTTER_COMP_DISABLE; // Not supported.
Sergunb 0:8f0d870509fe 1107 gc_state.modal.coord_select = 0; // G54
Sergunb 0:8f0d870509fe 1108 gc_state.modal.spindle = SPINDLE_DISABLE;
Sergunb 0:8f0d870509fe 1109 gc_state.modal.coolant = COOLANT_DISABLE;
Sergunb 0:8f0d870509fe 1110 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
Sergunb 0:8f0d870509fe 1111 #ifdef DEACTIVATE_PARKING_UPON_INIT
Sergunb 0:8f0d870509fe 1112 gc_state.modal.override = OVERRIDE_DISABLED;
Sergunb 0:8f0d870509fe 1113 #else
Sergunb 0:8f0d870509fe 1114 gc_state.modal.override = OVERRIDE_PARKING_MOTION;
Sergunb 0:8f0d870509fe 1115 #endif
Sergunb 0:8f0d870509fe 1116 #endif
Sergunb 0:8f0d870509fe 1117
Sergunb 0:8f0d870509fe 1118 #ifdef RESTORE_OVERRIDES_AFTER_PROGRAM_END
Sergunb 0:8f0d870509fe 1119 sys.f_override = DEFAULT_FEED_OVERRIDE;
Sergunb 0:8f0d870509fe 1120 sys.r_override = DEFAULT_RAPID_OVERRIDE;
Sergunb 0:8f0d870509fe 1121 sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE;
Sergunb 0:8f0d870509fe 1122 #endif
Sergunb 0:8f0d870509fe 1123
Sergunb 0:8f0d870509fe 1124 // Execute coordinate change and spindle/coolant stop.
Sergunb 0:8f0d870509fe 1125 if (sys.state != STATE_CHECK_MODE) {
Sergunb 0:8f0d870509fe 1126 if (!(settings_read_coord_data(gc_state.modal.coord_select,gc_state.coord_system))) { FAIL(STATUS_SETTING_READ_FAIL); }
Sergunb 0:8f0d870509fe 1127 system_flag_wco_change(); // Set to refresh immediately just in case something altered.
Sergunb 0:8f0d870509fe 1128 spindle_set_state(SPINDLE_DISABLE,0.0f);
Sergunb 0:8f0d870509fe 1129 coolant_set_state(COOLANT_DISABLE);
Sergunb 0:8f0d870509fe 1130 }
Sergunb 0:8f0d870509fe 1131 report_feedback_message(MESSAGE_PROGRAM_END);
Sergunb 0:8f0d870509fe 1132 }
Sergunb 0:8f0d870509fe 1133 gc_state.modal.program_flow = PROGRAM_FLOW_RUNNING; // Reset program flow.
Sergunb 0:8f0d870509fe 1134 }
Sergunb 0:8f0d870509fe 1135
Sergunb 0:8f0d870509fe 1136 // TODO: % to denote start of program.
Sergunb 0:8f0d870509fe 1137
Sergunb 0:8f0d870509fe 1138 return(STATUS_OK);
Sergunb 0:8f0d870509fe 1139 }
Sergunb 0:8f0d870509fe 1140
Sergunb 0:8f0d870509fe 1141
Sergunb 0:8f0d870509fe 1142 /*
Sergunb 0:8f0d870509fe 1143 Not supported:
Sergunb 0:8f0d870509fe 1144
Sergunb 0:8f0d870509fe 1145 - Canned cycles
Sergunb 0:8f0d870509fe 1146 - Tool radius compensation
Sergunb 0:8f0d870509fe 1147 - A,B,C-axes
Sergunb 0:8f0d870509fe 1148 - Evaluation of expressions
Sergunb 0:8f0d870509fe 1149 - Variables
Sergunb 0:8f0d870509fe 1150 - Override control (TBD)
Sergunb 0:8f0d870509fe 1151 - Tool changes
Sergunb 0:8f0d870509fe 1152 - Switches
Sergunb 0:8f0d870509fe 1153
Sergunb 0:8f0d870509fe 1154 (*) Indicates optional parameter, enabled through config.h and re-compile
Sergunb 0:8f0d870509fe 1155 group 0 = {G92.2, G92.3} (Non modal: Cancel and re-enable G92 offsets)
Sergunb 0:8f0d870509fe 1156 group 1 = {G81 - G89} (Motion modes: Canned cycles)
Sergunb 0:8f0d870509fe 1157 group 4 = {M1} (Optional stop, ignored)
Sergunb 0:8f0d870509fe 1158 group 6 = {M6} (Tool change)
Sergunb 0:8f0d870509fe 1159 group 7 = {G41, G42} cutter radius compensation (G40 is supported)
Sergunb 0:8f0d870509fe 1160 group 8 = {G43} tool length offset (G43.1/G49 are supported)
Sergunb 0:8f0d870509fe 1161 group 8 = {M7*} enable mist coolant (* Compile-option)
Sergunb 0:8f0d870509fe 1162 group 9 = {M48, M49, M56*} enable/disable override switches (* Compile-option)
Sergunb 0:8f0d870509fe 1163 group 10 = {G98, G99} return mode canned cycles
Sergunb 0:8f0d870509fe 1164 group 13 = {G61.1, G64} path control mode (G61 is supported)
Sergunb 0:8f0d870509fe 1165 */