From 20fbe3b588c7baa6bafdc42cc38d2d79d3af5340 Mon Sep 17 00:00:00 2001 From: Patrick Felixberger Date: Tue, 1 Jan 2019 18:20:52 +0100 Subject: [PATCH] Fixed backlash errors with soft reset; Code refactoring --- HAL/GPIO/GPIO.c | 4 +- HAL/SPI/SPI.c | 26 ++- HAL/SPI/SPI.h | 6 +- HAL/STM32/stm32f4xx_it.c | 1 + grbl/GCode.c | 468 ++++++++++++++++++++++++++------------- grbl/GCode.h | 41 ++-- grbl/MotionControl.c | 15 ++ grbl/MotionControl.h | 3 + main.c | 6 +- 9 files changed, 384 insertions(+), 186 deletions(-) diff --git a/HAL/GPIO/GPIO.c b/HAL/GPIO/GPIO.c index 51f6e89..661bc75 100644 --- a/HAL/GPIO/GPIO.c +++ b/HAL/GPIO/GPIO.c @@ -74,7 +74,7 @@ static void GPIO_InitStepper(void) GPIO_InitTypeDef GPIO_InitStructure; /* GPIO Configuration: D13, D12, D11, D7, D8, D2 */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4 | GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; @@ -169,7 +169,7 @@ static void GPIO_InitSystem(void) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN; GPIO_Init(GPIOA, &GPIO_InitStructure); diff --git a/HAL/SPI/SPI.c b/HAL/SPI/SPI.c index 1f47778..5796e73 100644 --- a/HAL/SPI/SPI.c +++ b/HAL/SPI/SPI.c @@ -108,12 +108,12 @@ void Spi_Init(SPI_TypeDef *SPIx, SPI_Mode mode) // Periph clock enable RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); - GPIO_PinAFConfig(GPIOC, GPIO_PinSource3, GPIO_AF_SPI3); - GPIO_PinAFConfig(GPIOC, GPIO_PinSource4, GPIO_AF_SPI3); - GPIO_PinAFConfig(GPIOC, GPIO_PinSource5, GPIO_AF_SPI3); + GPIO_PinAFConfig(GPIOC, GPIO_PinSource10, GPIO_AF_SPI3); + GPIO_PinAFConfig(GPIOC, GPIO_PinSource11, GPIO_AF_SPI3); + GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_SPI3); // Configure SPI pins: SCK, MOSI - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_5; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_12; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; @@ -121,7 +121,7 @@ void Spi_Init(SPI_TypeDef *SPIx, SPI_Mode mode) GPIO_Init(GPIOC, &GPIO_InitStructure); // Configure pins: MISO - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; GPIO_Init(GPIOC, &GPIO_InitStructure); // Reset SPI Interface @@ -132,10 +132,24 @@ void Spi_Init(SPI_TypeDef *SPIx, SPI_Mode mode) SPI_InitStructure.SPI_Mode = SPI_Mode_Master; SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; - SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4; SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; SPI_InitStructure.SPI_CRCPolynomial = 7; SPI_Init(SPIx, &SPI_InitStructure); + + // Initialize chip select + RCC_AHB1PeriphClockCmd(SPI3_CS_GPIO_CLK, ENABLE); + + // Configure CS pin + GPIO_InitStructure.GPIO_Pin = SPI3_CS_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(SPI3_CS_GPIO_PORT, &GPIO_InitStructure); + + // Deselect chip + GPIO_SetBits(SPI3_CS_GPIO_PORT, SPI3_CS_PIN); } SPI_CalculateCRC(SPIx, DISABLE); diff --git a/HAL/SPI/SPI.h b/HAL/SPI/SPI.h index 0106ad5..10a382c 100644 --- a/HAL/SPI/SPI.h +++ b/HAL/SPI/SPI.h @@ -19,9 +19,9 @@ #define SPI2_CS_GPIO_CLK RCC_APB2Periph_GPIOB // Default SPI3 CS Pins: PA15, PA4(AF) -#define SPI3_CS_PIN GPIO_Pin_15 -#define SPI3_CS_GPIO_PORT GPIOA -#define SPI3_CS_GPIO_CLK RCC_APB2Periph_GPIOA +#define SPI3_CS_PIN GPIO_Pin_2 +#define SPI3_CS_GPIO_PORT GPIOD +#define SPI3_CS_GPIO_CLK RCC_AHB1Periph_GPIOD #define SPI_PRESCALER_2 0x0000 diff --git a/HAL/STM32/stm32f4xx_it.c b/HAL/STM32/stm32f4xx_it.c index 5d13ff8..a377b3d 100644 --- a/HAL/STM32/stm32f4xx_it.c +++ b/HAL/STM32/stm32f4xx_it.c @@ -67,6 +67,7 @@ uint32_t millis(void) return gMillis; } + /** * @brief This function handles NMI exception. * @param None diff --git a/grbl/GCode.c b/grbl/GCode.c index 852fcf9..a67a5c8 100644 --- a/grbl/GCode.c +++ b/grbl/GCode.c @@ -4,7 +4,7 @@ Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud - Copyright (c) 2017 Patrick F. + Copyright (c) 2018 Patrick F. Grbl-Advanced is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -67,7 +67,8 @@ void GC_Init(void) memset(&gc_state, 0, sizeof(Parser_State_t)); // Load default G54 coordinate system. - if(!(Settings_ReadCoordData(gc_state.modal.coord_select, gc_state.coord_system))) { + if(!(Settings_ReadCoordData(gc_state.modal.coord_select, gc_state.coord_system))) + { Report_StatusMessage(STATUS_SETTING_READ_FAIL); } } @@ -137,19 +138,23 @@ uint8_t GC_ExecuteLine(char *line) // Start parsing after `$J=` char_counter = 3; } - else { + else + { char_counter = 0; } - while(line[char_counter] != 0) { // Loop until no more g-code words in line. - // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out. + while(line[char_counter] != 0) // Loop until no more g-code words in line. + { + // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out. letter = line[char_counter]; - if((letter < 'A') || (letter > 'Z')) { + if((letter < 'A') || (letter > 'Z')) + { return STATUS_EXPECTED_COMMAND_LETTER; } // [Expected word letter] char_counter++; - if(!Read_Float(line, &char_counter, &value)) { + if(!Read_Float(line, &char_counter, &value)) + { return STATUS_BAD_NUMBER_FORMAT; } // [Expected word value] @@ -178,8 +183,10 @@ uint8_t GC_ExecuteLine(char *line) case 10: case 28: case 30: case 92: // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block. // * G43.1 is also an axis command but is not explicitly defined this way. - if(mantissa == 0) { // Ignore G28.1, G30.1, and G92.1 - if (axis_command) { + if(mantissa == 0) // Ignore G28.1, G30.1, and G92.1 + { + if (axis_command) + { // [Axis word/command conflict] return STATUS_GCODE_AXIS_COMMAND_CONFLICT; } @@ -191,8 +198,10 @@ uint8_t GC_ExecuteLine(char *line) word_bit = MODAL_GROUP_G0; gc_block.non_modal_command = int_value; - if((int_value == 28) || (int_value == 30) || (int_value == 92)) { - if(!((mantissa == 0) || (mantissa == 10))) { + if((int_value == 28) || (int_value == 30) || (int_value == 92)) + { + if(!((mantissa == 0) || (mantissa == 10))) + { return STATUS_GCODE_UNSUPPORTED_COMMAND; } gc_block.non_modal_command += mantissa; @@ -203,7 +212,8 @@ uint8_t GC_ExecuteLine(char *line) case 0: case 1: case 2: case 3: case 38: // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. // * G43.1 is also an axis command but is not explicitly defined this way. - if(axis_command) { + if(axis_command) + { // [Axis word/command conflict] return STATUS_GCODE_AXIS_COMMAND_CONFLICT; } @@ -214,8 +224,10 @@ uint8_t GC_ExecuteLine(char *line) word_bit = MODAL_GROUP_G1; gc_block.modal.motion = int_value; - if(int_value == 38) { - if(!((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50))) { + if(int_value == 38) + { + if(!((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50))) + { return STATUS_GCODE_UNSUPPORTED_COMMAND; // [Unsupported G38.x command] } gc_block.modal.motion += (mantissa/10)+100; @@ -229,13 +241,16 @@ uint8_t GC_ExecuteLine(char *line) break; case 90: case 91: - if(mantissa == 0) { + if(mantissa == 0) + { word_bit = MODAL_GROUP_G3; gc_block.modal.distance = int_value - 90; } - else { + else + { word_bit = MODAL_GROUP_G4; - if ((mantissa != 10) || (int_value == 90)) { + if ((mantissa != 10) || (int_value == 90)) + { // [G90.1 not supported] return STATUS_GCODE_UNSUPPORTED_COMMAND; } @@ -266,20 +281,24 @@ uint8_t GC_ExecuteLine(char *line) // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed, // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49 // all are explicit axis commands, regardless if they require axis words or not. - if(axis_command) { + if(axis_command) + { // [Axis word/command conflict] return STATUS_GCODE_AXIS_COMMAND_CONFLICT; } axis_command = AXIS_COMMAND_TOOL_LENGTH_OFFSET; - if(int_value == 49) { // G49 - gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_CANCEL; + if(int_value == 49) // G49 + { + gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_CANCEL; } - else if(mantissa == 10) { // G43.1 - gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC; + else if(mantissa == 10) // G43.1 + { + gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC; } - else { + else + { // [Unsupported G43.x command] return STATUS_GCODE_UNSUPPORTED_COMMAND; } @@ -295,7 +314,8 @@ uint8_t GC_ExecuteLine(char *line) case 61: word_bit = MODAL_GROUP_G13; - if(mantissa != 0) { + if(mantissa != 0) + { // [G61.1 not supported] return STATUS_GCODE_UNSUPPORTED_COMMAND; } @@ -306,14 +326,16 @@ uint8_t GC_ExecuteLine(char *line) return STATUS_GCODE_UNSUPPORTED_COMMAND; // [Unsupported G command] } - if(mantissa > 0) { + if(mantissa > 0) + { // [Unsupported or invalid Gxx.x command] return STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER; } // Check for more than one command per modal group violations in the current block // NOTE: Variable 'word_bit' is always assigned, if the command is valid. - if(BIT_IS_TRUE(command_words,BIT(word_bit))) { + if(BIT_IS_TRUE(command_words,BIT(word_bit))) + { return STATUS_GCODE_MODAL_GROUP_VIOLATION; } command_words |= BIT(word_bit); @@ -321,7 +343,8 @@ uint8_t GC_ExecuteLine(char *line) case 'M': // Determine 'M' command and its modal group - if(mantissa > 0) { + if(mantissa > 0) + { // [No Mxx.x commands] return STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER; } @@ -331,7 +354,8 @@ uint8_t GC_ExecuteLine(char *line) case 0: case 1: case 2: case 30: word_bit = MODAL_GROUP_M4; - switch(int_value) { + switch(int_value) + { case 0: gc_block.modal.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause @@ -399,7 +423,8 @@ uint8_t GC_ExecuteLine(char *line) // Check for more than one command per modal group violations in the current block // NOTE: Variable 'word_bit' is always assigned, if the command is valid. - if(BIT_IS_TRUE(command_words, BIT(word_bit))) { + if(BIT_IS_TRUE(command_words, BIT(word_bit))) + { return STATUS_GCODE_MODAL_GROUP_VIOLATION; } command_words |= BIT(word_bit); @@ -425,11 +450,12 @@ uint8_t GC_ExecuteLine(char *line) case 'N': word_bit = WORD_N; gc_block.values.n = trunc(value); break; case 'P': word_bit = WORD_P; gc_block.values.p = value; break; // NOTE: For certain commands, P value must be an integer, but none of these commands are supported. - // case 'Q': // Not supported + case 'Q': word_bit = WORD_Q; gc_block.values.q = value; break; case 'R': word_bit = WORD_R; gc_block.values.r = value; break; case 'S': word_bit = WORD_S; gc_block.values.s = value; break; case 'T': word_bit = WORD_T; - if(value > MAX_TOOL_NUMBER) { + if(value > MAX_TOOL_NUMBER) + { return STATUS_GCODE_MAX_VALUE_EXCEEDED; } gc_block.values.t = int_value; @@ -443,14 +469,17 @@ uint8_t GC_ExecuteLine(char *line) } // NOTE: Variable 'word_bit' is always assigned, if the non-command letter is valid. - if (BIT_IS_TRUE(value_words, BIT(word_bit))) { + if (BIT_IS_TRUE(value_words, BIT(word_bit))) + { return STATUS_GCODE_WORD_REPEATED; } // [Word repeated] // Check for invalid negative values for words F, N, P, T, and S. // NOTE: Negative value check is done here simply for code-efficiency. - if(BIT(word_bit) & (BIT(WORD_F)|BIT(WORD_N)|BIT(WORD_P)|BIT(WORD_T)|BIT(WORD_S))) { - if(value < 0.0) { + if(BIT(word_bit) & (BIT(WORD_F)|BIT(WORD_N)|BIT(WORD_P)|BIT(WORD_T)|BIT(WORD_S))) + { + if(value < 0.0) + { // [Word value cannot be negative] return STATUS_NEGATIVE_VALUE; } @@ -492,17 +521,21 @@ uint8_t GC_ExecuteLine(char *line) // Determine implicit axis command conditions. Axis words have been passed, but no explicit axis // command has been sent. If so, set axis command to current motion mode. - if(axis_words) { - if(!axis_command) { + if(axis_words) + { + if(!axis_command) + { // Assign implicit motion-mode axis_command = AXIS_COMMAND_MOTION_MODE; } } // Check for valid line number N value. - if(BIT_IS_TRUE(value_words,BIT(WORD_N))) { + if(BIT_IS_TRUE(value_words,BIT(WORD_N))) + { // Line number value cannot be less than zero (done) or greater than max line number. - if(gc_block.values.n > MAX_LINE_NUMBER) { + if(gc_block.values.n > MAX_LINE_NUMBER) + { // [Exceeds max line number] return STATUS_GCODE_INVALID_LINE_NUMBER; } @@ -522,21 +555,27 @@ uint8_t GC_ExecuteLine(char *line) // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate // is not defined after switching to G94 from G93. // NOTE: For jogging, ignore prior feed rate mode. Enforce G94 and check for required F word. - if(gc_parser_flags & GC_PARSER_JOG_MOTION) { - if(BIT_IS_FALSE(value_words, BIT(WORD_F))) { + if(gc_parser_flags & GC_PARSER_JOG_MOTION) + { + if(BIT_IS_FALSE(value_words, BIT(WORD_F))) + { return STATUS_GCODE_UNDEFINED_FEED_RATE; } - if(gc_block.modal.units == UNITS_MODE_INCHES) { + if(gc_block.modal.units == UNITS_MODE_INCHES) + { gc_block.values.f *= MM_PER_INCH; } } - else { + else + { if(gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93 // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here. if(axis_command == AXIS_COMMAND_MOTION_MODE) { - if((gc_block.modal.motion != MOTION_MODE_NONE) && (gc_block.modal.motion != MOTION_MODE_SEEK)) { - if(BIT_IS_FALSE(value_words, BIT(WORD_F))) { + if((gc_block.modal.motion != MOTION_MODE_NONE) && (gc_block.modal.motion != MOTION_MODE_SEEK)) + { + if(BIT_IS_FALSE(value_words, BIT(WORD_F))) + { // [F word missing] return STATUS_GCODE_UNDEFINED_FEED_RATE; } @@ -558,12 +597,15 @@ uint8_t GC_ExecuteLine(char *line) else { // = G94 // - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value. if(gc_state.modal.feed_rate == FEED_RATE_MODE_UNITS_PER_MIN) { // Last state is also G94 - if(BIT_IS_TRUE(value_words, BIT(WORD_F))) { - if(gc_block.modal.units == UNITS_MODE_INCHES) { + if(BIT_IS_TRUE(value_words, BIT(WORD_F))) + { + if(gc_block.modal.units == UNITS_MODE_INCHES) + { gc_block.values.f *= MM_PER_INCH; } } - else { + else + { gc_block.values.f = gc_state.feed_rate; // Push last state feed rate } } // Else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word value. @@ -572,7 +614,8 @@ uint8_t GC_ExecuteLine(char *line) // bit_false(value_words,bit(WORD_F)); // NOTE: Single-meaning value word. Set at end of error-checking. // [4. Set spindle speed ]: S is negative (done.) - if(BIT_IS_FALSE(value_words, BIT(WORD_S))) { + if(BIT_IS_FALSE(value_words, BIT(WORD_S))) + { gc_block.values.s = gc_state.spindle_speed; } // bit_false(value_words,bit(WORD_S)); // NOTE: Single-meaning value word. Set at end of error-checking. @@ -586,8 +629,10 @@ uint8_t GC_ExecuteLine(char *line) // [9. Override control ]: Not supported except for a Grbl-only parking motion override control. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if(BIT_IS_TRUE(command_words, BIT(MODAL_GROUP_M9))) { // Already set as enabled in parser. - if(BIT_IS_TRUE(value_words, BIT(WORD_P))) { - if(gc_block.values.p == 0.0) { + if(BIT_IS_TRUE(value_words, BIT(WORD_P))) + { + if(gc_block.values.p == 0.0) + { gc_block.modal.override = OVERRIDE_DISABLED; } BIT_FALSE(value_words, BIT(WORD_P)); @@ -596,8 +641,10 @@ uint8_t GC_ExecuteLine(char *line) #endif // [10. Dwell ]: P value missing. P is negative (done.) NOTE: See below. - if(gc_block.non_modal_command == NON_MODAL_DWELL) { - if(BIT_IS_FALSE(value_words, BIT(WORD_P))) { + if(gc_block.non_modal_command == NON_MODAL_DWELL) + { + if(BIT_IS_FALSE(value_words, BIT(WORD_P))) + { // [P word missing] return STATUS_GCODE_VALUE_WORD_MISSING; } @@ -628,9 +675,11 @@ uint8_t GC_ExecuteLine(char *line) // [12. Set length units ]: N/A // Pre-convert XYZ coordinate values to millimeters, if applicable. uint8_t idx; - if(gc_block.modal.units == UNITS_MODE_INCHES) { + if(gc_block.modal.units == UNITS_MODE_INCHES) + { for(idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used. - if(BIT_IS_TRUE(axis_words, BIT(idx)) ) { + if(BIT_IS_TRUE(axis_words, BIT(idx))) + { gc_block.values.xyz[idx] *= MM_PER_INCH; } } @@ -647,8 +696,10 @@ uint8_t GC_ExecuteLine(char *line) // axis that is configured (in config.h). There should be an error if the configured axis // is absent or if any of the other axis words are present. if(axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates called in block. - if(gc_block.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) { - if(axis_words ^ (1< N_COORDINATE_SYSTEM) { + if(gc_block.modal.coord_select > N_COORDINATE_SYSTEM) + { // [Greater than N sys] return STATUS_GCODE_UNSUPPORTED_COORD_SYS; } - if(gc_state.modal.coord_select != gc_block.modal.coord_select) { - if(!(Settings_ReadCoordData(gc_block.modal.coord_select, block_coord_system))) { + if(gc_state.modal.coord_select != gc_block.modal.coord_select) + { + if(!(Settings_ReadCoordData(gc_block.modal.coord_select, block_coord_system))) + { return STATUS_SETTING_READ_FAIL; } } @@ -690,28 +744,36 @@ uint8_t GC_ExecuteLine(char *line) // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.) // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing. // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing. - if(!axis_words) { + if(!axis_words) + { // [No axis words] return STATUS_GCODE_NO_AXIS_WORDS; } - if(BIT_IS_FALSE(value_words, ((1< N_COORDINATE_SYSTEM) { + if(coord_select > N_COORDINATE_SYSTEM) + { // [Greater than N sys] return STATUS_GCODE_UNSUPPORTED_COORD_SYS; } - if(gc_block.values.l != 20) { - if(gc_block.values.l == 2) { - if(BIT_IS_TRUE(value_words, BIT(WORD_R))) { + if(gc_block.values.l != 20) + { + if(gc_block.values.l == 2) + { + if(BIT_IS_TRUE(value_words, BIT(WORD_R))) + { // [G10 L2 R not supported] return STATUS_GCODE_UNSUPPORTED_COMMAND; } - } else { + } + else + { // [Unsupported L] return STATUS_GCODE_UNSUPPORTED_COMMAND; } @@ -719,17 +781,20 @@ uint8_t GC_ExecuteLine(char *line) BIT_FALSE(value_words, (BIT(WORD_L) | BIT(WORD_P))); // Determine coordinate system to change and try to load from EEPROM. - if(coord_select > 0) { + if(coord_select > 0) + { // Adjust P1-P6 index to EEPROM coordinate data indexing. coord_select--; } - else { + else + { // Index P0 as the active coordinate system coord_select = gc_block.modal.coord_select; } // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command. - if(!Settings_ReadCoordData(coord_select, gc_block.values.ijk)) { + if(!Settings_ReadCoordData(coord_select, gc_block.values.ijk)) + { // [EEPROM read fail] return STATUS_SETTING_READ_FAIL; } @@ -737,16 +802,20 @@ uint8_t GC_ExecuteLine(char *line) // Pre-calculate the coordinate data changes. for(idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used. // Update axes defined only in block. Always in machine coordinates. Can change non-active system. - if(BIT_IS_TRUE(axis_words, BIT(idx))) { - if(gc_block.values.l == 20) { + if(BIT_IS_TRUE(axis_words, BIT(idx))) + { + if(gc_block.values.l == 20) + { // L20: Update coordinate system axis at current position (with modifiers) with programmed value // WPos = MPos - WCS - G92 - TLO -> WCS = MPos - G92 - TLO - WPos gc_block.values.ijk[idx] = gc_state.position[idx] - gc_state.coord_offset[idx] - gc_block.values.xyz[idx]; - if(idx == TOOL_LENGTH_OFFSET_AXIS) { + if(idx == TOOL_LENGTH_OFFSET_AXIS) + { gc_block.values.ijk[idx] -= gc_state.tool_length_offset; } } - else { + else + { // L2: Update coordinate system axis to programmed value. gc_block.values.ijk[idx] = gc_block.values.xyz[idx]; } @@ -756,21 +825,26 @@ uint8_t GC_ExecuteLine(char *line) case NON_MODAL_SET_COORDINATE_OFFSET: // [G92 Errors]: No axis words. - if(!axis_words) { + if(!axis_words) + { // [No axis words] return STATUS_GCODE_NO_AXIS_WORDS; } + // Update axes defined only in block. Offsets current system to defined value. Does not update when // active coordinate system is selected, but is still active unless G92.1 disables it. for(idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used. - if(BIT_IS_TRUE(axis_words, BIT(idx)) ) { + if(BIT_IS_TRUE(axis_words, BIT(idx))) + { // WPos = MPos - WCS - G92 - TLO -> G92 = MPos - WCS - TLO - WPos gc_block.values.xyz[idx] = gc_state.position[idx] - block_coord_system[idx] - gc_block.values.xyz[idx]; - if(idx == TOOL_LENGTH_OFFSET_AXIS) { + if(idx == TOOL_LENGTH_OFFSET_AXIS) + { gc_block.values.xyz[idx] -= gc_state.tool_length_offset; } } - else { + else + { gc_block.values.xyz[idx] = gc_state.coord_offset[idx]; } } @@ -782,23 +856,30 @@ uint8_t GC_ExecuteLine(char *line) // modes applied. This includes the motion mode commands. We can now pre-compute the target position. // NOTE: Tool offsets may be appended to these conversions when/if this feature is added. if(axis_command != AXIS_COMMAND_TOOL_LENGTH_OFFSET) { // TLO block any axis command. - if(axis_words) { + if(axis_words) + { for(idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space. - if(BIT_IS_FALSE(axis_words, BIT(idx))) { + if(BIT_IS_FALSE(axis_words, BIT(idx))) + { gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position. } - else { + else + { // Update specified value according to distance mode or ignore if absolute override is active. // NOTE: G53 is never active with G28/30 since they are in the same modal group. - if(gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) { + if(gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) + { // Apply coordinate offsets based on distance mode. - if(gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) { + if(gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) + { gc_block.values.xyz[idx] += block_coord_system[idx] + gc_state.coord_offset[idx]; - if(idx == TOOL_LENGTH_OFFSET_AXIS) { + if(idx == TOOL_LENGTH_OFFSET_AXIS) + { gc_block.values.xyz[idx] += gc_state.tool_length_offset; } } - else { // Incremental mode + else + { // Incremental mode gc_block.values.xyz[idx] += gc_state.position[idx]; } } @@ -815,25 +896,34 @@ uint8_t GC_ExecuteLine(char *line) // [G28/30 Errors]: Cutter compensation is enabled. // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command. - if(gc_block.non_modal_command == NON_MODAL_GO_HOME_0) { - if(!Settings_ReadCoordData(SETTING_INDEX_G28, gc_block.values.ijk)) { + if(gc_block.non_modal_command == NON_MODAL_GO_HOME_0) + { + if(!Settings_ReadCoordData(SETTING_INDEX_G28, gc_block.values.ijk)) + { return STATUS_SETTING_READ_FAIL; } } - else { // == NON_MODAL_GO_HOME_1 - if (!Settings_ReadCoordData(SETTING_INDEX_G30, gc_block.values.ijk)) { + else // == NON_MODAL_GO_HOME_1 + { + if (!Settings_ReadCoordData(SETTING_INDEX_G30, gc_block.values.ijk)) + { return STATUS_SETTING_READ_FAIL; } } - if(axis_words) { + + if(axis_words) + { // Move only the axes specified in secondary move. - for(idx = 0; idx < N_AXIS; idx++) { - if(!(axis_words & (1<condition |= PL_COND_FLAG_INVERSE_TIME; } @@ -1205,13 +1331,18 @@ uint8_t GC_ExecuteLine(char *line) pl_data->feed_rate = gc_state.feed_rate; // Record data for planner use. // [4. Set spindle speed ]: - if((gc_state.spindle_speed != gc_block.values.s) || BIT_IS_TRUE(gc_parser_flags,GC_PARSER_LASER_FORCE_SYNC)) { - if(gc_state.modal.spindle != SPINDLE_DISABLE) { - if(BIT_IS_FALSE(gc_parser_flags, GC_PARSER_LASER_ISMOTION)) { - if(BIT_IS_TRUE(gc_parser_flags, GC_PARSER_LASER_DISABLE)) { + if((gc_state.spindle_speed != gc_block.values.s) || BIT_IS_TRUE(gc_parser_flags,GC_PARSER_LASER_FORCE_SYNC)) + { + if(gc_state.modal.spindle != SPINDLE_DISABLE) + { + if(BIT_IS_FALSE(gc_parser_flags, GC_PARSER_LASER_ISMOTION)) + { + if(BIT_IS_TRUE(gc_parser_flags, GC_PARSER_LASER_DISABLE)) + { Spindle_Sync(gc_state.modal.spindle, 0.0); } - else { + else + { Spindle_Sync(gc_state.modal.spindle, gc_block.values.s); } } @@ -1219,8 +1350,10 @@ uint8_t GC_ExecuteLine(char *line) gc_state.spindle_speed = gc_block.values.s; // Update spindle speed state. } + // NOTE: Pass zero spindle speed for all restricted laser motions. - if(BIT_IS_FALSE(gc_parser_flags, GC_PARSER_LASER_DISABLE)) { + if(BIT_IS_FALSE(gc_parser_flags, GC_PARSER_LASER_DISABLE)) + { pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use. } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already. @@ -1231,7 +1364,8 @@ uint8_t GC_ExecuteLine(char *line) // [6. Change tool ]: NOT SUPPORTED // [7. Spindle control ]: - if(gc_state.modal.spindle != gc_block.modal.spindle) { + if(gc_state.modal.spindle != gc_block.modal.spindle) + { // Update spindle control and apply spindle speed when enabling it in this block. // NOTE: All spindle state changes are synced, even in laser mode. Also, pl_data, // rather than gc_state, is used to manage laser state for non-laser motions. @@ -1242,7 +1376,8 @@ uint8_t GC_ExecuteLine(char *line) pl_data->condition |= gc_state.modal.spindle; // Set condition flag for planner use. // [8. Coolant control ]: - if (gc_state.modal.coolant != gc_block.modal.coolant) { + if (gc_state.modal.coolant != gc_block.modal.coolant) + { // NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states // can exist at the same time, while coolant disable clears all states. Coolant_Sync(gc_block.modal.coolant); @@ -1252,14 +1387,16 @@ uint8_t GC_ExecuteLine(char *line) // [9. Override control ]: NOT SUPPORTED. Always enabled. Except for a Grbl-only parking control. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if(gc_state.modal.override != gc_block.modal.override) { + if(gc_state.modal.override != gc_block.modal.override) + { gc_state.modal.override = gc_block.modal.override; MC_OverrideCtrlUpdate(gc_state.modal.override); } #endif // [10. Dwell ]: - if(gc_block.non_modal_command == NON_MODAL_DWELL) { + if(gc_block.non_modal_command == NON_MODAL_DWELL) + { MC_Dwell(gc_block.values.p); } @@ -1282,14 +1419,16 @@ uint8_t GC_ExecuteLine(char *line) gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS] = 0.0; } // else G43.1 - if(gc_state.tool_length_offset != gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS]) { + if(gc_state.tool_length_offset != gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS]) + { gc_state.tool_length_offset = gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS]; System_FlagWcoChange(); } } // [15. Coordinate system selection ]: - if(gc_state.modal.coord_select != gc_block.modal.coord_select) { + if(gc_state.modal.coord_select != gc_block.modal.coord_select) + { gc_state.modal.coord_select = gc_block.modal.coord_select; memcpy(gc_state.coord_system, block_coord_system, N_AXIS*sizeof(float)); System_FlagWcoChange(); @@ -1302,6 +1441,7 @@ uint8_t GC_ExecuteLine(char *line) gc_state.modal.distance = gc_block.modal.distance; // [18. Set retract mode ]: NOT SUPPORTED + gc_state.modal.retract = gc_block.modal.retract; // [19. Go to predefined position, Set G10, or Set axis offsets ]: switch(gc_block.non_modal_command) @@ -1309,7 +1449,8 @@ uint8_t GC_ExecuteLine(char *line) case NON_MODAL_SET_COORDINATE_DATA: Settings_WriteCoordData(coord_select,gc_block.values.ijk); // Update system coordinate system if currently active. - if(gc_state.modal.coord_select == coord_select) { + if(gc_state.modal.coord_select == coord_select) + { memcpy(gc_state.coord_system, gc_block.values.ijk, N_AXIS*sizeof(float)); System_FlagWcoChange(); } @@ -1319,7 +1460,8 @@ uint8_t GC_ExecuteLine(char *line) // Move to intermediate position before going home. Obeys current coordinate system and offsets // and absolute and incremental modes. pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. - if(axis_command) { + if(axis_command) + { MC_Line(gc_block.values.xyz, pl_data); } @@ -1351,17 +1493,22 @@ uint8_t GC_ExecuteLine(char *line) // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes. // Enter motion modes only if there are axis words or a motion mode command word in the block. gc_state.modal.motion = gc_block.modal.motion; - if(gc_state.modal.motion != MOTION_MODE_NONE) { - if(axis_command == AXIS_COMMAND_MOTION_MODE) { + if(gc_state.modal.motion != MOTION_MODE_NONE) + { + if(axis_command == AXIS_COMMAND_MOTION_MODE) + { uint8_t gc_update_pos = GC_UPDATE_POS_TARGET; - if(gc_state.modal.motion == MOTION_MODE_LINEAR) { + if(gc_state.modal.motion == MOTION_MODE_LINEAR) + { MC_Line(gc_block.values.xyz, pl_data); } - else if(gc_state.modal.motion == MOTION_MODE_SEEK) { + else if(gc_state.modal.motion == MOTION_MODE_SEEK) + { pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. MC_Line(gc_block.values.xyz, pl_data); } - else if((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) { + else if((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) + { MC_Arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r, axis_0, axis_1, axis_linear, BIT_IS_TRUE(gc_parser_flags, GC_PARSER_ARC_IS_CLOCKWISE)); } @@ -1377,10 +1524,12 @@ uint8_t GC_ExecuteLine(char *line) // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position // in any intermediate location. - if(gc_update_pos == GC_UPDATE_POS_TARGET) { + if(gc_update_pos == GC_UPDATE_POS_TARGET) + { memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); // gc_state.position[] = gc_block.values.xyz[] } - else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) { + else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) + { GC_SyncPosition(); // gc_state.position[] = sys_position } // == GC_UPDATE_POS_NONE } @@ -1390,11 +1539,14 @@ uint8_t GC_ExecuteLine(char *line) // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may // refill and can only be resumed by the cycle start run-time command. gc_state.modal.program_flow = gc_block.modal.program_flow; - if(gc_state.modal.program_flow) { + if(gc_state.modal.program_flow) + { Protocol_BufferSynchronize(); // Sync and finish all remaining buffered motions before moving on. - if(gc_state.modal.program_flow == PROGRAM_FLOW_PAUSED) { - if(sys.state != STATE_CHECK_MODE) { + if(gc_state.modal.program_flow == PROGRAM_FLOW_PAUSED) + { + if(sys.state != STATE_CHECK_MODE) + { System_SetExecStateFlag(EXEC_FEED_HOLD); // Use feed hold for program pause. Protocol_ExecuteRealtime(); // Execute suspend. } @@ -1427,8 +1579,10 @@ uint8_t GC_ExecuteLine(char *line) #endif // Execute coordinate change and spindle/coolant stop. - if(sys.state != STATE_CHECK_MODE) { - if(!(Settings_ReadCoordData(gc_state.modal.coord_select, gc_state.coord_system))) { + if(sys.state != STATE_CHECK_MODE) + { + if(!(Settings_ReadCoordData(gc_state.modal.coord_select, gc_state.coord_system))) + { return STATUS_SETTING_READ_FAIL; } diff --git a/grbl/GCode.h b/grbl/GCode.h index c012607..1e4df00 100644 --- a/grbl/GCode.h +++ b/grbl/GCode.h @@ -33,22 +33,24 @@ // a unique motion. These are defined in the NIST RS274-NGC v3 g-code standard, available online, // and are similar/identical to other g-code interpreters by manufacturers (Haas,Fanuc,Mazak,etc). // NOTE: Modal group define values must be sequential and starting from zero. -#define MODAL_GROUP_G0 0 // [G4,G10,G28,G28.1,G30,G30.1,G53,G92,G92.1] Non-modal -#define MODAL_GROUP_G1 1 // [G0,G1,G2,G3,G38.2,G38.3,G38.4,G38.5,G80] Motion -#define MODAL_GROUP_G2 2 // [G17,G18,G19] Plane selection -#define MODAL_GROUP_G3 3 // [G90,G91] Distance mode -#define MODAL_GROUP_G4 4 // [G91.1] Arc IJK distance mode -#define MODAL_GROUP_G5 5 // [G93,G94] Feed rate mode -#define MODAL_GROUP_G6 6 // [G20,G21] Units -#define MODAL_GROUP_G7 7 // [G40] Cutter radius compensation mode. G41/42 NOT SUPPORTED. -#define MODAL_GROUP_G8 8 // [G43.1,G49] Tool length offset -#define MODAL_GROUP_G12 9 // [G54,G55,G56,G57,G58,G59] Coordinate system selection -#define MODAL_GROUP_G13 10 // [G61] Control mode +#define MODAL_GROUP_G0 0 // [G4,G10,G28,G28.1,G30,G30.1,G53,G92,G92.1] Non-modal +#define MODAL_GROUP_G1 1 // [G0,G1,G2,G3,G38.2,G38.3,G38.4,G38.5,G80] Motion +#define MODAL_GROUP_G2 2 // [G17,G18,G19] Plane selection +#define MODAL_GROUP_G3 3 // [G90,G91] Distance mode +#define MODAL_GROUP_G4 4 // [G91.1] Arc IJK distance mode +#define MODAL_GROUP_G5 5 // [G93,G94] Feed rate mode +#define MODAL_GROUP_G6 6 // [G20,G21] Units +#define MODAL_GROUP_G7 7 // [G40] Cutter radius compensation mode. G41/42 NOT SUPPORTED. +#define MODAL_GROUP_G8 8 // [G43.1,G49] Tool length offset +#define MODAL_GROUP_G12 9 // [G54,G55,G56,G57,G58,G59] Coordinate system selection +#define MODAL_GROUP_G13 10 // [G61] Control mode + +#define MODAL_GROUP_G10 11 // [G98, G99] Canned Cycles Return Mode #define MODAL_GROUP_M4 11 // [M0,M1,M2,M30] Stopping -#define MODAL_GROUP_M7 12 // [M3,M4,M5] Spindle turning -#define MODAL_GROUP_M8 13 // [M7,M8,M9] Coolant control -#define MODAL_GROUP_M9 14 // [M56] Override control +#define MODAL_GROUP_M7 12 // [M3,M4,M5] Spindle turning +#define MODAL_GROUP_M8 13 // [M7,M8,M9] Coolant control +#define MODAL_GROUP_M9 14 // [M56] Override control // Define command actions for within execution-type modal groups (motion, stopping, non-modal). Used @@ -81,6 +83,9 @@ #define MOTION_MODE_PROBE_AWAY 142 // G38.4 (Do not alter value) #define MOTION_MODE_PROBE_AWAY_NO_ERROR 143 // G38.5 (Do not alter value) #define MOTION_MODE_NONE 80 // G80 (Do not alter value) +#define MOTION_MODE_DRILL 81 // G81 +#define MOTION_MODE_DRILL_DWELL 82 // G82 +#define MOTION_MODE_DRILL_PECK 83 // G83 // Modal Group G2: Plane select #define PLANE_SELECT_XY 0 // G17 (Default: Must be zero) @@ -129,6 +134,10 @@ #define TOOL_LENGTH_OFFSET_CANCEL 0 // G49 (Default: Must be zero) #define TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC 1 // G43.1 +// Modal Group G10: Canned Cycle Return Level +#define RETRACT_OLD_Z 0 // G98 (Default: Must be zero) +#define RETRACT_SPECIFIED_R 1 + // Modal Group G12: Active work coordinate system // N/A: Stores coordinate system value (54-59) to change to. @@ -146,6 +155,7 @@ #define WORD_X 10 #define WORD_Y 11 #define WORD_Z 12 +#define WORD_Q 13 // Define g-code parser position updating flags #define GC_UPDATE_POS_TARGET 0 // Must be zero @@ -183,6 +193,7 @@ typedef struct { uint8_t feed_rate; // {G93,G94} uint8_t units; // {G20,G21} uint8_t distance; // {G90,G91} + uint8_t retract; // {G98,G99} // uint8_t distance_arc; // {G91.1} NOTE: Don't track. Only default supported. uint8_t plane_select; // {G17,G18,G19} // uint8_t cutter_comp; // {G40} NOTE: Don't track. Only default supported. @@ -201,7 +212,7 @@ typedef struct { uint8_t l; // G10 or canned cycles parameters int32_t n; // Line number float p; // G10 or dwell parameters - // float q; // G82 peck drilling + float q; // G82 peck drilling float r; // Arc radius float s; // Spindle speed uint8_t t; // Tool selection diff --git a/grbl/MotionControl.c b/grbl/MotionControl.c index dd4275d..7f1bdab 100644 --- a/grbl/MotionControl.c +++ b/grbl/MotionControl.c @@ -39,6 +39,20 @@ static float target_prev[N_AXIS] = {0.0}; static uint8_t dir_negative[N_AXIS] = {0}; +void MC_Init(void) +{ + int32_t current_position[N_AXIS]; // Copy current state of the system position variable + + for(uint8_t i = 0; i < N_AXIS; i++) + { + dir_negative[i] = 0; + } + + memcpy(current_position, sys_position, sizeof(sys_position)); + System_ConvertArraySteps2Mpos(target_prev, current_position); +} + + // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. @@ -305,6 +319,7 @@ void MC_Dwell(float seconds) return; } + // TODO: DWELL sys.state Protocol_BufferSynchronize(); Delay_sec(seconds, DELAY_MODE_DWELL); } diff --git a/grbl/MotionControl.h b/grbl/MotionControl.h index d22c4a8..9ab6fd8 100644 --- a/grbl/MotionControl.h +++ b/grbl/MotionControl.h @@ -35,6 +35,9 @@ #define HOMING_CYCLE_Z BIT(Z_AXIS) +void MC_Init(void); + + // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // (1 minute)/feed_rate time. diff --git a/main.c b/main.c index 987761c..76f7fe2 100644 --- a/main.c +++ b/main.c @@ -40,9 +40,6 @@ volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bit int main(void) { - // Init SysTick 1ms - SysTick_Init(); - // Init formatted output Print_Init(); @@ -52,6 +49,9 @@ int main(void) System_ResetPosition(); + // Init SysTick 1ms + SysTick_Init(); + if(BIT_IS_TRUE(settings.flags, BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM;