diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index 9d6ca1c0..615b2513 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -54,6 +54,8 @@ Some features should not be changed. See notes below. #define ENABLE_BLUETOOTH // enable bluetooth ... turns of if $I= something +#define ENABLE_SD_CARD // enable use of SD Card to run jobs + // 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 @@ -69,7 +71,7 @@ Some features should not be changed. See notes below. // NOTE: All override realtime commands must be in the extended ASCII character set, starting // at character value 128 (0x80) and up to 255 (0xFF). If the normal set of realtime commands, // such as status reports, feed hold, reset, and cycle start, are moved to the extended set -// space, serial.c's RX ISR will need to be modified to accomodate the change. +// space, serial.c's RX ISR will need to be modified to accommodate the change. // #define CMD_RESET 0x80 // #define CMD_STATUS_REPORT 0x81 // #define CMD_CYCLE_START 0x82 @@ -189,7 +191,7 @@ Some features should not be changed. See notes below. // immediately forces a feed hold and then safely de-energizes the machine. Resuming is blocked until // the safety door is re-engaged. When it is, Grbl will re-energize the machine and then resume on the // previous tool path, as if nothing happened. -//#define ENABLE_SAFETY_DOOR_INPUT_PIN // Default disabled. Uncomment to enable. +//#define ENABLE_SAFETY_DOOR_INPUT_PIN // ESP32 Leave this enabled for now .. code for undefined not ready // After the safety door switch has been toggled and restored, this setting sets the power-up delay // between restoring the spindle and coolant and resuming the cycle. @@ -216,6 +218,8 @@ Some features should not be changed. See notes below. // problems if not externally pulled up. Ignoring will always return not activated when read. //#define IGNORE_CONTROL_PINS + + // Inverts select limit pin states based on the following mask. This effects all limit pin functions, // such as hard limits and homing. However, this is different from overall invert limits setting. // This build option will invert only the limit pins defined here, and then the invert limits setting diff --git a/Grbl_Esp32/defaults.h b/Grbl_Esp32/defaults.h index 3ae0cb45..59ea89e7 100644 --- a/Grbl_Esp32/defaults.h +++ b/Grbl_Esp32/defaults.h @@ -84,4 +84,4 @@ -#endif \ No newline at end of file +#endif diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index 07b23642..800fe47d 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 "20180904" +#define GRBL_VERSION_BUILD "20180906" //#include #include @@ -46,6 +46,7 @@ #include "gcode.h" #include "limits.h" #include "motion_control.h" +#include "print.h" #include "probe.h" #include "protocol.h" #include "report.h" @@ -53,6 +54,11 @@ #include "spindle_control.h" #include "stepper.h" #include "jog.h" + #ifdef ENABLE_BLUETOOTH #include "grbl_bluetooth.h" +#endif + +#ifdef ENABLE_SD_CARD + #include "grbl_sd.h" #endif \ No newline at end of file diff --git a/Grbl_Esp32/grbl_bluetooth.cpp b/Grbl_Esp32/grbl_bluetooth.cpp index 97d19e8e..d13f3957 100644 --- a/Grbl_Esp32/grbl_bluetooth.cpp +++ b/Grbl_Esp32/grbl_bluetooth.cpp @@ -1,11 +1,11 @@ -#include "BluetoothSerial.h" +#include "grbl.h" BluetoothSerial SerialBT; void bluetooth_init(char *name) { if (!SerialBT.begin(name)) - { - Serial.printf("BlueTooth Failed: %s", name); - } + { + report_status_message(STATUS_BT_FAIL_BEGIN); + } } \ No newline at end of file diff --git a/Grbl_Esp32/grbl_sd.cpp b/Grbl_Esp32/grbl_sd.cpp new file mode 100644 index 00000000..cf27a474 --- /dev/null +++ b/Grbl_Esp32/grbl_sd.cpp @@ -0,0 +1,194 @@ +/* + grbl_sd.cpp - Adds SD Card Features to Grbl_ESP32 + Part of Grbl_ESP32 + + Copyright (c) 2018 Barton Dring Buildlog.net + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "grbl_sd.h" + +// Define line flags. Includes comment type tracking and line overflow detection. +#define LINE_FLAG_OVERFLOW bit(0) +#define LINE_FLAG_COMMENT_PARENTHESES bit(1) +#define LINE_FLAG_COMMENT_SEMICOLON bit(2) + + + +File myFile; +char fileTypes[FILE_TYPE_COUNT][8] = {".NC", ".TXT", ".GCODE"}; // filter out files not of these types (s/b UPPERCASE) +bool SD_file_running = false; // a file has started but not completed +bool SD_ready_next = false; // Grbl has processed a line and is waiting for another + +// attempt to mount the SD card +bool sd_mount() { + if(!SD.begin()){ + report_status_message(STATUS_SD_FAILED_MOUNT); + return false; + } + return true; +} + +void listDir(fs::FS &fs, const char * dirname, uint8_t levels){ + char temp_filename[128]; // to help filter by extension TODO: 128 needs a definition based on something + + File root = fs.open(dirname); + if(!root){ + report_status_message(STATUS_SD_FAILED_OPEN_DIR); + return; + } + if(!root.isDirectory()){ + report_status_message(STATUS_SD_DIR_NOT_FOUND); + return; + } + + File file = root.openNextFile(); + while(file){ + if(file.isDirectory()){ + if(levels){ + listDir(fs, file.name(), levels -1); + } + } else { + strcpy(temp_filename, file.name()); // make a copy + + // convert it to uppercase so it is easy to filter + for(int i = 0; i <= strlen(file.name()); i++){ + temp_filename[i] = toupper(temp_filename[i]); + } + + // now filter for accetable file types + for (uint8_t i=0; i < FILE_TYPE_COUNT; i++) // make sure it is a valid file type + { + if (strstr(temp_filename, fileTypes[i])) { + grbl_sendf("[FILE:%s,SIZE:%d]\r\n", file.name(), file.size()); + break; + } + } + } + file = root.openNextFile(); + } +} + +boolean openFile(fs::FS &fs, const char * path){ + myFile = fs.open(path); + if(!myFile){ + report_status_message(STATUS_SD_FAILED_READ); + return false; + } + + SD_file_running = true; + SD_ready_next = false; // this will get set to true when Grbl issues "ok" message + return true; +} + +boolean closeFile(){ + if(!myFile){ + return false; + } + + SD_file_running = false; + SD_ready_next = false; + myFile.close(); + return true; +} + +/* + read a line from the SD card + strip whitespace + strip comments per http://linuxcnc.org/docs/ja/html/gcode/overview.html#gcode:comments + make uppercase + return true if a line is +*/ +boolean readFileLine(char *line) { + char c; + uint8_t index = 0; + uint8_t line_flags = 0; + + if (!myFile) { + report_status_message(STATUS_SD_FAILED_READ); + return false; + } + + + while(myFile.available()){ + c = myFile.read(); + + + if (c == '\r' || c == ' ' ) { + // ignore these whitespace items + } + else if (c == '(') { + line_flags |= LINE_FLAG_COMMENT_PARENTHESES; + } + else if (c == ')') { + // End of '()' comment. Resume line allowed. + if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) { line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); } + } + else if (c == ';') { + // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. + if (!(line_flags & LINE_FLAG_COMMENT_PARENTHESES)) // semi colon inside parentheses do not mean anything + line_flags |= LINE_FLAG_COMMENT_SEMICOLON; + } + else if (c == '\n') { // found the newline, so mark the end and return true + line[index] = '\0'; + return true; + } + else { // add characters to the line + if (!line_flags) { + c = toupper(c); // make upper case + line[index] = c; + index++; + } + } + + if (index == 255) // name is too long so return false + { + line[index] = '\0'; + report_status_message(STATUS_OVERFLOW); + return false; + } + } + // some files end without a newline + if (index !=0) { + line[index] = '\0'; + return true; + } + else // empty line after new line + return false; +} + +// return a percentage complete 50.5 = 50.5% +float sd_report_perc_complete() { + if (!myFile) + return 0.0; + + return ((float)myFile.position() / (float)myFile.size() * 100.0); +} + +/* +void readFile(fs::FS &fs, const char * path){ + + File file = fs.open(path); + if(!file){ + report_status_message(STATUS_SD_FAILED_READ); + return; + } + + while(file.available()){ + Serial.write(file.read()); + } + file.close(); +} +*/ \ No newline at end of file diff --git a/Grbl_Esp32/grbl_sd.h b/Grbl_Esp32/grbl_sd.h new file mode 100644 index 00000000..f58a80df --- /dev/null +++ b/Grbl_Esp32/grbl_sd.h @@ -0,0 +1,38 @@ +/* + * Connect the SD card to the following pins: + * + * SD Card | ESP32 + * D2 - + * D3 SS + * CMD MOSI + * VSS GND + * VDD 3.3V + * CLK SCK + * VSS GND + * D0 MISO + * D1 - + */ + +#ifndef grbl_sd_h +#define grbl_sd_h + + + #include "grbl.h" + #include "FS.h" + #include "SD.h" + #include "SPI.h" + +#define FILE_TYPE_COUNT 3 // number of acceptable gcode file types in array + +extern bool SD_file_running; // a file has started but not completed +extern bool SD_ready_next; // Grbl has processed a line and is waiting for another + +bool sd_mount(); +void listDir(fs::FS &fs, const char * dirname, uint8_t levels); +boolean openFile(fs::FS &fs, const char * path); +boolean closeFile(); +boolean readFileLine(char *line); +void readFile(fs::FS &fs, const char * path); +float sd_report_perc_complete(); + +#endif \ No newline at end of file diff --git a/Grbl_Esp32/limits.cpp b/Grbl_Esp32/limits.cpp index 91e42134..7ebc99de 100644 --- a/Grbl_Esp32/limits.cpp +++ b/Grbl_Esp32/limits.cpp @@ -93,7 +93,7 @@ void limits_go_home(uint8_t cycle_mask) if (bit_istrue(cycle_mask,bit(idx))) { // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. // NOTE: settings.max_travel[] is stored as a negative value. - max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]); + max_travel = MAX(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]); } } diff --git a/Grbl_Esp32/nuts_bolts.cpp b/Grbl_Esp32/nuts_bolts.cpp index 5dc6c4ec..983820e3 100644 --- a/Grbl_Esp32/nuts_bolts.cpp +++ b/Grbl_Esp32/nuts_bolts.cpp @@ -158,7 +158,7 @@ float limit_value_by_axis_maximum(float *max_value, float *unit_vec) float limit_value = SOME_LARGE_VALUE; for (idx=0; idx (b)) ? (a) : (b)) -#define min(a,b) (((a) < (b)) ? (a) : (b)) +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) // changed to upper case to remove conflicts with other libraries +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) // changed to upper case to remove conflicts with other libraries #define isequal_position_vector(a,b) !(memcmp(a, b, sizeof(float)*N_AXIS)) // Bit field and masking macros diff --git a/Grbl_Esp32/planner.cpp b/Grbl_Esp32/planner.cpp index 2b61073f..82ebb43b 100644 --- a/Grbl_Esp32/planner.cpp +++ b/Grbl_Esp32/planner.cpp @@ -143,7 +143,7 @@ static void planner_recalculate() plan_block_t *current = &block_buffer[block_index]; // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + current->entry_speed_sqr = MIN( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); block_index = plan_prev_block_index(block_index); if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. @@ -347,7 +347,7 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data) target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]); block->steps[idx] = labs(target_steps[idx]-position_steps[idx]); } - block->step_event_count = max(block->step_event_count, block->steps[idx]); + block->step_event_count = MAX(block->step_event_count, block->steps[idx]); if (idx == A_MOTOR) { delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] + target_steps[Y_AXIS]-position_steps[Y_AXIS])/settings.steps_per_mm[idx]; } else if (idx == B_MOTOR) { @@ -358,7 +358,7 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data) #else target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]); block->steps[idx] = labs(target_steps[idx]-position_steps[idx]); - block->step_event_count = max(block->step_event_count, block->steps[idx]); + block->step_event_count = MAX(block->step_event_count, block->steps[idx]); delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx]; #endif unit_vec[idx] = delta_mm; // Store unit vector numerator @@ -435,7 +435,7 @@ uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data) convert_delta_vector_to_unit_vector(junction_unit_vec); float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec); float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + block->max_junction_speed_sqr = MAX( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, (junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } } diff --git a/Grbl_Esp32/print.cpp b/Grbl_Esp32/print.cpp new file mode 100644 index 00000000..1a2885fb --- /dev/null +++ b/Grbl_Esp32/print.cpp @@ -0,0 +1,157 @@ +/* + print.c - Functions for formatting output strings + Part of Grbl + + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 2018 - Bart Dring This file was modifed for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include "grbl.h" + + + + +// void printIntegerInBase(unsigned long n, unsigned long base) +// { +// unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. +// unsigned long i = 0; +// +// if (n == 0) { +// serial_write('0'); +// return; +// } +// +// while (n > 0) { +// buf[i++] = n % base; +// n /= base; +// } +// +// for (; i > 0; i--) +// serial_write(buf[i - 1] < 10 ? +// '0' + buf[i - 1] : +// 'A' + buf[i - 1] - 10); +// } + + +// Prints an uint8 variable in base 10. +void print_uint8_base10(uint8_t n) +{ + uint8_t digit_a = 0; + uint8_t digit_b = 0; + if (n >= 100) { // 100-255 + digit_a = '0' + n % 10; + n /= 10; + } + if (n >= 10) { // 10-99 + digit_b = '0' + n % 10; + n /= 10; + } + serial_write('0' + n); + if (digit_b) { serial_write(digit_b); } + if (digit_a) { serial_write(digit_a); } +} + + +// Prints an uint8 variable in base 2 with desired number of desired digits. +void print_uint8_base2_ndigit(uint8_t n, uint8_t digits) { + unsigned char buf[digits]; + uint8_t i = 0; + + for (; i < digits; i++) { + buf[i] = n % 2 ; + n /= 2; + } + + for (; i > 0; i--) + Serial.print('0' + buf[i - 1]); +} + + +void print_uint32_base10(uint32_t n) +{ + if (n == 0) { + Serial.print('0'); + return; + } + + unsigned char buf[10]; + uint8_t i = 0; + + while (n > 0) { + buf[i++] = n % 10; + n /= 10; + } + + for (; i > 0; i--) + Serial.print('0' + buf[i-1]); +} + + +void printInteger(long n) +{ + if (n < 0) { + Serial.print('-'); + print_uint32_base10(-n); + } else { + print_uint32_base10(n); + } +} + + +// Convert float to string by immediately converting to a long integer, which contains +// more digits than a float. Number of decimal places, which are tracked by a counter, +// may be set by the user. The integer is then efficiently converted to a string. +// NOTE: AVR '%' and '/' integer operations are very efficient. Bitshifting speed-up +// techniques are actually just slightly slower. Found this out the hard way. +void printFloat(float n, uint8_t decimal_places) +{ + Serial.print(n, decimal_places); +} + + +// Floating value printing handlers for special variables types used in Grbl and are defined +// in the config.h. +// - CoordValue: Handles all position or coordinate values in inches or mm reporting. +// - RateValue: Handles feed rate and current velocity in inches or mm reporting. +void printFloat_CoordValue(float n) { + if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { + printFloat(n*INCH_PER_MM,N_DECIMAL_COORDVALUE_INCH); + } else { + printFloat(n,N_DECIMAL_COORDVALUE_MM); + } +} + +void printFloat_RateValue(float n) { + if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { + printFloat(n*INCH_PER_MM,N_DECIMAL_RATEVALUE_INCH); + } else { + printFloat(n,N_DECIMAL_RATEVALUE_MM); + } +} + +// Debug tool to print free memory in bytes at the called point. +// NOTE: Keep commented unless using. Part of this function always gets compiled in. +// void printFreeMemory() +// { +// extern int __heap_start, *__brkval; +// uint16_t free; // Up to 64k values. +// free = (int) &free - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); +// printInteger((int32_t)free); +// printString(" "); +// } diff --git a/Grbl_Esp32/print.h b/Grbl_Esp32/print.h new file mode 100644 index 00000000..9ee2559d --- /dev/null +++ b/Grbl_Esp32/print.h @@ -0,0 +1,54 @@ +/* + print.h - Functions for formatting output strings + Part of Grbl + + Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC + Copyright (c) 2009-2011 Simen Svale Skogsrud + + 2018 - Bart Dring This file was modifed for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef print_h +#define print_h + + +void printString(const char *s); + +void printPgmString(const char *s); + +void printInteger(long n); + +void print_uint32_base10(uint32_t n); + +// Prints an uint8 variable in base 10. +void print_uint8_base10(uint8_t n); + +// Prints an uint8 variable in base 2 with desired number of desired digits. +void print_uint8_base2_ndigit(uint8_t n, uint8_t digits); + +void printFloat(float n, uint8_t decimal_places); + +// Floating value printing handlers for special variables types used in Grbl. +// - CoordValue: Handles all position or coordinate values in inches or mm reporting. +// - RateValue: Handles feed rate and current velocity in inches or mm reporting. +void printFloat_CoordValue(float n); +void printFloat_RateValue(float n); + +// Debug tool to print free memory in bytes at the called point. Not used otherwise. +void printFreeMemory(); + +#endif diff --git a/Grbl_Esp32/protocol.cpp b/Grbl_Esp32/protocol.cpp index 1d9a24e5..8ecc2598 100644 --- a/Grbl_Esp32/protocol.cpp +++ b/Grbl_Esp32/protocol.cpp @@ -76,6 +76,21 @@ void protocol_main_loop() uint8_t c; for (;;) { + #ifdef ENABLE_SD_CARD + if (SD_ready_next) { + char fileLine[255]; + if (readFileLine(fileLine)) { + SD_ready_next = false; + report_status_message(gc_execute_line(fileLine)); + } + else { + closeFile(); // close file and clear SD ready/running flags + // TODO some type of alert + } + } + #endif + + // Process one line of incoming serial data, as the data becomes available. Performs an // initial filtering by removing spaces and comments and capitalizing all letters. while((c = serial_read()) != SERIAL_NO_DATA) { @@ -122,10 +137,14 @@ void protocol_main_loop() } else { if (c <= ' ') { // Throw away whitepace and control characters - } else if (c == '/') { + } + /* + else if (c == '/') { // Block delete NOT SUPPORTED. Ignore character. // NOTE: If supported, would simply need to check the system if block delete is enabled. - } else if (c == '(') { + } + */ + else if (c == '(') { // Enable comments flag and ignore all characters until ')' or EOL. // NOTE: This doesn't follow the NIST definition exactly, but is good enough for now. // In the future, we could simply remove the items within the comments, but retain the @@ -426,8 +445,8 @@ void protocol_exec_rt_system() if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) { new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT; } if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) { new_f_override += FEED_OVERRIDE_FINE_INCREMENT; } if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) { new_f_override -= FEED_OVERRIDE_FINE_INCREMENT; } - new_f_override = min(new_f_override,MAX_FEED_RATE_OVERRIDE); - new_f_override = max(new_f_override,MIN_FEED_RATE_OVERRIDE); + new_f_override = MIN(new_f_override,MAX_FEED_RATE_OVERRIDE); + new_f_override = MAX(new_f_override,MIN_FEED_RATE_OVERRIDE); uint8_t new_r_override = sys.r_override; if (rt_exec & EXEC_RAPID_OVR_RESET) { new_r_override = DEFAULT_RAPID_OVERRIDE; } @@ -454,8 +473,8 @@ void protocol_exec_rt_system() if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) { last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT; } if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) { last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT; } if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) { last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT; } - last_s_override = min(last_s_override,MAX_SPINDLE_SPEED_OVERRIDE); - last_s_override = max(last_s_override,MIN_SPINDLE_SPEED_OVERRIDE); + last_s_override = MIN(last_s_override,MAX_SPINDLE_SPEED_OVERRIDE); + last_s_override = MAX(last_s_override,MIN_SPINDLE_SPEED_OVERRIDE); if (last_s_override != sys.spindle_speed_ovr) { bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); @@ -582,7 +601,7 @@ static void protocol_exec_rt_suspend() if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { memcpy(restore_target,parking_target,sizeof(parking_target)); retract_waypoint += restore_target[PARKING_AXIS]; - retract_waypoint = min(retract_waypoint,PARKING_TARGET); + retract_waypoint = MIN(retract_waypoint,PARKING_TARGET); } // Execute slow pull-out parking retract motion. Parking requires homing enabled, the diff --git a/Grbl_Esp32/report.cpp b/Grbl_Esp32/report.cpp index 7e1f5fca..34f51677 100644 --- a/Grbl_Esp32/report.cpp +++ b/Grbl_Esp32/report.cpp @@ -63,6 +63,31 @@ void grbl_send(char *text) Serial.print(text); } +// This is a formating version of the grbl_send(...) function that work like printf +void grbl_sendf(const char *format, ...) +{ + char loc_buf[64]; + char * temp = loc_buf; + va_list arg; + va_list copy; + va_start(arg, format); + va_copy(copy, arg); + size_t len = vsnprintf(NULL, 0, format, arg); + va_end(copy); + if(len >= sizeof(loc_buf)){ + temp = new char[len+1]; + if(temp == NULL) { + return; + } + } + len = vsnprintf(temp, len+1, format, arg); + grbl_send(temp); + va_end(arg); + if(len > 64){ + delete[] temp; + } +} + // 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; @@ -104,17 +129,20 @@ void get_state(char *foo) // from a critical error, such as a triggered hard limit. Interface should always monitor for these // responses. void report_status_message(uint8_t status_code) -{ - - char status[15]; - +{ switch(status_code) { case STATUS_OK: // STATUS_OK - grbl_send("ok\r\n"); + #ifdef ENABLE_SD_CARD + if (SD_file_running) + SD_ready_next = true; // flag so system_execute_line() will send the next line + else + grbl_send("ok\r\n"); + #else + grbl_send("ok\r\n"); + #endif break; default: - sprintf(status, "error:%d\r\n", status_code); - grbl_send(status); + grbl_sendf("error:%d\r\n", status_code); } } @@ -122,12 +150,9 @@ void report_status_message(uint8_t status_code) // Prints alarm messages. void report_alarm_message(uint8_t alarm_code) -{ - char alarm[32]; - snprintf(alarm, 32, "ALARM:%d\r\n", alarm_code); - grbl_send(alarm); - - delay_ms(500); // Force delay to ensure message clears serial write buffer. +{ + grbl_sendf("ALARM:%d\r\n", alarm_code); + delay_ms(500); // Force delay to ensure message clears serial write buffer. } // Prints feedback messages. This serves as a centralized method to provide additional @@ -137,37 +162,30 @@ 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) { - char msg[40]; - - strcpy(msg, "[MSG:"); - - switch(message_code) { + switch(message_code) { case MESSAGE_CRITICAL_EVENT: - strcat(msg, "Reset to continue"); break; + grbl_sendf("[MSG:%s]\r\n", "Reset to continue"); break; case MESSAGE_ALARM_LOCK: - strcat(msg, "'$H'|'$X' to unlock"); break; + grbl_sendf("[MSG:%s]\r\n", "'$H'|'$X' to unlock"); break; case MESSAGE_ALARM_UNLOCK: - strcat(msg, "Caution: Unlocked"); break; + grbl_sendf("[MSG:%s]\r\n", "Caution: Unlocked"); break; case MESSAGE_ENABLED: - strcat(msg, "Enabled"); break; + grbl_sendf("[MSG:%s]\r\n", "Enabled"); break; case MESSAGE_DISABLED: - strcat(msg, "Disabled"); break; + grbl_sendf("[MSG:%s]\r\n", "Disabled"); break; case MESSAGE_SAFETY_DOOR_AJAR: - strcat(msg, "Check Door"); break; + grbl_sendf("[MSG:%s]\r\n", "Check Door"); break; case MESSAGE_CHECK_LIMITS: - strcat(msg, "Check Limits"); break; + grbl_sendf("[MSG:%s]\r\n", "Check Limits"); break; case MESSAGE_PROGRAM_END: - strcat(msg, "Pgm End"); break; + grbl_sendf("[MSG:%s]\r\n", "Pgm End"); break; case MESSAGE_RESTORE_DEFAULTS: - strcat(msg, "Restoring defaults"); break; + grbl_sendf("[MSG:%s]\r\n", "Restoring defaults"); break; case MESSAGE_SPINDLE_RESTORE: - strcat(msg, "Restoring spindle"); break; + grbl_sendf("[MSG:%s]\r\n", "Restoring spindle"); break; case MESSAGE_SLEEP_MODE: - strcat(msg, "Sleeping"); break; - } - strcat(msg, "]\r\n"); - - grbl_send(msg); + grbl_sendf("[MSG:%s]\r\n", "Sleeping"); break; + } } @@ -179,7 +197,7 @@ void report_init_message() // Grbl help message void report_grbl_help() { - grbl_send("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n"); + grbl_send("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $F ~ ! ? ctrl-x]\r\n"); #ifdef VERBOSE_HELP #ifdef ENABLE_BLUETOOTH return; // to much data for BT until fixed @@ -415,23 +433,17 @@ void report_gcode_modes() // Prints specified startup line void report_startup_line(uint8_t n, char *line) -{ - char temp[80]; - - sprintf(temp, "$N%d=%s\r\n", n, line); - - grbl_send(temp); - +{ + grbl_sendf("$N%d=%s\r\n", n, line); } void report_execute_startup_message(char *line, uint8_t status_code) { char temp[80]; - sprintf(temp, ">%s:", line); - grbl_send(temp); + grbl_sendf(">%s:", line); - report_status_message(status_code); // TODO ====================================== + report_status_message(status_code); // TODO reduce number of back to back BT sends } // Prints build info line @@ -465,42 +477,21 @@ void report_build_info(char *line) strcat(build_info,"H"); #endif #ifdef LIMITS_TWO_SWITCHES_ON_AXES - strcat(build_info,"T"); - #endif - #ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES - strcat(build_info,"A"); - #endif - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - strcat(build_info,"+"); + strcat(build_info,"L"); #endif #ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES - strcat(build_info,"A"); - #endif - #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN - strcat(build_info,"D"); - #endif - #ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED - strcat(build_info,"0"); - #endif - #ifdef ENABLE_SOFTWARE_DEBOUNCE - strcat(build_info,"S"); - #endif - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - strcat(build_info,"R"); - #endif - #ifndef HOMING_INIT_LOCK - strcat(build_info,"L"); + strcat(build_info,"A"); #endif #ifdef ENABLE_BLUETOOTH strcat(build_info,"B"); #endif - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - strcat(build_info,"+"); - #endif + #ifdef ENABLE_SD_CARD + strcat(build_info,"S"); + #endif #ifndef ENABLE_RESTORE_EEPROM_WIPE_ALL // NOTE: Shown when disabled. strcat(build_info,"*"); #endif - #ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // NOTE: Shown when disabled. + #ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // NOTE: Shown when disabled. strcat(build_info,"$"); #endif #ifndef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS // NOTE: Shown when disabled. @@ -726,6 +717,13 @@ void report_realtime_status() } } #endif + + #ifdef ENABLE_SD_CARD + if (SD_file_running) { + sprintf(temp, "|SD:%4.2f", sd_report_perc_complete()); + strcat(status, temp); + } + #endif strcat(status, ">\r\n"); diff --git a/Grbl_Esp32/report.h b/Grbl_Esp32/report.h index e20abe9f..632ff012 100644 --- a/Grbl_Esp32/report.h +++ b/Grbl_Esp32/report.h @@ -97,7 +97,9 @@ #define MESSAGE_SPINDLE_RESTORE 10 #define MESSAGE_SLEEP_MODE 11 +// functions to send data to the user. void grbl_send(char *text); +void grbl_sendf(const char *format, ...); // Prints system status messages. void report_status_message(uint8_t status_code); @@ -146,4 +148,4 @@ void report_build_info(char *line); void settings_help(); -#endif \ No newline at end of file +#endif diff --git a/Grbl_Esp32/serial.cpp b/Grbl_Esp32/serial.cpp index 41450b5c..841eaefb 100644 --- a/Grbl_Esp32/serial.cpp +++ b/Grbl_Esp32/serial.cpp @@ -81,6 +81,8 @@ void serialCheckTask(void *pvParameters) } + + // 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. switch (data) { @@ -121,6 +123,7 @@ void serialCheckTask(void *pvParameters) } // Throw away any unfound extended-ASCII character by not passing it to the serial buffer. } else { // Write character to buffer + vTaskEnterCritical(&myMutex); next_head = serial_rx_buffer_head + 1; if (next_head == RX_RING_BUFFER) { next_head = 0; } @@ -129,11 +132,13 @@ void serialCheckTask(void *pvParameters) if (next_head != serial_rx_buffer_tail) { serial_rx_buffer[serial_rx_buffer_head] = data; serial_rx_buffer_head = next_head; - } + } vTaskExitCritical(&myMutex); } } // switch data + + } // if something available vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks } // while(true) @@ -161,7 +166,7 @@ uint8_t serial_read() tail++; if (tail == RX_RING_BUFFER) { tail = 0; } serial_rx_buffer_tail = tail; - vTaskExitCritical(&myMutex); + vTaskExitCritical(&myMutex); return data; } } diff --git a/Grbl_Esp32/system.cpp b/Grbl_Esp32/system.cpp index c8cd9c6f..8fe8feca 100644 --- a/Grbl_Esp32/system.cpp +++ b/Grbl_Esp32/system.cpp @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC - 2018 - Bart Dring This file was modifed for use on the ESP32 + 2018 - Bart Dring This file was modified for use on the ESP32 CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify @@ -24,9 +24,7 @@ void system_ini() // Renamed from system_init() due to conflict with esp32 files { // setup control inputs - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - pinMode(CONTROL_SAFETY_DOOR_PIN, INPUT); - #endif + pinMode(CONTROL_SAFETY_DOOR_PIN, INPUT); pinMode(CONTROL_RESET_PIN, INPUT); pinMode(CONTROL_FEED_HOLD_PIN, INPUT); pinMode(CONTROL_CYCLE_START_PIN, INPUT); @@ -95,6 +93,7 @@ uint8_t system_execute_line(char *line) uint8_t char_counter = 1; uint8_t helper_var = 0; // Helper variable float parameter, value; + switch( line[char_counter] ) { case 0 : report_grbl_help(); break; case 'J' : // Jogging @@ -219,6 +218,40 @@ uint8_t system_execute_line(char *line) helper_var = true; // Set helper_var to flag storing method. // No break. Continues into default: to read remaining command characters. } + #ifdef ENABLE_SD_CARD // ==================== SD Card ============================ + case 'F': + char fileLine[255]; + if ( line[char_counter+1] == 0 ) { + // show the files + //Serial.println("Show the files"); + listDir(SD, "/", 10); // 10 directory levels should be fine + } + else if (line[char_counter+1] == 'M') { + sd_mount(); + } + else if (line[char_counter+1] == '=') { // run a file + if (sys.state != STATE_IDLE) { return(STATUS_SYSTEM_GC_LOCK); } // must be in idle to run a file + char_counter += 2; + helper_var = char_counter; // Set helper variable as counter to start of user info line. + // shift the user info over to the beginning of the line "$F=/FOO.NC" becomes "/FOO.NC" + do { + line[char_counter-helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + + openFile(SD, line); + if (!readFileLine(fileLine)) { + return(STATUS_SD_FILE_EMPTY); + closeFile(); + } + else { + report_status_message(gc_execute_line(fileLine)); // execute the first line + } + } + else { + return(STATUS_INVALID_STATEMENT); + } + break; + #endif // =============================== SD Card ======================================== default : // Storing setting methods [IDLE/ALARM] if(!read_float(line, &char_counter, ¶meter)) { return(STATUS_BAD_NUMBER_FORMAT); } if(line[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); } @@ -382,13 +415,11 @@ uint8_t system_control_get_state() { #ifdef IGNORE_CONTROL_PINS return 0; - #endif - - - uint8_t control_state = 0; - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - if (digitalRead(CONTROL_SAFETY_DOOR_PIN)) { control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; } #endif + + + uint8_t control_state = 0; + if (digitalRead(CONTROL_SAFETY_DOOR_PIN)) { control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; } if (digitalRead(CONTROL_RESET_PIN)) { control_state |= CONTROL_PIN_INDEX_RESET; } if (digitalRead(CONTROL_FEED_HOLD_PIN)) { control_state |= CONTROL_PIN_INDEX_FEED_HOLD; } if (digitalRead(CONTROL_CYCLE_START_PIN)) { control_state |= CONTROL_PIN_INDEX_CYCLE_START; } diff --git a/Grbl_Esp32/tests/parsetest.nc b/Grbl_Esp32/tests/parsetest.nc new file mode 100644 index 00000000..2ef7beea --- /dev/null +++ b/Grbl_Esp32/tests/parsetest.nc @@ -0,0 +1,6 @@ +G21 +G90 (A standard comment) +G1 Z3.810 F228.6 ; a LinuxCNC style comment +G0x0x0 (some lowercase) +G0 X10 (internal comment) Y0 +G0X0 (internal comment; with semi colon) Y0Z3 diff --git a/README.md b/README.md index 77ab6f49..ea1f15a6 100644 --- a/README.md +++ b/README.md @@ -2,18 +2,20 @@ # Grbl (CNC Controller) For ESP32 -![ESP32](http://www.buildlog.net/blog/wp-content/uploads/2018/08/20180819_170123.jpg) +![ESP32](http://www.buildlog.net/blog/wp-content/uploads/2018/07/20180718_175943.jpg) ### Status: Functional Beta (See minor issues below) + + ### Project Overview -This is a port of [Grbl](https://github.com/gnea/grbl) for the ESP32. The ESP32 is a great target for Grbl for the following reasons +This is a port of [Grbl](https://github.com/gnea/grbl) for the ESP32. The ESP32 is potentially a great target for Grbl for the following reasons - **Faster** - Faster step rates and additional features could be added - **Lower Cost** - - **Small footprint** - -- **Lots of Flash and RAM** - A larger planner buffer could be used and more features could be added. +- **More Flash and RAM** - A larger planner buffer could be used and more features could be added. - **I/O** - It has just about the same number of pins as an Arduino UNO, the original target for Grbl - **Peripherals** - It has more timers and advanced features than an UNO. These can also be mapped to pins more flexibly. - **Connectivity** - Bluetooth and WiFi built in. @@ -21,12 +23,10 @@ This is a port of [Grbl](https://github.com/gnea/grbl) for the ESP32. The ESP32 ### Issues / Changes 1. **Direction pin delay** - Not implemented yet. Some drivers require a couple of microseconds after the direction pin is set before you start the step pulse. The original plan was to [use the RMT feature](http://www.buildlog.net/blog/?s=rmt), but that has issues when trying to use it in an Interrupt. **This is typically a option in Grbl that is not used.** -2. **Limit Switch debouncing** is not supported yet. It might be better to us an R/C filter for now. The controller shown above uses R/C filters, which seems to solve the problem. +2. **Limit Switch debouncing** is not supported yet. It does not seem to be a problem on my test rigs. The dev board uses R/C filters on all inputs. 3. **Step Pulse Invert:** This has not been fully tested. I suggest...leaving $2=0 -### Hardware: -The hardware in the image above is an open source, reference design that is [for sale at Tindie](https://www.tindie.com/products/33366583/grblesp32-cnc-development-board/). The source files are on this [blog post](http://www.buildlog.net/blog/2018/08/grbl_esp32-cnc-development-board/). ### Using It @@ -36,20 +36,20 @@ I use the NodeMCU 32S version of the ESP32. I suggest starting with that if you For basic instructions on using Grbl use the [gnea/grbl wiki](https://github.com/gnea/grbl/wiki). That is the Arduino version of Grbl, so keep that in mind regarding hardware setup. If you have questions ask via the GitHub issue system for this project. -Note: Unlike Grbl on Arduinos, the controller does not reboot when you connect to it via USB. This may cause confusion on some older senders. Most modern Grbl senders will send a reset (Ctrl-X or 0x18) instead of relying on the reboot. I do not plan on changing this. It is better to do reboot when a connection is opened. - Be sure you have external pullup resistors on any GPIO34-39 that you use. These default to door, start, hold and reset functions. -[Bluetooth wiki page](https://github.com/bdring/Grbl_Esp32/wiki/Using-Bletooth) +Using Bluetooth: -[SD Card wiki page](https://github.com/bdring/Grbl_Esp32/wiki/Using-the-SD-Card) +- [See the Wiki page](https://github.com/bdring/Grbl_Esp32/wiki/Using-Bletooth) -### TODO List +Using SD Card -- 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 -- Wifi -- Web Server with browser based control and setup. +- [See this wiki page](https://github.com/bdring/Grbl_Esp32/wiki/Using-the-SD-Card) + +### Roadmap + +- Add Wifi support and a web page interface +- Add spindle enable and direction. ### Credits @@ -59,15 +59,13 @@ The original [Grbl](https://github.com/gnea/grbl) is an awesome project by Sunge I would love to have help with this project. There are many things that need to be improved and added, especially BlueTooth and/or WiFi. If you need hardware to test with, I might be able to help with a test PCB. -![](http://www.buildlog.net/blog/wp-content/uploads/2018/07/slack_hash_128.png) [Slack channel](https://join.slack.com/t/buildlog/shared_invite/enQtNDA1ODM5MzI3MjE2LWYxNzMwZmNmMWVhYmUzMDdiYWQxMjk2MWQ1NzJhYzc2Mjg5NmRjMWI2MmM3OGE4M2JiZWQ2MThjMjQ3Y2U2OTE) for this project. +![](http://www.buildlog.net/blog/wp-content/uploads/2018/07/slack_hash_128.png) There is a slack channel for the development this project. Ask for an Invite ### FAQ Start asking questions...I'll put the frequent ones here. -### Donate? -Please considering donating to this open source project. -[![paypal](https://www.paypalobjects.com/en_US/i/btn/btn_donate_LG.gif)](https://www.paypal.com/cgi-bin/webscr?cmd=_s-xclick&hosted_button_id=3ZAF7SF5PCXTN) + diff --git a/doc/csv/build_option_codes_en_US.csv b/doc/csv/build_option_codes_en_US.csv index 867b489c..9a6d0fb4 100644 --- a/doc/csv/build_option_codes_en_US.csv +++ b/doc/csv/build_option_codes_en_US.csv @@ -19,5 +19,4 @@ $,Restore EEPROM `$` settings command,Disabled I,Build info write user string command,Disabled E,Force sync upon EEPROM write,Disabled W,Force sync upon work coordinate offset change,Disabled -L,Homing initialization auto-lock,Disabled -B,Bluetooth Enabled \ No newline at end of file +L,Homing initialization auto-lock,Disabled \ No newline at end of file diff --git a/doc/csv/error_codes_en_US.csv b/doc/csv/error_codes_en_US.csv index c447054f..43d0b984 100644 --- a/doc/csv/error_codes_en_US.csv +++ b/doc/csv/error_codes_en_US.csv @@ -34,4 +34,10 @@ "35","Invalid gcode ID:35","G2 and G3 arcs require at least one in-plane offset word." "36","Invalid gcode ID:36","Unused value words found in block." "37","Invalid gcode ID:37","G43.1 dynamic tool length offset is not assigned to configured tool length axis." -"38","Invalid gcode ID:38","Tool number greater than max supported value." \ No newline at end of file +"38","Invalid gcode ID:38","Tool number greater than max supported value." +"60","SD failed to mount" +"61","SD card failed to open file for reading" +"62","SD card failed to open directory" +"63","SD Card directory not found" +"64","SD Card file empty" +"70","Bluetooth failed to start"