diff --git a/Grbl_Esp32/cpu_map.h b/Grbl_Esp32/cpu_map.h index 9a0bd58a..3a89274e 100644 --- a/Grbl_Esp32/cpu_map.h +++ b/Grbl_Esp32/cpu_map.h @@ -85,7 +85,7 @@ #define X_DIRECTION_PIN GPIO_NUM_14 #define Y_STEP_PIN GPIO_NUM_26 #define Y_DIRECTION_PIN GPIO_NUM_15 - #define COOLANT_FLOOD_PIN GPIO_NUM_25 + //#define COOLANT_FLOOD_PIN GPIO_NUM_25 #define SPINDLE_PWM_PIN GPIO_NUM_2 #define X_LIMIT_PIN GPIO_NUM_17 #define Z_LIMIT_PIN GPIO_NUM_16 @@ -94,7 +94,7 @@ #define X_DIRECTION_PIN GPIO_NUM_26 #define Y_STEP_PIN GPIO_NUM_14 #define Y_DIRECTION_PIN GPIO_NUM_25 - #define COOLANT_FLOOD_PIN GPIO_NUM_16 + //#define COOLANT_FLOOD_PIN GPIO_NUM_16 #define SPINDLE_PWM_PIN GPIO_NUM_17 #define X_LIMIT_PIN GPIO_NUM_2 #define Z_LIMIT_PIN GPIO_NUM_15 @@ -114,8 +114,9 @@ // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 - #define COOLANT_MIST_PIN GPIO_NUM_21 - + //#define COOLANT_MIST_PIN GPIO_NUM_21 + #define USER_DIGITAL_PIN_1 GPIO_NUM_21 + #define USER_DIGITAL_PIN_2 GPIO_NUM_25 #define SPINDLE_PWM_CHANNEL 0 diff --git a/Grbl_Esp32/gcode.cpp b/Grbl_Esp32/gcode.cpp index 7128ca93..413bb3b0 100644 --- a/Grbl_Esp32/gcode.cpp +++ b/Grbl_Esp32/gcode.cpp @@ -368,7 +368,23 @@ uint8_t gc_execute_line(char *line, uint8_t client) FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command] } break; - default: + case 62: + case 63: + //grbl_sendf(CLIENT_SERIAL,"M%d...\r\n", int_value); + word_bit = MODAL_GROUP_M10; + switch (int_value) { + case 62: + gc_block.modal.io_control = NON_MODAL_IO_ENABLE; + break; + case 63: + gc_block.modal.io_control = NON_MODAL_IO_DISABLE; + break; + default: + break; + } + + break; + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command] } @@ -618,6 +634,13 @@ uint8_t gc_execute_line(char *line, uint8_t client) } bit_false(value_words,bit(WORD_P)); } + + if ( (gc_block.modal.io_control == NON_MODAL_IO_ENABLE) || (gc_block.modal.io_control == NON_MODAL_IO_DISABLE)) { + if (bit_isfalse(value_words,bit(WORD_P))) { + FAIL(STATUS_GCODE_VALUE_WORD_MISSING); // [P word missing] + } + bit_false(value_words,bit(WORD_P)); + } // [11. Set active plane ]: N/A switch (gc_block.modal.plane_select) { @@ -1216,6 +1239,16 @@ uint8_t gc_execute_line(char *line, uint8_t client) } pl_data->condition |= gc_state.modal.coolant; // Set condition flag for planner use. + // turn on/off an i/o pin + if ( (gc_block.modal.io_control == NON_MODAL_IO_ENABLE) || (gc_block.modal.io_control == NON_MODAL_IO_DISABLE) ) { + if (gc_block.values.p <= MAX_USER_DIGITAL_PIN) { + sys_io_control(1<<(int)gc_block.values.p, (gc_block.modal.io_control == NON_MODAL_IO_ENABLE)); + } + else { + FAIL(STATUS_P_PARAM_MAX_EXCEEDED); + } + } + // [9. Enable/disable feed rate or spindle overrides ]: NOT SUPPORTED. Always enabled. // [10. Dwell ]: diff --git a/Grbl_Esp32/gcode.h b/Grbl_Esp32/gcode.h index b50f6b14..f3ddf3f0 100644 --- a/Grbl_Esp32/gcode.h +++ b/Grbl_Esp32/gcode.h @@ -46,6 +46,7 @@ #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_M10 14 // [M62, M63] User Defined http://linuxcnc.org/docs/html/gcode/overview.html#_modal_groups // #define OTHER_INPUT_F 14 // #define OTHER_INPUT_S 15 @@ -124,10 +125,17 @@ #define COOLANT_FLOOD_ENABLE PL_COND_FLAG_COOLANT_FLOOD // M8 (NOTE: Uses planner condition bit flag) #define COOLANT_MIST_ENABLE PL_COND_FLAG_COOLANT_MIST // M7 (NOTE: Uses planner condition bit flag) +// modal Group M10: User I/O control +#define NON_MODAL_IO_ENABLE 1 +#define NON_MODAL_IO_DISABLE 2 +#define MAX_USER_DIGITAL_PIN 4 + // Modal Group G8: Tool length offset #define TOOL_LENGTH_OFFSET_CANCEL 0 // G49 (Default: Must be zero) #define TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC 1 // G43.1 + + // Modal Group G12: Active work coordinate system // N/A: Stores coordinate system value (54-59) to change to. @@ -192,6 +200,7 @@ typedef struct { uint8_t program_flow; // {M0,M1,M2,M30} uint8_t coolant; // {M7,M8,M9} uint8_t spindle; // {M3,M4,M5} + uint8_t io_control; // {M62, M63} } gc_modal_t; typedef struct { diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index 704fda23..c3503a10 100644 --- a/Grbl_Esp32/grbl.h +++ b/Grbl_Esp32/grbl.h @@ -20,7 +20,7 @@ // Grbl versioning system #define GRBL_VERSION "1.1f" -#define GRBL_VERSION_BUILD "20191011" +#define GRBL_VERSION_BUILD "20191015" //#include #include diff --git a/Grbl_Esp32/motion_control.cpp b/Grbl_Esp32/motion_control.cpp index 3e31e21c..64b952e8 100644 --- a/Grbl_Esp32/motion_control.cpp +++ b/Grbl_Esp32/motion_control.cpp @@ -418,7 +418,9 @@ void mc_reset() // Kill spindle and coolant. spindle_stop(); - coolant_stop(); + coolant_stop(); + // turn off all digital I/O + sys_io_control(0xFF, false); #ifdef ENABLE_SD_CARD // do we need to stop a running SD job? diff --git a/Grbl_Esp32/report.h b/Grbl_Esp32/report.h index 33115346..82cd68b0 100644 --- a/Grbl_Esp32/report.h +++ b/Grbl_Esp32/report.h @@ -62,6 +62,7 @@ #define STATUS_GCODE_UNUSED_WORDS 36 #define STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR 37 #define STATUS_GCODE_MAX_VALUE_EXCEEDED 38 +#define STATUS_P_PARAM_MAX_EXCEEDED 39 #define STATUS_SD_FAILED_MOUNT 60 // SD Failed to mount #define STATUS_SD_FAILED_READ 61 // SD Failed to read file diff --git a/Grbl_Esp32/spindle_control.cpp b/Grbl_Esp32/spindle_control.cpp index 44c77e7f..3714b8c2 100644 --- a/Grbl_Esp32/spindle_control.cpp +++ b/Grbl_Esp32/spindle_control.cpp @@ -106,16 +106,10 @@ void spindle_set_speed(uint32_t pwm_value) #ifndef INVERT_SPINDLE_PWM grbl_analogWrite(SPINDLE_PWM_CHANNEL, pwm_value); #else - if (pwm_value == 0) { - grbl_analogWrite(SPINDLE_PWM_CHANNEL, (1<