diff --git a/Grbl_Esp32/Grbl_Esp32.ino b/Grbl_Esp32/Grbl_Esp32.ino index 5a90412e..f02c92d2 100644 --- a/Grbl_Esp32/Grbl_Esp32.ino +++ b/Grbl_Esp32/Grbl_Esp32.ino @@ -44,6 +44,18 @@ void setup() { stepper_init(); // Configure stepper pins and interrupt timers system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) + + + #ifdef ENABLE_BLUETOOTH + // if $I has some text, that is the bluetooth name + // This is a temporary convenience until a new setting is defined + char line[LINE_BUFFER_SIZE]; + settings_read_build_info(line); + if (line[0] != '\0') { + Serial.printf("Starting Bluetooth:%s", line); + bluetooth_init(line); + } + #endif diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index 8df95f7c..588a1ac4 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -52,6 +52,8 @@ Some features should not be changed. See notes below. // Serial baud rate #define BAUD_RATE 115200 +#define ENABLE_BLUETOOTH // enable bluetooth ... turns of if $I= something + // Define realtime command special characters. These characters are 'picked-off' directly from the // serial read data stream and are not passed to the grbl line execution parser. Select characters // that do not and must not exist in the streamed g-code program. ASCII control characters may be diff --git a/Grbl_Esp32/cpu_map.h b/Grbl_Esp32/cpu_map.h index a2bc408d..17f8da11 100644 --- a/Grbl_Esp32/cpu_map.h +++ b/Grbl_Esp32/cpu_map.h @@ -27,16 +27,19 @@ re-assigning numbers (gpio34-39) are inputs only and don't have software pullup/down functions + You MUST use external pullups or noise WILL cause problems. Unlike the AVR version certain pins are not forced into the same port. Therefore, bit masks are not use the same way and typically should not be changed. They are just preserved right now to make it easy to stay in sync with AVR grbl + + */ + // This is the CPU Map for the ESP32 CNC Controller R2 - #define X_STEP_PIN GPIO_NUM_12 #define Y_STEP_PIN GPIO_NUM_14 #define Z_STEP_PIN GPIO_NUM_27 @@ -56,7 +59,7 @@ #define STEPPERS_DISABLE_PIN GPIO_NUM_13 - #define COOLANT_FLOOD_PIN GPIO_NUM_18 + #define COOLANT_FLOOD_PIN GPIO_NUM_16 #define COOLANT_MIST_PIN GPIO_NUM_19 #define SPINDLE_PWM_PIN GPIO_NUM_17 @@ -71,9 +74,9 @@ #define Y_LIMIT_BIT 1 #define Z_LIMIT_BIT 2 - #define X_LIMIT_PIN GPIO_NUM_21 - #define Y_LIMIT_PIN GPIO_NUM_22 - #define Z_LIMIT_PIN GPIO_NUM_23 + #define X_LIMIT_PIN GPIO_NUM_2 + #define Y_LIMIT_PIN GPIO_NUM_4 + #define Z_LIMIT_PIN GPIO_NUM_15 #define LIMIT_MASK B111 // don't change #define PROBE_PIN GPIO_NUM_32 @@ -93,13 +96,7 @@ #define STEPPER_OFF_TIMER_PRESCALE 8 // gives a frequency of 10MHz #define STEPPER_OFF_PERIOD_uSEC 3 // each tick is - #define STEP_PULSE_MIN 3 // uSeconds + #define STEP_PULSE_MIN 2 // uSeconds #define STEP_PULSE_MAX 10 // uSeconds - #define STEP_OFF_AFTER_DELAY 0xFF - - - -#endif - - +#endif \ No newline at end of file diff --git a/Grbl_Esp32/defaults.h b/Grbl_Esp32/defaults.h index 72f96c02..b6ec8afc 100644 --- a/Grbl_Esp32/defaults.h +++ b/Grbl_Esp32/defaults.h @@ -33,14 +33,14 @@ #ifdef DEFAULTS_GENERIC // Grbl generic default settings. Should work across different machines. - #define DEFAULT_STEP_PULSE_MICROSECONDS 10 - #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled) + #define DEFAULT_STEP_PULSE_MICROSECONDS 3 + #define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // msec (0-254, 255 keeps steppers enabled) - #define DEFAULT_STEPPING_INVERT_MASK 1 // uint8_t - #define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t + #define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t + #define DEFAULT_DIRECTION_INVERT_MASK 0 // uint8_t #define DEFAULT_INVERT_ST_ENABLE 0 // boolean #define DEFAULT_INVERT_LIMIT_PINS 1 // boolean - #define DEFAULT_INVERT_PROBE_PIN 0 // boolean + #define DEFAULT_INVERT_PROBE_PIN 1 // boolean #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled @@ -52,8 +52,8 @@ #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 0 // false - #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir - #define DEFAULT_HOMING_FEED_RATE 25.0 // mm/min + #define DEFAULT_HOMING_DIR_MASK 3 // move positive dir Z, negative X,Y + #define DEFAULT_HOMING_FEED_RATE 100.0 // mm/min #define DEFAULT_HOMING_SEEK_RATE 500.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 1.0 // mm @@ -67,17 +67,17 @@ #define DEFAULT_Y_STEPS_PER_MM 250.0 #define DEFAULT_Z_STEPS_PER_MM 250.0 - #define DEFAULT_X_MAX_RATE 500.0 // mm/min - #define DEFAULT_Y_MAX_RATE 500.0 // mm/min - #define DEFAULT_Z_MAX_RATE 500.0 // mm/min + #define DEFAULT_X_MAX_RATE 6000.0 // mm/min + #define DEFAULT_Y_MAX_RATE 6000.0 // mm/min + #define DEFAULT_Z_MAX_RATE 6000.0 // mm/min - #define DEFAULT_X_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 - #define DEFAULT_Y_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 - #define DEFAULT_Z_ACCELERATION (10.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_X_ACCELERATION (50.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_Y_ACCELERATION (50.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 + #define DEFAULT_Z_ACCELERATION (50.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2 - #define DEFAULT_X_MAX_TRAVEL 200.0 // mm NOTE: Must be a positive value. - #define DEFAULT_Y_MAX_TRAVEL 200.0 // mm NOTE: Must be a positive value. - #define DEFAULT_Z_MAX_TRAVEL 200.0 // mm NOTE: Must be a positive value. + #define DEFAULT_X_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value. + #define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value. + #define DEFAULT_Z_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value. #endif diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index b7a8f388..3564891b 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 "20170801" +#define GRBL_VERSION_BUILD "20180817" //#include #include @@ -52,10 +52,6 @@ #include "spindle_control.h" #include "stepper.h" #include "jog.h" - - - - - - - +#ifdef ENABLE_BLUETOOTH + #include "grbl_bluetooth.h" +#endif \ No newline at end of file diff --git a/Grbl_Esp32/grbl_bluetooth.cpp b/Grbl_Esp32/grbl_bluetooth.cpp new file mode 100644 index 00000000..97d19e8e --- /dev/null +++ b/Grbl_Esp32/grbl_bluetooth.cpp @@ -0,0 +1,11 @@ +#include "BluetoothSerial.h" + +BluetoothSerial SerialBT; + +void bluetooth_init(char *name) +{ + if (!SerialBT.begin(name)) + { + Serial.printf("BlueTooth Failed: %s", name); + } +} \ No newline at end of file diff --git a/Grbl_Esp32/grbl_bluetooth.h b/Grbl_Esp32/grbl_bluetooth.h new file mode 100644 index 00000000..4b0f52a0 --- /dev/null +++ b/Grbl_Esp32/grbl_bluetooth.h @@ -0,0 +1,14 @@ +#ifndef grbl_bluetooth_h +#define grbl_bluetooth_h + + #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) + #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it + #endif + + #include "grbl.h" + #include "BluetoothSerial.h" + + extern BluetoothSerial SerialBT; + void bluetooth_init(char *name); + +#endif \ No newline at end of file diff --git a/Grbl_Esp32/report.cpp b/Grbl_Esp32/report.cpp index 6e6c3990..0bab2f57 100644 --- a/Grbl_Esp32/report.cpp +++ b/Grbl_Esp32/report.cpp @@ -27,29 +27,61 @@ For the most part, these functions primarily are called from protocol.c methods. If a different style feedback is desired (i.e. JSON), then a user can change these following methods to accomodate their needs. + + + ESP32 Notes: + + Major rewrite to fix issues with BlueTooth. As described here there is a + when you try to send data a single byte at a time using SerialBT.write(...). + https://github.com/espressif/arduino-esp32/issues/1537 + + A solution is to send messages as a string using SerialBT.print(...). Use + a short delay after each send. Therefore this file needed to be rewritten + to work that way. AVR Grbl was written to be super efficient to give it + good performance. This is far less efficient, but the ESP32 can handle it. + Do not use this version of the file with AVR Grbl. + + ESP32 discussion here ... https://github.com/bdring/Grbl_Esp32/issues/3 + + */ #include "grbl.h" -#define printPgmString Serial.print -#define PSTR -#define printString Serial.print -// Internal report utilities to reduce flash with repetitive tasks turned into functions. -void report_util_setting_prefix(uint8_t n) { serial_write('$'); print_uint8_base10(n); serial_write('='); } -static void report_util_line_feed() { printPgmString(PSTR("\r\n")); } -static void report_util_feedback_line_feed() { serial_write(']'); report_util_line_feed(); } -static void report_util_gcode_modes_G() { printPgmString(PSTR(" G")); } -static void report_util_gcode_modes_M() { printPgmString(PSTR(" M")); } -// static void report_util_comment_line_feed() { serial_write(')'); report_util_line_feed(); } -static void report_util_axis_values(float *axis_value) { +// this is a generic send function that everything should use, so interfaces could be added (Bluetooth, etc) +void grbl_send(char *text) +{ + #ifdef ENABLE_BLUETOOTH + if (SerialBT.hasClient()) + { + SerialBT.print(text); + //delay(10); // possible fix for dropped characters + } + #endif + + Serial.print(text); +} + +// formats axis values into a string and returns that string in rpt +static void report_util_axis_values(float *axis_value, char *rpt) { uint8_t idx; + char axisVal[10]; + + rpt[0] = '\0'; + for (idx=0; idx= AXIS_SETTINGS_INCREMENT) { - n -= AXIS_SETTINGS_INCREMENT; - idx++; - } - serial_write(n+'x'); - switch (idx) { - case 0: printPgmString(PSTR(":stp/mm")); break; - case 1: printPgmString(PSTR(":mm/min")); break; - case 2: printPgmString(PSTR(":mm/s^2")); break; - case 3: printPgmString(PSTR(":mm max")); break; - } - break; } - report_util_comment_line_feed(); } -*/ - -static void report_util_uint8_setting(uint8_t n, int val) { - report_util_setting_prefix(n); - print_uint8_base10(val); - report_util_line_feed(); // report_util_setting_string(n); -} -static void report_util_float_setting(uint8_t n, float val, uint8_t n_decimal) { - report_util_setting_prefix(n); - printFloat(val,n_decimal); - report_util_line_feed(); // report_util_setting_string(n); -} - // Handles the primary confirmation protocol response for streaming interfaces and human-feedback. // For every incoming line, this method responds with an 'ok' for a successful command or an @@ -132,13 +105,16 @@ static void report_util_float_setting(uint8_t n, float val, uint8_t n_decimal) { // responses. void report_status_message(uint8_t status_code) { + + char status[15]; + switch(status_code) { case STATUS_OK: // STATUS_OK - printPgmString(PSTR("ok\r\n")); break; + grbl_send("ok\r\n"); + break; default: - printPgmString(PSTR("error:")); - print_uint8_base10(status_code); - report_util_line_feed(); + sprintf(status, "error:%d\r\n", status_code); + grbl_send(status); } } @@ -147,15 +123,14 @@ void report_status_message(uint8_t status_code) // Prints alarm messages. void report_alarm_message(uint8_t alarm_code) { - printPgmString(PSTR("ALARM:")); - print_uint8_base10(alarm_code); - report_util_line_feed(); + char alarm[10]; + + sprintf(alarm, "ALARM:%d\r\n", alarm_code); + grbl_send(alarm); + delay_ms(500); // Force delay to ensure message clears serial write buffer. } - - - // Prints feedback messages. This serves as a centralized method to provide additional // user feedback for things that are not of the status/alarm message protocol. These are // messages such as setup warnings, switch toggling, and how to exit alarms. @@ -163,45 +138,53 @@ void report_alarm_message(uint8_t alarm_code) // is installed, the message number codes are less than zero. void report_feedback_message(uint8_t message_code) { - printPgmString(PSTR("[MSG:")); + char msg[40]; + + strcpy(msg, "[MSG:"); + switch(message_code) { case MESSAGE_CRITICAL_EVENT: - printPgmString(PSTR("Reset to continue")); break; + strcat(msg, "Reset to continue"); break; case MESSAGE_ALARM_LOCK: - printPgmString(PSTR("'$H'|'$X' to unlock")); break; + strcat(msg, "'$H'|'$X' to unlock"); break; case MESSAGE_ALARM_UNLOCK: - printPgmString(PSTR("Caution: Unlocked")); break; + strcat(msg, "Caution: Unlocked"); break; case MESSAGE_ENABLED: - printPgmString(PSTR("Enabled")); break; + strcat(msg, "Enabled"); break; case MESSAGE_DISABLED: - printPgmString(PSTR("Disabled")); break; + strcat(msg, "Disabled"); break; case MESSAGE_SAFETY_DOOR_AJAR: - printPgmString(PSTR("Check Door")); break; + strcat(msg, "Check Door"); break; case MESSAGE_CHECK_LIMITS: - printPgmString(PSTR("Check Limits")); break; + strcat(msg, "Check Limits"); break; case MESSAGE_PROGRAM_END: - printPgmString(PSTR("Pgm End")); break; + strcat(msg, "Pgm End"); break; case MESSAGE_RESTORE_DEFAULTS: - printPgmString(PSTR("Restoring defaults")); break; + strcat(msg, "Restoring defaults"); break; case MESSAGE_SPINDLE_RESTORE: - printPgmString(PSTR("Restoring spindle")); break; + strcat(msg, "Restoring spindle"); break; case MESSAGE_SLEEP_MODE: - printPgmString(PSTR("Sleeping")); break; - } - report_util_feedback_line_feed(); + strcat(msg, "Sleeping"); break; + } + strcat(msg, "]\r\n"); + + grbl_send(msg); } // Welcome message void report_init_message() { - printPgmString(PSTR("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n")); + grbl_send("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n"); } // Grbl help message -void report_grbl_help() { - printPgmString(PSTR("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n")); - #ifdef VERBOSE_HELP +void report_grbl_help() { + grbl_send("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n"); + #ifdef VERBOSE_HELP + #ifdef ENABLE_BLUETOOTH + return; // to much data for BT until fixed + #endif settings_help(); #endif } @@ -211,253 +194,326 @@ void report_grbl_help() { // NOTE: The numbering scheme here must correlate to storing in settings.c void report_grbl_settings() { // Print Grbl settings. - report_util_uint8_setting(0,settings.pulse_microseconds); - report_util_uint8_setting(1,settings.stepper_idle_lock_time); - report_util_uint8_setting(2,settings.step_invert_mask); - report_util_uint8_setting(3,settings.dir_invert_mask); - report_util_uint8_setting(4,bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); - report_util_uint8_setting(5,bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)); - report_util_uint8_setting(6,bit_istrue(settings.flags,BITFLAG_INVERT_PROBE_PIN)); - report_util_uint8_setting(10,settings.status_report_mask); - report_util_float_setting(11,settings.junction_deviation,N_DECIMAL_SETTINGVALUE); - report_util_float_setting(12,settings.arc_tolerance,N_DECIMAL_SETTINGVALUE); - report_util_uint8_setting(13,bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); - report_util_uint8_setting(20,bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); - report_util_uint8_setting(21,bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); - report_util_uint8_setting(22,bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); - report_util_uint8_setting(23,settings.homing_dir_mask); + char setting[20]; + char rpt[800]; - report_util_float_setting(24,settings.homing_feed_rate,N_DECIMAL_SETTINGVALUE); - report_util_float_setting(25,settings.homing_seek_rate,N_DECIMAL_SETTINGVALUE); - report_util_uint8_setting(26,settings.homing_debounce_delay); - report_util_float_setting(27,settings.homing_pulloff,N_DECIMAL_SETTINGVALUE); - report_util_float_setting(30,settings.rpm_max,N_DECIMAL_RPMVALUE); - report_util_float_setting(31,settings.rpm_min,N_DECIMAL_RPMVALUE); + rpt[0] = '\0'; + + sprintf(setting, "$0=%d\r\n", settings.pulse_microseconds); strcat(rpt, setting); + sprintf(setting, "$1=%d\r\n", settings.stepper_idle_lock_time); strcat(rpt, setting); + sprintf(setting, "$2=%d\r\n", settings.step_invert_mask); strcat(rpt, setting); + sprintf(setting, "$3=%d\r\n", settings.dir_invert_mask); strcat(rpt, setting); + sprintf(setting, "$4=%d\r\n", bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)); strcat(rpt, setting); + sprintf(setting, "$5=%d\r\n", bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)); strcat(rpt, setting); + sprintf(setting, "$6=%d\r\n", bit_istrue(settings.flags,BITFLAG_INVERT_PROBE_PIN)); strcat(rpt, setting); + sprintf(setting, "$10=%d\r\n", settings.status_report_mask); strcat(rpt, setting); + + sprintf(setting, "$11=%4.3f\r\n", settings.junction_deviation); strcat(rpt, setting); + sprintf(setting, "$12=%4.3f\r\n", settings.arc_tolerance); strcat(rpt, setting); + + sprintf(setting, "$13=%d\r\n", bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)); strcat(rpt, setting); + sprintf(setting, "$20=%d\r\n", bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)); strcat(rpt, setting); + sprintf(setting, "$21=%d\r\n", bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)); strcat(rpt, setting); + sprintf(setting, "$22=%d\r\n", bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)); strcat(rpt, setting); + sprintf(setting, "$23=%d\r\n", settings.homing_dir_mask); strcat(rpt, setting); + + sprintf(setting, "$24=%4.3f\r\n", settings.homing_feed_rate); strcat(rpt, setting); + sprintf(setting, "$25=%4.3f\r\n", settings.homing_seek_rate); strcat(rpt, setting); + + sprintf(setting, "$26=%d\r\n", settings.homing_debounce_delay); strcat(rpt, setting); + + sprintf(setting, "$27=%4.3f\r\n", settings.homing_pulloff); strcat(rpt, setting); + sprintf(setting, "$30=%4.3f\r\n", settings.rpm_max); strcat(rpt, setting); + sprintf(setting, "$31=%4.3f\r\n", settings.rpm_min); strcat(rpt, setting); + #ifdef VARIABLE_SPINDLE - report_util_uint8_setting(32,bit_istrue(settings.flags,BITFLAG_LASER_MODE)); + sprintf(setting, "$32=%d\r\n", bit_istrue(settings.flags,BITFLAG_LASER_MODE)); strcat(rpt, setting); #else - report_util_uint8_setting(32,0); + strcat(rpt, "$32=0\r\n"); #endif + // Print axis settings uint8_t idx, set_idx; uint8_t val = AXIS_SETTINGS_START_VAL; for (set_idx=0; set_idx= MOTION_MODE_PROBE_TOWARD) { - printPgmString(PSTR("38.")); - print_uint8_base10(gc_state.modal.motion - (MOTION_MODE_PROBE_TOWARD-2)); + sprintf(temp, "38.%d", gc_state.modal.motion - (MOTION_MODE_PROBE_TOWARD-2)); } else { - print_uint8_base10(gc_state.modal.motion); + sprintf(temp, "%d", gc_state.modal.motion); } + strcat(modes_rpt, temp); - report_util_gcode_modes_G(); - print_uint8_base10(gc_state.modal.coord_select+54); + sprintf(temp, " G%d", gc_state.modal.coord_select+54); + strcat(modes_rpt, temp); + + sprintf(temp, " G%d", gc_state.modal.plane_select+17); + strcat(modes_rpt, temp); - report_util_gcode_modes_G(); - print_uint8_base10(gc_state.modal.plane_select+17); + sprintf(temp, " G%d", 21-gc_state.modal.units); + strcat(modes_rpt, temp); - report_util_gcode_modes_G(); - print_uint8_base10(21-gc_state.modal.units); + sprintf(temp, " G%d", gc_state.modal.distance+90); + strcat(modes_rpt, temp); - report_util_gcode_modes_G(); - print_uint8_base10(gc_state.modal.distance+90); - - report_util_gcode_modes_G(); - print_uint8_base10(94-gc_state.modal.feed_rate); + sprintf(temp, " G%d", 94-gc_state.modal.feed_rate); + strcat(modes_rpt, temp); + if (gc_state.modal.program_flow) { - report_util_gcode_modes_M(); + //report_util_gcode_modes_M(); switch (gc_state.modal.program_flow) { - case PROGRAM_FLOW_PAUSED : serial_write('0'); break; + case PROGRAM_FLOW_PAUSED : strcat(modes_rpt, " M0"); //serial_write('0'); break; // case PROGRAM_FLOW_OPTIONAL_STOP : serial_write('1'); break; // M1 is ignored and not supported. case PROGRAM_FLOW_COMPLETED_M2 : case PROGRAM_FLOW_COMPLETED_M30 : - print_uint8_base10(gc_state.modal.program_flow); + sprintf(temp, " M%d", gc_state.modal.program_flow); + strcat(modes_rpt, temp); break; } } - report_util_gcode_modes_M(); + switch (gc_state.modal.spindle) { - case SPINDLE_ENABLE_CW : serial_write('3'); break; - case SPINDLE_ENABLE_CCW : serial_write('4'); break; - case SPINDLE_DISABLE : serial_write('5'); break; + case SPINDLE_ENABLE_CW : strcat(modes_rpt, " M3"); break; + case SPINDLE_ENABLE_CCW : strcat(modes_rpt, " M4"); break; + case SPINDLE_DISABLE : strcat(modes_rpt, " M5"); break; } - report_util_gcode_modes_M(); + //report_util_gcode_modes_M(); #ifdef ENABLE_M7 if (gc_state.modal.coolant) { // Note: Multiple coolant states may be active at the same time. - if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_MIST) { serial_write('7'); } - if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_FLOOD) { serial_write('8'); } - } else { serial_write('9'); } + if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_MIST) { strcat(modes_rpt, " M7"); } + if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_FLOOD) { strcat(modes_rpt, " M8"); } + } else { strcat(modes_rpt, " M9"); } #else - if (gc_state.modal.coolant) { serial_write('8'); } - else { serial_write('9'); } + if (gc_state.modal.coolant) { strcat(modes_rpt, " M8"); } + else { strcat(modes_rpt, " M9"); } #endif - printPgmString(PSTR(" T")); - print_uint8_base10(gc_state.tool); + sprintf(temp, " T%d", gc_state.tool); + strcat(modes_rpt, temp); - printPgmString(PSTR(" F")); - printFloat_RateValue(gc_state.feed_rate); - - #ifdef VARIABLE_SPINDLE - printPgmString(PSTR(" S")); - printFloat(gc_state.spindle_speed,N_DECIMAL_RPMVALUE); + sprintf(temp, " F%4.3f", gc_state.feed_rate); + strcat(modes_rpt, temp); + + #ifdef VARIABLE_SPINDLE + sprintf(temp, " S%4.3f", gc_state.spindle_speed); + strcat(modes_rpt, temp); #endif - report_util_feedback_line_feed(); + strcat(modes_rpt, "]\r\n"); + + grbl_send(modes_rpt); } + + // Prints specified startup line void report_startup_line(uint8_t n, char *line) { - printPgmString(PSTR("$N")); - print_uint8_base10(n); - serial_write('='); - printString(line); - report_util_line_feed(); + char temp[80]; + + sprintf(temp, "$N%d=%s\r\n", n, line); + + grbl_send(temp); + } void report_execute_startup_message(char *line, uint8_t status_code) { - serial_write('>'); - printString(line); - serial_write(':'); - report_status_message(status_code); + char temp[80]; + + sprintf(temp, ">%s:", line); + grbl_send(temp); + + report_status_message(status_code); // TODO ====================================== } // Prints build info line void report_build_info(char *line) { - printPgmString(PSTR("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":")); - printString(line); - report_util_feedback_line_feed(); - printPgmString(PSTR("[OPT:")); // Generate compile-time build option list + char build_info[50]; + + strcpy(build_info, "[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":"); + strcat(build_info, line); + strcat(build_info, "]\r\n[OPT:"); + #ifdef VARIABLE_SPINDLE - serial_write('V'); + strcat(build_info,"V"); #endif #ifdef USE_LINE_NUMBERS - serial_write('N'); + strcat(build_info,"N"); #endif #ifdef ENABLE_M7 - serial_write('M'); + strcat(build_info,"M"); #endif #ifdef COREXY - serial_write('C'); + strcat(build_info,"C"); #endif #ifdef PARKING_ENABLE - serial_write('P'); + strcat(build_info,"P"); #endif #ifdef HOMING_FORCE_SET_ORIGIN - serial_write('Z'); + strcat(build_info,"Z"); #endif #ifdef HOMING_SINGLE_AXIS_COMMANDS - serial_write('H'); + strcat(build_info,"H"); #endif #ifdef LIMITS_TWO_SWITCHES_ON_AXES - serial_write('L'); + strcat(build_info,"L"); #endif #ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES - serial_write('A'); + strcat(build_info,"A"); #endif + #ifdef ENABLE_BLUETOOTH + strcat(build_info,"B"); + #endif #ifndef ENABLE_RESTORE_EEPROM_WIPE_ALL // NOTE: Shown when disabled. - serial_write('*'); + strcat(build_info,"*"); #endif #ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // NOTE: Shown when disabled. - serial_write('$'); + strcat(build_info,"$"); #endif #ifndef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS // NOTE: Shown when disabled. - serial_write('#'); + strcat(build_info,"#"); #endif #ifndef ENABLE_BUILD_INFO_WRITE_COMMAND // NOTE: Shown when disabled. - serial_write('I'); + strcat(build_info,"I"); #endif #ifndef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE // NOTE: Shown when disabled. - serial_write('E'); + strcat(build_info,"E"); #endif #ifndef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE // NOTE: Shown when disabled. - serial_write('W'); + strcat(build_info,"W"); #endif // NOTE: Compiled values, like override increments/max/min values, may be added at some point later. // These will likely have a comma delimiter to separate them. - - report_util_feedback_line_feed(); + + strcat(build_info,"]\r\n"); + grbl_send(build_info); } + + // Prints the character string line Grbl has received from the user, which has been pre-parsed, // and has been sent into protocol_execute_line() routine to be executed by Grbl. void report_echo_line_received(char *line) { - printPgmString(PSTR("[echo: ")); printString(line); - report_util_feedback_line_feed(); + char temp[80]; + + sprintf(temp, "[echo: %s]\r\n", line); + } +//--------------------------------------------- Converted up to here --------------------------------------------------------------------- + // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram // and the actual location of the CNC machine. Users may change the following function to their @@ -470,42 +526,46 @@ void report_realtime_status() int32_t current_position[N_AXIS]; // Copy current state of the system position variable memcpy(current_position,sys_position,sizeof(sys_position)); float print_position[N_AXIS]; + + char status[200]; + char temp[50]; + system_convert_array_steps_to_mpos(print_position,current_position); - // Report current machine state and sub-states - serial_write('<'); + // Report current machine state and sub-states + strcpy(status, "<"); switch (sys.state) { - case STATE_IDLE: printPgmString(("Idle")); break; - case STATE_CYCLE: printPgmString(PSTR("Run")); break; + case STATE_IDLE: strcat(status, "Idle"); break; + case STATE_CYCLE: strcat(status, "Run"); break; case STATE_HOLD: if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { - printPgmString(PSTR("Hold:")); - if (sys.suspend & SUSPEND_HOLD_COMPLETE) { serial_write('0'); } // Ready to resume - else { serial_write('1'); } // Actively holding + strcat(status, "Hold:"); + if (sys.suspend & SUSPEND_HOLD_COMPLETE) { strcat(status, "0"); } // Ready to resume + else { strcat(status, "1"); } // Actively holding break; } // Continues to print jog state during jog cancel. - case STATE_JOG: printPgmString(PSTR("Jog")); break; - case STATE_HOMING: printPgmString(PSTR("Home")); break; - case STATE_ALARM: printPgmString(PSTR("Alarm")); break; - case STATE_CHECK_MODE: printPgmString(PSTR("Check")); break; + case STATE_JOG: strcat(status, "Jog"); break; + case STATE_HOMING: strcat(status, "Home"); break; + case STATE_ALARM: strcat(status, "Alarm"); break; + case STATE_CHECK_MODE: strcat(status, "Check"); break; case STATE_SAFETY_DOOR: - printPgmString(PSTR("Door:")); + strcat(status, "Door:"); if (sys.suspend & SUSPEND_INITIATE_RESTORE) { - serial_write('3'); // Restoring + strcat(status, "3"); // Restoring } else { if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { - serial_write('1'); // Door ajar + strcat(status, "1"); // Door ajar } else { - serial_write('0'); + strcat(status, "0"); } // Door closed and ready to resume } else { - serial_write('2'); // Retracting + strcat(status, "2"); // Retracting } } break; - case STATE_SLEEP: printPgmString(PSTR("Sleep")); break; + case STATE_SLEEP: strcat(status, "Sleep"); break; } float wco[N_AXIS]; @@ -523,19 +583,18 @@ void report_realtime_status() // Report machine position if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_POSITION_TYPE)) { - printPgmString(PSTR("|MPos:")); + strcat(status, "|MPos:"); } else { - printPgmString(PSTR("|WPos:")); + strcat(status, "|WPos:"); } - report_util_axis_values(print_position); + report_util_axis_values(print_position, temp); + strcat(status, temp); // Returns planner and serial read buffer states. #ifdef REPORT_FIELD_BUFFER_STATE - if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_BUFFER_STATE)) { - printPgmString(PSTR("|Bf:")); - print_uint8_base10(plan_get_block_buffer_available()); - serial_write(','); - print_uint8_base10(serial_get_rx_buffer_available()); + if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_BUFFER_STATE)) { + sprintf(temp, "|Bf:%d,%d", plan_get_block_buffer_available(), serial_get_rx_buffer_available()); + strcat(status, temp); } #endif @@ -546,8 +605,8 @@ void report_realtime_status() if (cur_block != NULL) { uint32_t ln = cur_block->line_number; if (ln > 0) { - printPgmString(PSTR("|Ln:")); - printInteger(ln); + sprintf(temp, "|Ln:%d", ln); + strcat(status, temp); } } #endif @@ -555,14 +614,13 @@ void report_realtime_status() // Report realtime feed speed #ifdef REPORT_FIELD_CURRENT_FEED_SPEED - #ifdef VARIABLE_SPINDLE - printPgmString(PSTR("|FS:")); - printFloat_RateValue(st_get_realtime_rate()); - serial_write(','); - printFloat(sys.spindle_speed,N_DECIMAL_RPMVALUE); + #ifdef VARIABLE_SPINDLE + sprintf(temp, "|FS:%4.3f,%4.3f", st_get_realtime_rate(), sys.spindle_speed); + strcat(status, temp); #else - printPgmString(PSTR("|F:")); - printFloat_RateValue(st_get_realtime_rate()); + + sprintf(temp, "|F:%4.3f", st_get_realtime_rate()); + strcat(status, temp); #endif #endif @@ -570,30 +628,30 @@ void report_realtime_status() uint8_t lim_pin_state = limits_get_state(); uint8_t ctrl_pin_state = system_control_get_state(); uint8_t prb_pin_state = probe_get_state(); - if (lim_pin_state | ctrl_pin_state | prb_pin_state) { - printPgmString(PSTR("|Pn:")); - if (prb_pin_state) { serial_write('P'); } + if (lim_pin_state | ctrl_pin_state | prb_pin_state) { + strcat(status, "|Pn:"); + if (prb_pin_state) { strcat(status, "P"); } if (lim_pin_state) { - if (bit_istrue(lim_pin_state,bit(X_AXIS))) { serial_write('X'); } - if (bit_istrue(lim_pin_state,bit(Y_AXIS))) { serial_write('Y'); } - if (bit_istrue(lim_pin_state,bit(Z_AXIS))) { serial_write('Z'); } + if (bit_istrue(lim_pin_state,bit(X_AXIS))) { strcat(status, "X"); } + if (bit_istrue(lim_pin_state,bit(Y_AXIS))) { strcat(status, "Y"); } + if (bit_istrue(lim_pin_state,bit(Z_AXIS))) { strcat(status, "Z"); } #ifdef A_AXIS - if (bit_istrue(lim_pin_state,bit(A_AXIS))) { serial_write('A'); } + if (bit_istrue(lim_pin_state,bit(A_AXIS))) { strcat(status, "A"); } #endif #ifdef B_AXIS - if (bit_istrue(lim_pin_state,bit(B_AXIS))) { serial_write('B'); } + if (bit_istrue(lim_pin_state,bit(B_AXIS))) { strcat(status, "B"); } #endif #ifdef C_AXIS - if (bit_istrue(lim_pin_state,bit(C_AXIS))) { serial_write('C'); } + if (bit_istrue(lim_pin_state,bit(C_AXIS))) { strcat(status, "C"); } #endif } if (ctrl_pin_state) { #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_SAFETY_DOOR)) { serial_write('D'); } + if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_SAFETY_DOOR)) { strcat(status, "D"); } #endif - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_RESET)) { serial_write('R'); } - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_FEED_HOLD)) { serial_write('H'); } - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_CYCLE_START)) { serial_write('S'); } + if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_RESET)) { strcat(status, "R"); } + if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_FEED_HOLD)) { strcat(status, "H"); } + if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_CYCLE_START)) { strcat(status, "S"); } } } #endif @@ -605,8 +663,9 @@ void report_realtime_status() sys.report_wco_counter = (REPORT_WCO_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh } else { sys.report_wco_counter = (REPORT_WCO_REFRESH_IDLE_COUNT-1); } if (sys.report_ovr_counter == 0) { sys.report_ovr_counter = 1; } // Set override on next report. - printPgmString(PSTR("|WCO:")); - report_util_axis_values(wco); + strcat(status, "|WCO:"); + report_util_axis_values(wco, temp); + strcat(status, temp); } #endif @@ -615,41 +674,39 @@ void report_realtime_status() else { if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { sys.report_ovr_counter = (REPORT_OVR_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh - } else { sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT-1); } - printPgmString(PSTR("|Ov:")); - print_uint8_base10(sys.f_override); - serial_write(','); - print_uint8_base10(sys.r_override); - serial_write(','); - print_uint8_base10(sys.spindle_speed_ovr); + } else { sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT-1); } + sprintf(temp, "|Ov:%d,%d,%d", sys.f_override, sys.r_override, sys.spindle_speed_ovr); + strcat(status, temp); + uint8_t sp_state = spindle_get_state(); uint8_t cl_state = coolant_get_state(); if (sp_state || cl_state) { - printPgmString(PSTR("|A:")); + strcat(status, "|A:"); if (sp_state) { // != SPINDLE_STATE_DISABLE #ifdef VARIABLE_SPINDLE #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN - serial_write('S'); // CW + strcat(status, "S"); // CW #else - if (sp_state == SPINDLE_STATE_CW) { serial_write('S'); } // CW - else { serial_write('C'); } // CCW + if (sp_state == SPINDLE_STATE_CW) { strcat(status, "S"); } // CW + else { strcat(status, "C"); } // CCW #endif #else - if (sp_state & SPINDLE_STATE_CW) { serial_write('S'); } // CW - else { serial_write('C'); } // CCW + if (sp_state & SPINDLE_STATE_CW) { strcat(status, "S"); } // CW + else { strcat(status, "C"); } // CCW #endif } - if (cl_state & COOLANT_STATE_FLOOD) { serial_write('F'); } + if (cl_state & COOLANT_STATE_FLOOD) { strcat(status, "F"); } #ifdef ENABLE_M7 - if (cl_state & COOLANT_STATE_MIST) { serial_write('M'); } + if (cl_state & COOLANT_STATE_MIST) { strcat(status, "M"); } #endif } } #endif - serial_write('>'); - report_util_line_feed(); + strcat(status, ">\r\n"); + + grbl_send(status); } void report_realtime_steps() diff --git a/Grbl_Esp32/serial.cpp b/Grbl_Esp32/serial.cpp index 082ce1d2..41450b5c 100644 --- a/Grbl_Esp32/serial.cpp +++ b/Grbl_Esp32/serial.cpp @@ -64,9 +64,22 @@ void serialCheckTask(void *pvParameters) while(true) // run continuously { - while (Serial.available()) // loop until all characters are read + #ifdef ENABLE_BLUETOOTH + while (Serial.available() || (SerialBT.hasClient() && SerialBT.available())) + #else + while (Serial.available()) + #endif { - data = Serial.read(); + if (Serial.available()) + data = Serial.read(); + else + { + #ifdef ENABLE_BLUETOOTH + data = SerialBT.read(); + Serial.write(data); // echo all data to serial + #endif + } + // Pick off realtime command characters directly from the serial stream. These characters are // not passed into the main buffer, but these set system state flag bits for realtime execution. @@ -121,9 +134,9 @@ void serialCheckTask(void *pvParameters) } } // switch data - } // if data + } // if something available vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks - } // while + } // while(true) } void serial_reset_read_buffer() diff --git a/README.md b/README.md index 3b898cbc..8e70e541 100644 --- a/README.md +++ b/README.md @@ -40,19 +40,20 @@ Note: Unlike Grbl on Arduinos, the controller does not reboot when you connect t Be sure you have external pullup resistors on any GPIO34-39 that you use. These default to door, start, hold and reset functions. -BlueTooth: (Experimental) +Using Bluetooth: -- Use the dev branch of the firmware with BlueTooth.- in the name. -- Use a serial port terminal to set the BlueTooth name using $I=NAME, where NAME is the BlueTooth name you want. I don't know all the naming rules, so keep it short and simple. There is no capability to use a password yet. -- Reboot the ESP32 to turn on BlueTooth with that name. Grbl will now respond on either BlueTooth or Serial data. All BlueTooth sends are echo'd on the Serial port if you want to watch the data. -- Android Instructions - - Pair with the device that has the name you chose. - - Get a gcode sender (Grbl Controller is recommended) +- [See the Wiki page](https://github.com/bdring/Grbl_Esp32/wiki/Using-Bletooth) ### TODO List - RMT. The RMT feature is a ideal for direction and step features, but apparently has issues working in interrupts. See [this forum post](https://www.esp32.com/viewtopic.php?f=19&t=6397&hilit=grbl) and [this blog post](http://www.buildlog.net/blog/?s=rmt). It would be great to get it working. - Add spindle enable and direction. +- Add SD card. Initially only these features will be supported. + - Mount Card + - List Files + - Run File + - Get job status (bytes send / file size) + - (Optional) Update firmware from SD card ### Credits