diff --git a/.gitignore b/.gitignore index 077f454b7..a6ac31d48 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,12 @@ embedded/dist .vscode/ .history/ *.pyc +.vs/ +Debug/ +Release/ +*.vsarduino.h +__vm/ +*.user +*.vcxproj +*.vcxproj.filters +*.suo diff --git a/Grbl_Esp32.sln b/Grbl_Esp32.sln new file mode 100644 index 000000000..0ae0bade4 --- /dev/null +++ b/Grbl_Esp32.sln @@ -0,0 +1,31 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.29306.81 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Grbl_Esp32", "Grbl_Esp32.vcxproj", "{11C8A44F-A303-4885-B5AD-5B65F7FE41C0}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x64.ActiveCfg = Debug|x64 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x64.Build.0 = Debug|x64 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x86.ActiveCfg = Debug|Win32 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Debug|x86.Build.0 = Debug|Win32 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x64.ActiveCfg = Release|x64 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x64.Build.0 = Release|x64 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x86.ActiveCfg = Release|Win32 + {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {EEC94F4B-059C-4596-94B8-1C4C9CE5E0DD} + EndGlobalSection +EndGlobal diff --git a/Grbl_Esp32/Custom/torchHeightControl.cpp b/Grbl_Esp32/Custom/torchHeightControl.cpp new file mode 100644 index 000000000..ea78c5b1d --- /dev/null +++ b/Grbl_Esp32/Custom/torchHeightControl.cpp @@ -0,0 +1,176 @@ +/* + torchHeightControl.cpp + Part of Grbl_ESP32 + + copyright (c) 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 + 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 . + + -------------------------------------------------------------- + + This contains all the special features required for "Plasma Torch Height Control" + This Torch Height Control Custom File Was Created By: William Curry +*/ +// This file is enabled by defining: CUSTOM_CODE_FILENAME "Custom/torchHeightControl.cpp" +// in Machines/6_pack_thc.h, thus causing this file to be included +// from ../custom_code.cpp +static TaskHandle_t THCSyncTaskHandle = 0; +static TaskHandle_t THCVoltageTaskHandle = 0; +unsigned long THCCounter = 0; //For debugging only +unsigned long lastDebugPrintTimeMillis; //For debugging only, last time debug info was printed +bool alwaysPrintWhenTHCRunning = true; +unsigned long arcOnTime; //milliseconds at which plasma arc was turned on +//Be careful with this next setting becuase if you send to many steps to stepper.cpp per iteration it may overrun how many steps the machine can do per milliseond at slower x/y speeds +int numberStepsPerIteration = 1;//How many steps we want to send to the stepper.cpp for each iteration of THC this will be an adjustable setting in the future %%%%%%%%%% +bool thcRunning; +int thcIterationMs = 20; +int directionDownPinState = 1; +int directionUpPinState = 0; +float thcPinVoltage; +float torchVoltage; +float torchVoltageFiltered; ///Current filtered value used for THC routine +float voltageSetpoint; +int voltageInt; +int currentDirectionState; ////get state of direction pin +bool directionDifference; +int thcZStep; +int thcDirDown; +// Function // Step Z Down Voltage too high +void thcStepZDown(){ + if ((thc_debug_setting->get()||alwaysPrintWhenTHCRunning) && ((millis() - lastDebugPrintTimeMillis) > thc_debugprint_millis->get()) ) + { + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Setpoint = %4.1f THC Voltage = %4.1f Moving Z Down", voltageSetpoint, torchVoltageFiltered); + lastDebugPrintTimeMillis = millis(); + } + thcZStep = numberStepsPerIteration; // set the number of steps per iteration of THC so stepper.cpp can execute the steps + thcDirDown = 1; //Set the step direction so stepper.cpp knows which way to step +} + +// Function // Step Z Up Voltage too Low +void thcStepZUp(){ + if ((thc_debug_setting->get()||alwaysPrintWhenTHCRunning) && ((millis() - lastDebugPrintTimeMillis) > thc_debugprint_millis->get()) ) + { + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Setpoint = %4.1f THC Voltage = %4.1f Moving Z Up", voltageSetpoint, torchVoltageFiltered); + lastDebugPrintTimeMillis = millis(); + } + thcZStep = numberStepsPerIteration; // set the number of steps per iteration of THC so stepper.cpp can execute the steps + thcDirDown = 0; //Set the step direction so stepper.cpp knows which way to step +} + +void machine_init() { + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "Bill's THC Initialized"); + // setup a task that will do torch height control + xTaskCreatePinnedToCore(THCSyncTask, // task + "THCSyncTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + &THCSyncTaskHandle, + 0 // core + ); + // setup a task that will filter the torch voltage + xTaskCreatePinnedToCore(THCVoltageTask, // task + "THCVoltageTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + &THCVoltageTaskHandle, + 0 // core + ); +} + +// this task is the main THC loop +void THCSyncTask(void* pvParameters) { + TickType_t xthcWakeTime; + const TickType_t xTHCFrequency = (thc_iter_freq -> get()) + 1; // (ms) + xthcWakeTime = xTaskGetTickCount(); // Initialise the xthcWakeTime variable with the current time. + while (true) { // don't ever return from this or the task dies + //Get the state of the plasma cutter torch on relay + uint8_t plasmaState = coolant_get_state(); //Using the coolant flood output to turn on the plasma cutter + if(sys.suspend) + { + coolant_set_state(COOLANT_DISABLE); //Disable plasma if system state is suspended + } + if(plasmaState && (voltageSetpoint > 30) && !sys.suspend) //Plasma Has Been Turned On and the Voltage Setpoint is greater than 30 volts Start The THC Routine + { + if((millis()- arcOnTime) > (thc_arc_delay_time->get())) + { + thcRunning = true; + //digitalWrite(I2SO(24),HIGH); + //digitalWrite(I2SO(25),HIGH); + //digitalWrite(I2SO(26),HIGH); + //digitalWrite(I2SO(27),HIGH); + } + else + { + thcRunning = false; + } + } + else + { + arcOnTime = millis(); //Reset arc on delay timer + thcRunning = false; + thcZStep = 0; //Reset all the steps we sent to stepper.cpp + if (thc_debug_setting->get() && ((millis() - lastDebugPrintTimeMillis) > thc_debugprint_millis->get()) ) + { + //grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Interation # %d", THCCounter); + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Voltage Filt = %4.1f", torchVoltageFiltered); + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Voltage Unfiltered = %4.1f", torchVoltage); + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Pin Voltage = %3.2f", thcPinVoltage); + grbl_msg_sendf(CLIENT_ALL, MSG_LEVEL_INFO, "THC Voltage Setting = %4.1f", voltageSetpoint); + lastDebugPrintTimeMillis = millis(); + } + //digitalWrite(I2SO(24),LOW); + //digitalWrite(I2SO(25),LOW); + //digitalWrite(I2SO(26),LOW); + //digitalWrite(I2SO(27),LOW); + } + + if((torchVoltageFiltered > voltageSetpoint) && thcRunning) //Voltage is too high and were running THC step Z down + { + thcStepZDown(); + } + else if((torchVoltageFiltered < voltageSetpoint)&& thcRunning) //Voltage is too low and were running THC step Z up + { + thcStepZUp(); + } + + THCCounter ++; + voltageSetpoint = (thc_voltage_setting -> get()); + + vTaskDelayUntil(&xthcWakeTime, thc_iter_freq -> get() + 1); //Adding +1 so the loop doesn't crash if set to 0 + } +} + +// this task is THC Voltage Filtering Loop +void THCVoltageTask(void* pvParameters) { + TickType_t xLastVoltageWakeTime; + const TickType_t xTHCVoltageFrequency = 2; // (ms) + xLastVoltageWakeTime = xTaskGetTickCount(); // Initialise the xLastVoltageWakeTime variable with the current time. + while (true) {// don't ever return from this or the task dies + voltageInt = analogRead(THC_VOLTAGE_PIN); + thcPinVoltage = voltageInt * (3.3 / 4095); //0-3.3 volts at torch input pin + torchVoltage = (thcPinVoltage*(VOLTAGE_DIVIDER_R1+VOLTAGE_DIVIDER_R2))/VOLTAGE_DIVIDER_R2;//0-207 volts for R1 = 470K R2 = 7.6K + if(thcRunning) ///If the Main THC Loop is running Start filtering the voltage + { + torchVoltageFiltered = torchVoltageFiltered * (thc_voltage_filter_value -> get()) + torchVoltage * (1-(thc_voltage_filter_value -> get())); //Rough filter for voltage input + } + else + { + torchVoltageFiltered = torchVoltage; + } + vTaskDelayUntil(&xLastVoltageWakeTime, xTHCVoltageFrequency + 1); + } +} \ No newline at end of file diff --git a/Grbl_Esp32/Machines/6_pack_stepstick_v1.h b/Grbl_Esp32/Machines/6_pack_stepstick_v1.h index cb844d8fd..1c02e4da1 100644 --- a/Grbl_Esp32/Machines/6_pack_stepstick_v1.h +++ b/Grbl_Esp32/Machines/6_pack_stepstick_v1.h @@ -83,8 +83,8 @@ #define Y_LIMIT_PIN GPIO_NUM_32 #define Z_LIMIT_PIN GPIO_NUM_35 #define A_LIMIT_PIN GPIO_NUM_34 -#define B_LIMIT_PIN GPIO_NUM_39 -#define C_LIMIT_PIN GPIO_NUM_36 +//#define B_LIMIT_PIN GPIO_NUM_39 +//#define C_LIMIT_PIN GPIO_NUM_36 #define PROBE_PIN GPIO_NUM_25 @@ -117,13 +117,13 @@ #define COOLANT_FLOOD_PIN I2SO(27) */ -/* + // RS485 In socket #3 #define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG // only one spindle at a time #define HUANYANG_TXD_PIN GPIO_NUM_26 #define HUANYANG_RTS_PIN GPIO_NUM_4 #define HUANYANG_RXD_PIN GPIO_NUM_16 -*/ + // === Default settings diff --git a/Grbl_Esp32/Machines/6_pack_thc.h b/Grbl_Esp32/Machines/6_pack_thc.h new file mode 100644 index 000000000..104924538 --- /dev/null +++ b/Grbl_Esp32/Machines/6_pack_thc.h @@ -0,0 +1,132 @@ +/* + 6_pack_thc.h + + Covers all V1 versions V1p0, V1p1, etc + + Part of Grbl_ESP32 + Pin assignments for the ESP32 I2S 6-axis board + 2018 - Bart Dring + 2020 - Mitch Bradley + 2020 - Michiyasu Odaki + 2020 - William Curry - Plasma Torch Height Control + Grbl_ESP32 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_ESP32. If not, see . + This 6-Pack Torch Height Control Custom File Was Created By: William Curry +*/ +#define MACHINE_NAME "6 Pack Controller with Plasma Torch Height Control" +#define CUSTOM_CODE_FILENAME "Custom/torchHeightControl.cpp"//define the torch height control custom code file +// tells grbl we have some special functions to call +#define USE_MACHINE_INIT +#ifdef N_AXIS + #undef N_AXIS +#endif +#define N_AXIS 3 +//Enable the SD Card +#ifndef ENABLE_SD_CARD + #define ENABLE_SD_CARD +#endif +/// THC Settings, these can be displayed in G Code Sender by Typing $S to show strings or $+ to show numbers like shown below +#define SHOW_EXTENDED_SETTINGS +#define DEFAULT_THC_DEBUG 0 //$300=1// Boolean //If true this will print torch height debug information out +#define DEFAULT_THC_DEBUG_PRINT_MILLIS 5000 //$301=1000//milliseconds // debug print time in milliseconds +#define DEFAULT_THC_TARGET_VOLTAGE 0 //$302=0// Volts // default target voltage (a real value would be 100 volts) this must be greater than 30 volts for THC to run +#define DEFAULT_THC_ARC_DELAY_TIME 750 //$303=750//milliseconds // Time for Arc to Start before running THC +#define DEFAULT_THC_VOLTAGE_FILTER_VALUE 0.98 //$304=0.98//ND// Torch Voltage Filter Time Constant +#define DEFAULT_THC_ITER_FREQ 5 //$305=5//milliseconds// Torch Height Control Time Between Calls, this will directly effect ... +// ... the rate at which the Z axis moves. A higher muber means the Z axis will move slower since THC is called less often + +// === Special Features +///Define Torch Height Control +#define torchHeightControl //Used in stepper.cpp +/// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +// Define USE_I2S_OUT_STREAM if buffering is used. +// (there will be a delay between the specified I/O operation and the actual I/O execution) +#define USE_I2S_OUT_STREAM +#undef USE_RMT_STEPS +#define STEP_PULSE_DELAY 10 //Mircoseconds to delay between setting direction pin and setting step pin high +//#define USE_STEPSTICK //Bill comment makes sure MS1,2,3 !reset and !sleep are set +/* #ifdef ENABLE_WIFI + #undef ENABLE_WIFI ///Turn off wifi since this blocks the ADC Channel 2 Pins from Reading in Voltage Which is needed for THC +#endif */ + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + +/// Axis Data +#define USE_GANGED_AXES // allow two motors on an axis + +#define STEPPER_Z_MS3 I2SO(3) // Z_CS +#define STEPPER_Y_MS3 I2SO(6) // Y_CS +#define STEPPER_X_MS3 I2SO(11) // X_CS +#define STEPPER_Y2_MS3 I2SO(14) // Y2/A_CS +//#define STEPPER_B_MS3 I2SO(19) // B_CS +//#define STEPPER_C_MS3 I2SO(22) // C_CS +#define STEPPER_RESET GPIO_NUM_19 +//Terminal 1 +#define Z_DISABLE_PIN I2SO(0) +#define Z_DIRECTION_PIN I2SO(1) +#define Z_STEP_PIN I2SO(2) + +//Y Is a Ganged Motor +#define Y_AXIS_SQUARING +//Terminal 2 +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) +//Terminal 4 +#define Y2_DIRECTION_PIN I2SO(12) //Motor A Dir +#define Y2_STEP_PIN I2SO(13) //Motor A Step +#define Y2_DISABLE_PIN I2SO(15) //Motor A Disable + +///Terminal 3 +#define X_DISABLE_PIN I2SO(8) +#define X_DIRECTION_PIN I2SO(9) +#define X_STEP_PIN I2SO(10) +/* Bill Comment out these axis' + +#define B_DISABLE_PIN I2SO(16) +#define B_DIRECTION_PIN I2SO(17) +#define B_STEP_PIN I2SO(18) + +#define C_DIRECTION_PIN I2SO(20) +#define C_STEP_PIN I2SO(21) +#define C_DISABLE_PIN I2SO(23) +*/ +/// CNC Modules +///CNC Module #1 THC Voltage +#define THC_VOLTAGE_PIN GPIO_NUM_32 + +///Resistance Values needed to determine arc voltage i.e. Vout = (Vs*R2)/(R1+R2) +#define VOLTAGE_DIVIDER_R1 470 ///470K Ohms +#define VOLTAGE_DIVIDER_R2 7.6 ///7.6K Ohms + +//CNC Module # 2 Opto Isolated +#define X_LIMIT_PIN GPIO_NUM_2 +#define Y_LIMIT_PIN GPIO_NUM_25 +#define Z_LIMIT_PIN GPIO_NUM_39 +#define PROBE_PIN GPIO_NUM_36 + +///CNC Module # 3 Spindle Relay +#define SPINDLE_TYPE SPINDLE_TYPE_RELAY +#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 +//#define COOLANT_MIST_PIN GPIO_NUM_26 +///CNC Module # 4 Plasma Torch Relay AKA Coolant Flood Pin +#define COOLANT_FLOOD_PIN GPIO_NUM_14 + +///CNC Module # 5 Empty + +///Torch Height Control Custom Functions +void THCSyncTask(void* pvParameters); //Task called for Torch height controlled thats defined in torchHeightControl.cpp +void THCVoltageTask(void* pvParameters); //Task called for Torch height controlled thats defined in torchHeightControl.cpp +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/SettingsDefinitions.cpp b/Grbl_Esp32/SettingsDefinitions.cpp index 9c10c1a29..1e156b72d 100644 --- a/Grbl_Esp32/SettingsDefinitions.cpp +++ b/Grbl_Esp32/SettingsDefinitions.cpp @@ -49,6 +49,7 @@ IntSetting* spindle_pwm_bit_precision; EnumSetting* spindle_type; + enum_opt_t spindleTypes = { { "NONE", SPINDLE_TYPE_NONE, }, { "PWM", SPINDLE_TYPE_PWM, }, @@ -59,6 +60,13 @@ enum_opt_t spindleTypes = { { "BESC", SPINDLE_TYPE_BESC, }, { "10V", SPINDLE_TYPE_10V, }, }; +//Torch Height Control Settings +FlagSetting* thc_debug_setting; +FloatSetting* thc_voltage_setting; +IntSetting* thc_debugprint_millis; +IntSetting* thc_arc_delay_time; +FloatSetting* thc_voltage_filter_value; +IntSetting* thc_iter_freq; AxisSettings* x_axis_settings; AxisSettings* y_axis_settings; @@ -317,4 +325,11 @@ void make_settings() { pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); spindle_type = new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", SPINDLE_TYPE, &spindleTypes); stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, checkStallguardDebugMask); + //Register THC Settings + thc_debug_setting = new FlagSetting(EXTENDED, WG, "300", "THCDebug(0/1)", DEFAULT_THC_DEBUG); + thc_debugprint_millis = new IntSetting(EXTENDED, WG, "301", "THCDebugPrintTime(ms)", DEFAULT_THC_DEBUG_PRINT_MILLIS, 500, 10000); + thc_voltage_setting = new FloatSetting(EXTENDED, WG, "302", "THCVoltageTarget(volts)", DEFAULT_THC_TARGET_VOLTAGE, 0, 200); + thc_arc_delay_time = new IntSetting(EXTENDED, WG, "303", "THCArcOnDelayTime(ms)", DEFAULT_THC_ARC_DELAY_TIME, 0, 1000); + thc_voltage_filter_value = new FloatSetting(EXTENDED, WG, "304", "THCVoltageFilterTc", DEFAULT_THC_VOLTAGE_FILTER_VALUE, 0.0, 0.999); + thc_iter_freq = new IntSetting(EXTENDED, WG, "305", "THCIterFreq(ms)", DEFAULT_THC_ITER_FREQ, 1, 1000); } diff --git a/Grbl_Esp32/SettingsDefinitions.h b/Grbl_Esp32/SettingsDefinitions.h index 0e9de4427..f9bc00081 100644 --- a/Grbl_Esp32/SettingsDefinitions.h +++ b/Grbl_Esp32/SettingsDefinitions.h @@ -51,3 +51,10 @@ extern IntSetting* spindle_pwm_bit_precision; extern EnumSetting* spindle_type; extern AxisMaskSetting* stallguard_debug_mask; +//Plasma Torch Height Control Settings +extern FlagSetting* thc_debug_setting; +extern FloatSetting* thc_voltage_setting; +extern IntSetting* thc_debugprint_millis; +extern IntSetting* thc_arc_delay_time; +extern FloatSetting* thc_voltage_filter_value; +extern IntSetting* thc_iter_freq; \ No newline at end of file diff --git a/Grbl_Esp32/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/Spindles/HuanyangSpindle.cpp index e46c777c0..fdd9e81fc 100644 --- a/Grbl_Esp32/Spindles/HuanyangSpindle.cpp +++ b/Grbl_Esp32/Spindles/HuanyangSpindle.cpp @@ -2,6 +2,8 @@ HuanyangSpindle.cpp This is for a Huanyang VFD based spindle via RS485 Modbus. + Sorry for the lengthy comments, but finding the details on this + VFD was a PITA. I am just trying to help the next person. Part of Grbl_ESP32 2020 - Bart Dring @@ -21,6 +23,21 @@ VFDs are very dangerous. They have high voltages and are very powerful Remove power before changing bits. + ============================================================================== + + If a user changes state or RPM level, the command to do that is sent. If + the command is not responded to a message is sent to serial that there was + a timeout. If the Grbl is in a critical state, an alarm will be generated and + the machine stopped. + + If there are no commands to execute, various status items will be polled. If there + is no response, it will behave as described above. It will stop any running jobs with + an alarm. + + =============================================================================== + + Protocol Details + VFD frequencies are in Hz. Multiply by 60 for RPM before using spindle, VFD must be setup for RS485 and match your spindle @@ -32,20 +49,65 @@ PD015 10 Deceleration time (Test to optimize) PD023 1 Reverse run enabled PD142 3.7 Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??) + PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) PD163 1 RS485 Address: 1 (Typical. OK to change...see below) PD164 1 RS485 Baud rate: 9600 (Typical. OK to change...see below) PD165 3 RS485 Mode: RTU, 8N1 - Some references.... - Manual: http://www.hy-electrical.com/bf/inverter.pdf + The official documentation of the RS485 is horrible. I had to piece it together from + a lot of different sources + + Manuals: https://github.com/RobertOlechowski/Huanyang_VFD/tree/master/Documentations/pdf Reference: https://github.com/Smoothieware/Smoothieware/blob/edge/src/modules/tools/spindle/HuanyangSpindleControl.cpp Refernece: https://gist.github.com/Bouni/803492ed0aab3f944066 VFD settings: https://www.hobbytronics.co.za/Content/external/1159/Spindle_Settings.pdf + Spindle Talker 2 https://github.com/GilchristT/SpindleTalker2/releases + Python https://github.com/RobertOlechowski/Huanyang_VFD + + ========================================================================= + + Commands + ADDR CMD LEN DATA CRC + 0x01 0x03 0x01 0x01 0x31 0x88 Start spindle clockwise + 0x01 0x03 0x01 0x08 0xF1 0x8E Stop spindle + 0x01 0x03 0x01 0x11 0x30 0x44 Start spindle counter-clockwise + + Return values are + 0 = run + 1 = jog + 2 = r/f + 3 = running + 4 = jogging + 5 = r/f + 6 = Braking + 7 = Track start + + ========================================================================== + + Setting RPM + ADDR CMD LEN DATA CRC + 0x01 0x05 0x02 0x09 0xC4 0xBF 0x0F Write Frequency (0x9C4 = 2500 = 25.00HZ) + + Response is same as data sent + + ========================================================================== + + Status registers + Addr Read Len Reg DataH DataL CRC CRC + 0x01 0x04 0x03 0x00 0x00 0x00 CRC CRC // Set Frequency * 100 (25Hz = 2500) + 0x01 0x04 0x03 0x01 0x00 0x00 CRC CRC // Ouput Frequency * 100 + 0x01 0x04 0x03 0x02 0x00 0x00 CRC CRC // Ouput Amps * 10 + 0x01 0x04 0x03 0x03 0x00 0x00 0xF0 0x4E // Read RPM (example CRC shown) + 0x01 0x04 0x03 0x0 0x00 0x00 CRC CRC // DC voltage + 0x01 0x04 0x03 0x05 0x00 0x00 CRC CRC // AC voltage + 0x01 0x04 0x03 0x06 0x00 0x00 CRC CRC // Cont + 0x01 0x04 0x03 0x07 0x00 0x00 CRC CRC // VFD Temp + Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above) + + TODO: + Move CRC Calc to task to free up main task + - TODO - Returning errors to Grbl and handling them in Grbl. - What happens if the VFD does not respond - Add periodic pinging of VFD in run mode to see if it is still at correct RPM */ #include "SpindleClass.h" @@ -54,8 +116,10 @@ #define HUANYANG_UART_PORT UART_NUM_2 // hard coded for this port right now #define ECHO_TEST_CTS UART_PIN_NO_CHANGE // CTS pin is not used #define HUANYANG_BUF_SIZE 127 +#define HUANYANG_QUEUE_SIZE 10 // numv\ber of commands that can be queued up. #define RESPONSE_WAIT_TICKS 50 // how long to wait for a response #define HUANYANG_MAX_MSG_SIZE 16 // more than enough for a modbus message +#define HUANYANG_POLL_RATE 200 // in milliseconds betwwen commands // OK to change these // #define them in your machine definition file if you want different values @@ -75,12 +139,27 @@ typedef struct { char msg[HUANYANG_MAX_MSG_SIZE]; } hy_command_t; +typedef enum : uint8_t { + READ_SET_FREQ = 0, // The set frequency + READ_OUTPUT_FREQ = 1, // The current operating frequency + READ_OUTPUT_AMPS = 2, // + READ_SET_RPM = 3, // This is the last requested freq even in off mode + READ_DC_VOLTAGE = 4, // + READ_AC_VOLTAGE = 5, // + READ_CONT = 6, // counting value??? + READ_TEMP = 7, // +} read_register_t; + QueueHandle_t hy_cmd_queue; static TaskHandle_t vfd_cmdTaskHandle = 0; +bool hy_spindle_ok = true; + // The communications task void vfd_cmd_task(void* pvParameters) { + static bool unresponsive = false; // to pop off a message once each time it becomes unresponsive + uint8_t reg_item = 0x00; hy_command_t next_cmd; uint8_t rx_message[HUANYANG_MAX_MSG_SIZE]; @@ -88,33 +167,55 @@ void vfd_cmd_task(void* pvParameters) { if (xQueueReceive(hy_cmd_queue, &next_cmd, 0) == pdTRUE) { uart_flush(HUANYANG_UART_PORT); + //report_hex_msg(next_cmd.msg, "Tx: ", next_cmd.tx_length); uart_write_bytes(HUANYANG_UART_PORT, next_cmd.msg, next_cmd.tx_length); uint16_t read_length = uart_read_bytes(HUANYANG_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_TICKS); if (read_length < next_cmd.rx_length) { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RS485 Unresponsive"); - if (next_cmd.critical) - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Critical Spindle RS485 Unresponsive"); - // TODO Do something with this error - system_set_exec_alarm(EXEC_ALARM_SPINDLE_CONTROL); + if (!unresponsive) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RS485 Unresponsive %d", read_length); + if (next_cmd.critical) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Critical Spindle RS485 Unresponsive"); + system_set_exec_alarm(EXEC_ALARM_SPINDLE_CONTROL); + } + unresponsive = true; + } } else { - + // success + unresponsive = false; + //report_hex_msg(rx_message, "Rx: ", read_length); + uint32_t ret_value = ((uint32_t)rx_message[4] << 8) + rx_message[5]; + //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Item:%d value:%05d ", rx_message[3], ret_value); } + } else { - // TODO: Should we ping the spindle here to make sure it does not go off line? - // But there is virtually no real documentation on how to do this. + HuanyangSpindle :: read_value(reg_item); // only this appears to work all the time. Other registers are flakey. + if (reg_item < 0x03) + reg_item++; + else + { + reg_item = 0x00; + } + } - vTaskDelay(100); // TODO: What is the best value here? + vTaskDelay(HUANYANG_POLL_RATE); // TODO: What is the best value here? } } // ================== Class methods ================================== void HuanyangSpindle :: init() { + hy_spindle_ok = true; // initialize + + // fail if required items are not defined + if (!get_pins_and_settings()) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle errors"); + return; + } if (! _task_running) { // init can happen many times, we only want to start one task - hy_cmd_queue = xQueueCreate(5, sizeof(hy_command_t)); + hy_cmd_queue = xQueueCreate(HUANYANG_QUEUE_SIZE, sizeof(hy_command_t)); xTaskCreatePinnedToCore(vfd_cmd_task, // task "vfd_cmdTaskHandle", // name for task 2048, // size of task stack @@ -124,13 +225,7 @@ void HuanyangSpindle :: init() { 0 // core ); _task_running = true; - } - - // fail if required items are not defined - if (!get_pins_and_settings()) { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle errors"); - return; - } + } // this allows us to init() again later. // If you change certain settings, init() gets called agian @@ -176,30 +271,38 @@ void HuanyangSpindle :: init() { // It returns a message for each missing pin // Returns true if all pins are defined. bool HuanyangSpindle :: get_pins_and_settings() { - bool pins_ok = true; + #ifdef HUANYANG_TXD_PIN _txd_pin = HUANYANG_TXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Missing HUANYANG_TXD_PIN"); - pins_ok = false; + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Undefined HUANYANG_TXD_PIN"); + hy_spindle_ok = false; #endif #ifdef HUANYANG_RXD_PIN _rxd_pin = HUANYANG_RXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No HUANYANG_RXD_PIN"); - pins_ok = false; + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Undefined HUANYANG_RXD_PIN"); + hy_spindle_ok = false; #endif #ifdef HUANYANG_RTS_PIN _rts_pin = HUANYANG_RTS_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No HUANYANG_RTS_PIN"); - pins_ok = false; + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Undefined HUANYANG_RTS_PIN"); + hy_spindle_ok = false; #endif - return pins_ok; + if (laser_mode->get()) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); + hy_spindle_ok = false; + } + + _min_rpm = rpm_min->get(); + _max_rpm = rpm_max->get(); + + return hy_spindle_ok; } void HuanyangSpindle :: config_message() { @@ -209,24 +312,15 @@ void HuanyangSpindle :: config_message() { pinName(_txd_pin).c_str(), pinName(_rxd_pin).c_str(), pinName(_rts_pin).c_str()); - } -/* - ADDR CMD LEN DATA CRC - 0x01 0x03 0x01 0x01 0x31 0x88 Start spindle clockwise - 0x01 0x03 0x01 0x08 0xF1 0x8E Stop spindle - 0x01 0x03 0x01 0x11 0x30 0x44 Start spindle counter-clockwise - - 0x01 0x04 0x03 0x00 0x00 0x00 0xF0 0x4E Read Frequency -*/ void HuanyangSpindle :: set_state(uint8_t state, uint32_t rpm) { if (sys.abort) return; // Block during abort. bool critical = (sys.state == STATE_CYCLE || state != SPINDLE_DISABLE); - + if (_current_state != state) { // already at the desired state. This function gets called a lot. set_mode(state, critical); // critical if we are in a job set_rpm(rpm); @@ -251,6 +345,9 @@ void HuanyangSpindle :: set_state(uint8_t state, uint32_t rpm) { } bool HuanyangSpindle :: set_mode(uint8_t mode, bool critical) { + + if (!hy_spindle_ok) return false; + hy_command_t mode_cmd; mode_cmd.tx_length = 6; @@ -260,32 +357,47 @@ bool HuanyangSpindle :: set_mode(uint8_t mode, bool critical) { mode_cmd.msg[1] = 0x03; mode_cmd.msg[2] = 0x01; - if (mode == SPINDLE_ENABLE_CW) + if (mode == SPINDLE_ENABLE_CW) mode_cmd.msg[3] = 0x01; else if (mode == SPINDLE_ENABLE_CCW) mode_cmd.msg[3] = 0x11; - else //SPINDLE_DISABLE - mode_cmd.msg[3] = 0x08; + else { //SPINDLE_DISABLE + mode_cmd.msg[3] = 0x08; + + if (! xQueueReset(hy_cmd_queue)) { + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD spindle off, queue could not be reset"); + } + } add_ModRTU_CRC(mode_cmd.msg, mode_cmd.rx_length); mode_cmd.critical = critical; - ; + if (xQueueSend(hy_cmd_queue, &mode_cmd, 0) != pdTRUE) grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD Queue Full"); return true; } -/* - ADDR CMD LEN DATA CRC - 0x01 0x05 0x02 0x09 0xC4 0xBF 0x0F Write Frequency (0x9C4 = 2500 = 25.00HZ) -*/ + uint32_t HuanyangSpindle :: set_rpm(uint32_t rpm) { + if (!hy_spindle_ok) return 0; + hy_command_t rpm_cmd; + // apply override + rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) + + // apply limits + if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) + rpm = _max_rpm; + else if (rpm != 0 && rpm <= _min_rpm) + rpm = _min_rpm; + + sys.spindle_speed = rpm; + if (rpm == _current_rpm) // prevent setting same RPM twice - return rpm; + return rpm; _current_rpm = rpm; @@ -305,7 +417,7 @@ uint32_t HuanyangSpindle :: set_rpm(uint32_t rpm) { add_ModRTU_CRC(rpm_cmd.msg, rpm_cmd.tx_length); - rpm_cmd.critical = (sys.state == STATE_CYCLE); + rpm_cmd.critical = false; if (xQueueSend(hy_cmd_queue, &rpm_cmd, 0) != pdTRUE) grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD Queue Full"); @@ -313,44 +425,34 @@ uint32_t HuanyangSpindle :: set_rpm(uint32_t rpm) { return rpm; } -// 0x01 0x04 0x03 0x00 0x00 0x00 0xF0 0x4E Read Frequency + // This appears to read the control register and will return an RPM running or not. -uint16_t HuanyangSpindle :: read_rpm() { +void HuanyangSpindle :: read_value(uint8_t reg) { + uint16_t ret_value = 0; hy_command_t read_cmd; uint8_t rx_message[HUANYANG_MAX_MSG_SIZE]; read_cmd.tx_length = 8; - read_cmd.rx_length = 6; + read_cmd.rx_length = 8; read_cmd.msg[0] = HUANYANG_ADDR; read_cmd.msg[1] = 0x04; read_cmd.msg[2] = 0x03; - read_cmd.msg[3] = 0x00; + read_cmd.msg[3] = reg; read_cmd.msg[4] = 0x00; read_cmd.msg[5] = 0x00; - add_ModRTU_CRC(read_cmd.msg, read_cmd.tx_length); - - uart_flush(HUANYANG_UART_PORT); - uart_write_bytes(HUANYANG_UART_PORT, read_cmd.msg, read_cmd.tx_length); - - uint16_t read_length = uart_read_bytes(HUANYANG_UART_PORT, rx_message, read_cmd.rx_length, RESPONSE_WAIT_TICKS); + read_cmd.critical = (sys.state == STATE_CYCLE); // only critical if running a job TBD.... maybe spindle on? - if (read_length < read_cmd.rx_length) - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Read RPM Spindle RS485 Unresponsive"); - else { - uint32_t hz = ((uint32_t)rx_message[4] << 8) + rx_message[5]; - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "speed %d", (hz * 60) / 100); - - report_hex_msg(rx_message, "Rx:", read_length); - } + add_ModRTU_CRC(read_cmd.msg, read_cmd.tx_length); - return 0; + if (xQueueSend(hy_cmd_queue, &read_cmd, 0) != pdTRUE) + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "VFD Queue Full"); } -void HuanyangSpindle ::stop() { +void HuanyangSpindle ::stop() { set_mode(SPINDLE_DISABLE, false); } diff --git a/Grbl_Esp32/Spindles/SpindleClass.h b/Grbl_Esp32/Spindles/SpindleClass.h index 2043d0194..28a7fdd92 100644 --- a/Grbl_Esp32/Spindles/SpindleClass.h +++ b/Grbl_Esp32/Spindles/SpindleClass.h @@ -173,8 +173,12 @@ class HuanyangSpindle : public Spindle { uint8_t get_state(); uint32_t set_rpm(uint32_t rpm); void stop(); - static uint16_t read_rpm(); - static void add_ModRTU_CRC(char* buf, int full_msg_len); + static void read_value(uint8_t reg); + static void add_ModRTU_CRC(char* buf, int full_msg_len); + + protected: + uint32_t _min_rpm; + uint32_t _max_rpm; diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index e50f2103f..5147eb270 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -79,7 +79,7 @@ Some features should not be changed. See notes below. #define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. #define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds -#define USE_RMT_STEPS +//#define USE_RMT_STEPS // Include the file that loads the machine-specific config file. // machine.h must be edited to choose the desired file. @@ -116,7 +116,7 @@ Some features should not be changed. See notes below. #define ENABLE_SD_CARD // enable use of SD Card to run jobs -#define ENABLE_WIFI //enable wifi +//#define ENABLE_WIFI //enable wifi #if defined(ENABLE_WIFI) || defined(ENABLE_BLUETOOTH) #define WIFI_OR_BLUETOOTH @@ -319,7 +319,7 @@ Some features should not be changed. See notes below. // Inverts the spindle enable pin from low-disabled/high-enabled to low-enabled/high-disabled. Useful // for some pre-built electronic boards. -// #define INVERT_SPINDLE_ENABLE_PIN // Default disabled. Uncomment to enable. + #define INVERT_SPINDLE_ENABLE_PIN // Default disabled. Uncomment to enable. // Inverts the spindle PWM output pin from low-disabled/high-enabled to low-enabled/high-disabled. // #define INVERT_SPINDLE_OUTPUT_PIN // Default disabled. Uncomment to enable. diff --git a/Grbl_Esp32/defaults.h b/Grbl_Esp32/defaults.h index 67513c000..3ab18bc03 100644 --- a/Grbl_Esp32/defaults.h +++ b/Grbl_Esp32/defaults.h @@ -114,6 +114,32 @@ #define DEFAULT_HOMING_PULLOFF 1.0 // $27 mm #endif + // =========== THC Stuff =================== + #ifndef DEFAULT_THC_DEBUG + #define DEFAULT_THC_DEBUG 0 //Boolean//If true this will print torch height debug information out + #endif + + #ifndef DEFAULT_THC_TARGET_VOLTAGE + #define DEFAULT_THC_TARGET_VOLTAGE 0 //Volts// default target voltage (a real value would be 100 volts) + #endif + + #ifndef DEFAULT_THC_DEBUG_PRINT_MILLIS + #define DEFAULT_THC_DEBUG_PRINT_MILLIS 1000 //milliseconds// debug print time in milliseconds + #endif + + #ifndef DEFAULT_THC_ARC_DELAY_TIME + #define DEFAULT_THC_ARC_DELAY_TIME 250 //milliseconds// Time for Arc to Start before running THC + #endif + + #ifndef DEFAULT_THC_VOLTAGE_FILTER_VALUE + #define DEFAULT_THC_VOLTAGE_FILTER_VALUE 0.5 //ND// Torch Voltage Filter Time Constant,a higher value represents more ... + //...filtering on the volatage signal + #endif + + #ifndef DEFAULT_THC_ITER_FREQ + #define DEFAULT_THC_ITER_FREQ 20 //milliseconds// Torch Height Control Time Between Calls, this will directly effect ... + // ... the rate at which the Z axis moves, the machine must be restarted after changing this + #endif // ======== SPINDLE STUFF ==================== #ifndef SPINDLE_TYPE #define SPINDLE_TYPE SPINDLE_TYPE_NONE diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index 641ea8435..5741f3375 100644 --- a/Grbl_Esp32/grbl.h +++ b/Grbl_Esp32/grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system #define GRBL_VERSION "1.3a" -#define GRBL_VERSION_BUILD "20200725" +#define GRBL_VERSION_BUILD "20200727" //#include diff --git a/Grbl_Esp32/i2s_out.cpp b/Grbl_Esp32/i2s_out.cpp index 11d88d079..c3344619a 100644 --- a/Grbl_Esp32/i2s_out.cpp +++ b/Grbl_Esp32/i2s_out.cpp @@ -101,8 +101,8 @@ static portMUX_TYPE i2s_out_spinlock = portMUX_INITIALIZER_UNLOCKED; static int i2s_out_initialized = 0; #ifdef USE_I2S_OUT_STREAM -static volatile uint32_t i2s_out_pulse_period; -static uint32_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) +static volatile uint64_t i2s_out_pulse_period; +static uint64_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) static volatile i2s_out_pulse_func_t i2s_out_pulse_func; #endif @@ -340,10 +340,11 @@ static int IRAM_ATTR i2s_fillout_dma_buffer(lldesc_t *dma_desc) { if (i2s_out_pulser_status == STEPPING) { // fillout future DMA buffer (tail of the DMA buffer chains) if (i2s_out_pulse_func != NULL) { + uint32_t old_rw_pos = o_dma.rw_pos; I2S_OUT_PULSER_EXIT_CRITICAL(); // Temporarily unlocked status lock as it may be locked in pulse callback. (*i2s_out_pulse_func)(); // should be pushed into buffer max DMA_SAMPLE_SAFE_COUNT I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock again. - i2s_out_remain_time_until_next_pulse = i2s_out_pulse_period; + i2s_out_remain_time_until_next_pulse = i2s_out_pulse_period - I2S_OUT_USEC_PER_PULSE * (o_dma.rw_pos - old_rw_pos) + 2; if (i2s_out_pulser_status == WAITING) { // i2s_out_set_passthrough() has called from the pulse function. // It needs to go into pass-through mode. @@ -610,9 +611,10 @@ int IRAM_ATTR i2s_out_set_stepping() { return 0; } -int IRAM_ATTR i2s_out_set_pulse_period(uint32_t period) { +int IRAM_ATTR i2s_out_set_pulse_period(uint64_t period) { #ifdef USE_I2S_OUT_STREAM - i2s_out_pulse_period = period; + // Use 64-bit values to avoid overflowing during the calculation. + i2s_out_pulse_period = period * 1000000 / F_STEPPER_TIMER; #endif return 0; } diff --git a/Grbl_Esp32/i2s_out.h b/Grbl_Esp32/i2s_out.h index 4e88ac893..edd8af9c9 100644 --- a/Grbl_Esp32/i2s_out.h +++ b/Grbl_Esp32/i2s_out.h @@ -167,7 +167,7 @@ void i2s_out_delay(); Set the pulse callback period in microseconds (like the timer period for the ISR) */ -int i2s_out_set_pulse_period(uint32_t period); +int i2s_out_set_pulse_period(uint64_t period); /* Register a callback function to generate pulse data diff --git a/Grbl_Esp32/machine.h b/Grbl_Esp32/machine.h index 3f987a948..3a4b37c39 100644 --- a/Grbl_Esp32/machine.h +++ b/Grbl_Esp32/machine.h @@ -16,8 +16,8 @@ PWM // !!! For initial testing, start with test_drive.h which disables // all I/O pins -// #include "Machines/atari_1020.h" -#include "Machines/test_drive.h" +#include "Machines/6_pack_thc.h" +//#include "Machines/test_drive.h" // !!! For actual use, change the line above to select a board // from Machines/, for example: diff --git a/Grbl_Esp32/protocol.cpp b/Grbl_Esp32/protocol.cpp index f61516df1..01d77c664 100644 --- a/Grbl_Esp32/protocol.cpp +++ b/Grbl_Esp32/protocol.cpp @@ -39,21 +39,17 @@ typedef struct { } client_line_t; client_line_t client_lines[CLIENT_COUNT]; -static void empty_line(uint8_t client) -{ +static void empty_line(uint8_t client) { client_line_t* cl = &client_lines[client]; cl->len = 0; cl->buffer[0] = '\0'; } static void empty_lines() { - for (uint8_t client = 0; client < CLIENT_COUNT; client++) { + for (uint8_t client = 0; client < CLIENT_COUNT; client++) empty_line(client); - } } - -err_t add_char_to_line(char c, uint8_t client) -{ +err_t add_char_to_line(char c, uint8_t client) { client_line_t* cl = &client_lines[client]; // Simple editing for interactive input if (c == '\b') { @@ -64,9 +60,8 @@ err_t add_char_to_line(char c, uint8_t client) } return STATUS_OK; } - if (cl->len == (LINE_BUFFER_SIZE - 1)) { + if (cl->len == (LINE_BUFFER_SIZE - 1)) return STATUS_OVERFLOW; - } if (c == '\r' || c == '\n') { cl->len = 0; cl->line_number++; @@ -77,21 +72,17 @@ err_t add_char_to_line(char c, uint8_t client) return STATUS_OK; } -err_t execute_line(char* line, uint8_t client, auth_t auth_level) -{ +err_t execute_line(char* line, uint8_t client, auth_t auth_level) { err_t result = STATUS_OK; // Empty or comment line. For syncing purposes. - if (line[0] == 0) { + if (line[0] == 0) return STATUS_OK; - } // Grbl '$' or WebUI '[ESPxxx]' system command - if (line[0] == '$' || line[0] == '[') { + if (line[0] == '$' || line[0] == '[') return system_execute_line(line, client, auth_level); - } // Everything else is gcode. Block if in alarm or jog mode. - if (sys.state & (STATE_ALARM | STATE_JOG)) { + if (sys.state & (STATE_ALARM | STATE_JOG)) return STATUS_SYSTEM_GC_LOCK; - } return gc_execute_line(line, client); } @@ -99,7 +90,7 @@ err_t execute_line(char* line, uint8_t client, auth_t auth_level) GRBL PRIMARY LOOP: */ void protocol_main_loop() { - empty_client_buffers(); + serial_reset_read_buffer(CLIENT_ALL); empty_lines(); //uint8_t client = CLIENT_SERIAL; // default client // Perform some machine checks to make sure everything is good to go. @@ -156,25 +147,25 @@ void protocol_main_loop() { while ((c = serial_read(client)) != SERIAL_NO_DATA) { err_t res = add_char_to_line(c, client); switch (res) { - case STATUS_OK: - break; - case STATUS_EOL: - protocol_execute_realtime(); // Runtime command check point. - if (sys.abort) { - return; // Bail to calling function upon system abort - } - line = client_lines[client].buffer; + case STATUS_OK: + break; + case STATUS_EOL: + protocol_execute_realtime(); // Runtime command check point. + if (sys.abort) { + return; // Bail to calling function upon system abort + } + line = client_lines[client].buffer; #ifdef REPORT_ECHO_RAW_LINE_RECEIVED - report_echo_line_received(line, client); + report_echo_line_received(line, client); #endif - // auth_level can be upgraded by supplying a password on the command line - report_status_message(execute_line(line, client, LEVEL_GUEST), client); - empty_line(client); - break; - case STATUS_OVERFLOW: - report_status_message(STATUS_OVERFLOW, client); - empty_line(client); - break; + // auth_level can be upgraded by supplying a password on the command line + report_status_message(execute_line(line, client, LEVEL_GUEST), client); + empty_line(client); + break; + case STATUS_OVERFLOW: + report_status_message(STATUS_OVERFLOW, client); + empty_line(client); + break; } } // while serial read } // for clients @@ -188,9 +179,8 @@ void protocol_main_loop() { } // check to see if we should disable the stepper drivers ... esp32 work around for disable in main loop. if (stepper_idle) { - if (esp_timer_get_time() > stepper_idle_counter) { + if (esp_timer_get_time() > stepper_idle_counter) motors_set_disable(true); - } } } return; /* Never reached */ @@ -456,6 +446,9 @@ void protocol_exec_rt_system() { bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); sys.spindle_speed_ovr = last_s_override; sys.report_ovr_counter = 0; // Set to report change immediately + // If spinlde is on, tell it the rpm has been overridden + if (gc_state.modal.spindle != SPINDLE_DISABLE) + spindle->set_rpm(gc_state.spindle_speed); } if (rt_exec & EXEC_SPINDLE_OVR_STOP) { // Spindle stop override allowed only while in HOLD state. @@ -564,13 +557,13 @@ static void protocol_exec_rt_suspend() { // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (homing_enable->get() && - (parking_target[PARKING_AXIS] < PARKING_TARGET) && - laser_mode->get() && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + (parking_target[PARKING_AXIS] < PARKING_TARGET) && + !laser_mode->get() && + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else - if (homing_enable->get() && + if (homing_enable->get() && (parking_target[PARKING_AXIS] < PARKING_TARGET) && - laser_mode->get()) { + !laser_mode->get()) { #endif // Retract spindle by pullout distance. Ensure retraction motion moves away from // the workpiece and waypoint motion doesn't exceed the parking target location. @@ -584,9 +577,9 @@ static void protocol_exec_rt_suspend() { // NOTE: Clear accessory state after retract and after an aborted restore motion. pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION | PL_COND_FLAG_NO_FEED_OVERRIDE); pl_data->spindle_speed = 0.0; - spindle->set_state((SPINDLE_DISABLE, 0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - // Execute fast parking retract motion to parking target location. + spindle->set_state(SPINDLE_DISABLE, 0); // De-energize + coolant_set_state(COOLANT_DISABLE); // De-energize + // Execute fast parking retract motion to parking target location. if (parking_target[PARKING_AXIS] < PARKING_TARGET) { parking_target[PARKING_AXIS] = PARKING_TARGET; pl_data->feed_rate = PARKING_RATE; @@ -595,9 +588,9 @@ static void protocol_exec_rt_suspend() { } else { // Parking motion not possible. Just disable the spindle and coolant. // NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately. - ->set_state((SPINDLE_DISABLE, 0.0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - } + spindle->set_state(SPINDLE_DISABLE, 0); // De-energize + coolant_set_state(COOLANT_DISABLE); // De-energize + } #endif sys.suspend &= ~(SUSPEND_RESTART_RETRACT); sys.suspend |= SUSPEND_RETRACT_COMPLETE; @@ -624,7 +617,7 @@ static void protocol_exec_rt_suspend() { // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (homing_enable->get() && !laser_mode->get()) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else if (homing_enable->get() && !laser_mode->get()) { #endif @@ -638,8 +631,8 @@ static void protocol_exec_rt_suspend() { #endif // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. if (gc_state.modal.spindle != SPINDLE_DISABLE) { - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + // Block if safety door re-opened during prior restore actions. + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { if (laser_mode->get()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); @@ -650,8 +643,8 @@ static void protocol_exec_rt_suspend() { } } if (gc_state.modal.coolant != COOLANT_DISABLE) { - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + // Block if safety door re-opened during prior restore actions. + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. coolant_set_state((restore_condition & (PL_COND_FLAG_COOLANT_FLOOD | PL_COND_FLAG_COOLANT_FLOOD))); delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); @@ -661,7 +654,7 @@ static void protocol_exec_rt_suspend() { // Execute slow plunge motion from pull-out position to resume position. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (homing_enable->get() && !laser_mode->get()) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { #else if (homing_enable->get() && !laser_mode->get()) { #endif @@ -678,8 +671,8 @@ static void protocol_exec_rt_suspend() { } #endif if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { - sys.suspend |= SUSPEND_RESTORE_COMPLETE; - system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. + sys.suspend |= SUSPEND_RESTORE_COMPLETE; + system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. } } } diff --git a/Grbl_Esp32/protocol.h b/Grbl_Esp32/protocol.h index 941565064..92e067d6f 100644 --- a/Grbl_Esp32/protocol.h +++ b/Grbl_Esp32/protocol.h @@ -32,7 +32,7 @@ // memory space we can invest into here or we re-write the g-code parser not to have this // buffer. #ifndef LINE_BUFFER_SIZE - #define LINE_BUFFER_SIZE 80 + #define LINE_BUFFER_SIZE 256 #endif // Starts Grbl main loop. It handles all incoming characters from the serial port and executes diff --git a/Grbl_Esp32/serial.cpp b/Grbl_Esp32/serial.cpp index d2c3bb688..c8d72aa76 100644 --- a/Grbl_Esp32/serial.cpp +++ b/Grbl_Esp32/serial.cpp @@ -63,12 +63,6 @@ static TaskHandle_t serialCheckTaskHandle = 0; InputBuffer client_buffer[CLIENT_COUNT]; // create a buffer for each client -void empty_client_buffers() { - for (uint8_t i = 0; i < CLIENT_COUNT; i++) { - client_buffer[i].end(); - } -} - // Returns the number of bytes available in a client buffer. uint8_t serial_get_rx_buffer_available(uint8_t client) { return client_buffer[client].availableforwrite(); @@ -76,6 +70,7 @@ uint8_t serial_get_rx_buffer_available(uint8_t client) { void serial_init() { Serial.begin(BAUD_RATE); + Serial.setRxBufferSize(256); // reset all buffers serial_reset_read_buffer(CLIENT_ALL); grbl_send(CLIENT_SERIAL, "\r\n"); // create some white space after ESP32 boot info @@ -160,7 +155,7 @@ void serialCheckTask(void* pvParameters) { void serial_reset_read_buffer(uint8_t client) { for (uint8_t client_num = 0; client_num < CLIENT_COUNT; client_num++) { if (client == client_num || client == CLIENT_ALL) - client_buffer[client].begin(); + client_buffer[client_num].begin(); } } diff --git a/Grbl_Esp32/serial.h b/Grbl_Esp32/serial.h index 02b7e25f6..e3965484e 100644 --- a/Grbl_Esp32/serial.h +++ b/Grbl_Esp32/serial.h @@ -24,7 +24,7 @@ #include "grbl.h" #ifndef RX_BUFFER_SIZE - #define RX_BUFFER_SIZE 128 + #define RX_BUFFER_SIZE 256 #endif #ifndef TX_BUFFER_SIZE #ifdef USE_LINE_NUMBERS @@ -56,6 +56,4 @@ void execute_realtime_command(uint8_t command, uint8_t client); bool any_client_has_data(); bool is_realtime_command(uint8_t data); -void empty_client_buffers(); - #endif diff --git a/Grbl_Esp32/stepper.cpp b/Grbl_Esp32/stepper.cpp index ad8e54cd8..e7373f1bc 100644 --- a/Grbl_Esp32/stepper.cpp +++ b/Grbl_Esp32/stepper.cpp @@ -99,7 +99,11 @@ static uint8_t segment_next_head; // Step and direction port invert masks. static uint8_t step_port_invert_mask; static uint8_t dir_port_invert_mask; - +///Torch Height Control Step and Dir ints +#ifdef torchHeightControl + extern int thcZStep; + extern int thcDirDown; +#endif // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. static volatile uint8_t busy; @@ -235,6 +239,15 @@ void IRAM_ATTR onStepperDriverTimer(void* para) { // ISR It is time to take a st * is to keep pulse timing as regular as possible. */ static void stepper_pulse_func() { + +#ifdef torchHeightControl + if(thcZStep > 0) //If the torch height control step count requested is greater than 0 + { + st.dir_outbits ^= (-thcDirDown ^ st.dir_outbits) & (1UL << Z_AXIS); //Set the Z Axis Direction Bit to what THC wants + thcZStep --; //subtract out one of the steps requested by torchHeightControl.cpp + st.step_outbits |= bit(Z_AXIS); //Add a step for the Z Axis + } +#endif motors_set_direction_pins(st.dir_outbits); #ifdef USE_RMT_STEPS stepperRMT_Outputs(); @@ -1186,7 +1199,7 @@ void IRAM_ATTR Stepper_Timer_WritePeriod(uint64_t alarm_val) { #ifdef USE_I2S_OUT_STREAM // 1 tick = F_TIMERS / F_STEPPER_TIMER // Pulse ISR is called for each tick of alarm_val. - i2s_out_set_pulse_period(alarm_val / 60); + i2s_out_set_pulse_period(alarm_val); #else timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, alarm_val); #endif diff --git a/VisualStudio.md b/VisualStudio.md new file mode 100644 index 000000000..f79aef284 --- /dev/null +++ b/VisualStudio.md @@ -0,0 +1,26 @@ +# Getting started with GRBL_ESP32 in Visual Studio + +**!! Important !! There's a huge difference between Visual Studio and +Visual Studio Code. This document is about Visual Studio, not Visual +Studio Code!** + +First, get PlatformIO to work with Visual studio. The steps that +need to be taken for this are the following: + +1. Install python. This is needed for both PlatformIO and for generating + the vcxproj file. +2. From https://docs.platformio.org/en/latest/core/index.html#piocore + you should install the PlatformIO Core (CLI). Make sure you update + the command line search path. +3. Use python to generate a vcxproj file: `python generate_vcxproj.py`. +4. Start Grbl_Esp32.sln + +## Building + +Building is as easy as building your solution. + +## Uploading + +Uploading can be done from the command line using platformio. For +example, run `platformio run --target upload --upload-port COM7`. +For more details, see [the documentation of pio](https://dokk.org/documentation/platformio/v3.6.1/platforms/espressif32/). diff --git a/generate_vcxproj.py b/generate_vcxproj.py new file mode 100644 index 000000000..36b77907d --- /dev/null +++ b/generate_vcxproj.py @@ -0,0 +1,275 @@ +''' + Visual studio project file generator + Grbl_ESP32 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_ESP32. If not, see . + @authors: atlaste [github.com/atlaste] +''' + +PATHS_TO_SEARCH = ['Grbl_Esp32'] +HEADER_EXT = ['.h', '.inl'] +SOURCE_EXT = ['.c', '.cpp'] +OTHER_EXT = ['.ino', '.md'] + +import os, uuid + +def UUID(name): + return str(uuid.uuid3(uuid.NAMESPACE_OID, name)).upper() + +def FilterFromPath(path): + (head, tail) = os.path.split(path) + head = head.replace('/', '\\').replace('..\\', '').replace('.\\', '') + if head == '.': + return '' + + h = head[0:10]; + if h == 'Grbl_Esp32': + h = head[11:] + return h + +class Vcxproj: + # configuration, platform + ConfigurationFmt = '\n'.join([ + ' ', + ' {0}', + ' {1}', + ' ']) + # item name, item file + ItemFmt = '\n'.join([ + ' <{0} Include="{1}" />']) + + # configuration, platform + ConfigTypePropertyGroupFmt = '\n'.join([ + ' ', + ' Makefile', + ' true', + ' v142', + ' ']) + + # configuration, platform + ImportGroupFmt = '\n'.join([ + ' ', + ' ', + ' ' + ]) + + # configuration, platform + PIOPropertyGroupFmt = '\n'.join([ + ' ', + ' platformio --force run', + ' platformio --force run -t clean', + ' WIN32;_DEBUG;$(NMakePreprocessorDefinitions)', + ' $(HOMEDRIVE)$(HOMEPATH)\\.platformio\\packages\\toolchain-xtensa32\\xtensa-esp32-elf\\include;$(HOMEDRIVE)$(HOMEPATH)\\.platformio\\packages\\framework-arduinoespressif32\\cores\\esp32;$(NMakeIncludeSearchPath)', + ' ' + ]) + + @staticmethod + def Configuration(configuration, platform): + return Vcxproj.ConfigurationFmt.format(configuration, platform) + + @staticmethod + def Item(name, file): + return Vcxproj.ItemFmt.format(name, file) + + @staticmethod + def ConfigTypePropertyGroup(configuration, platform): + return Vcxproj.ConfigTypePropertyGroupFmt.format(configuration, platform) + + @staticmethod + def ImportGroup(configuration, platform): + return Vcxproj.ImportGroupFmt.format(configuration, platform) + + @staticmethod + def PIOPropertyGroup(configuration, platform): + return Vcxproj.PIOPropertyGroupFmt.format(configuration, platform) + +class Filters: + # itemtype, path, folder + ItemFmt = '\n'.join([ + ' <{0} Include="{1}">', + ' {2}', + ' ']) + + # folder, uuid + FilterFmt = '\n'.join([ + ' ', + ' {{{1}}}', + ' ']) + + @staticmethod + def Item(itemtype, path): + folder = FilterFromPath(path) + return Filters.ItemFmt.format(itemtype, path, folder) + + @staticmethod + def Filter(folder): + uid = UUID(folder) + return Filters.FilterFmt.format(folder, uid) + +class Generator: + Folders = set() + + # Files + Headers = set() + Sources = set() + Others = set() + + # Stuffs + Platforms = set(['Win32','x64']) + Configurations = set(['Debug','Release']) + Name = 'Grbl_Esp32' + + def AddFolder(self, path): + filt = FilterFromPath(path) + if filt == '': + return + if filt not in self.Folders: + self.Folders.add(filt) + filters = '' + for f in os.path.split(filt): + filters = os.path.join(filters, f) + if filters != '': + self.Folders.add(filters) + + def AddFile(self, path): + (root, ext) = os.path.splitext(path) + if ext in HEADER_EXT: + self.Headers.add(path) + elif ext in SOURCE_EXT: + self.Sources.add(path) + elif ext in OTHER_EXT: + self.Others.add(path) + else: + return + + self.AddFolder(path) + + def Walk(self, path): + if path == 'Grbl_Esp32\\Custom' or path == 'Grbl_Esp32/Custom': + return + if os.path.isfile(path): + self.AddFile(path) + else: + for subPath in os.listdir(path): + self.Walk(os.path.join(path, subPath)) + + def CreateProject(self): + project = [] + project.append('') + project.append('') + + project.append('') + for p in self.Platforms: + for c in self.Configurations: + project.append(Vcxproj.Configuration(c, p)) + project.append('') + + # cpp header files + project.append('') + for f in self.Headers: + project.append(Vcxproj.Item('ClInclude', f)) + project.append('') + + # cpp source files + project.append('') + for f in self.Sources: + project.append(Vcxproj.Item('ClCompile', f)) + project.append('') + + # md files and ino file + project.append('') + for f in self.Others: + project.append(Vcxproj.Item('None', f)) + project.append('') + + # Bookkeeping, compilation, etc. + project.append('') + project.append(' 16.0') + project.append(' {11C8A44F-A303-4885-B5AD-5B65F7FE41C0}') + project.append(' Win32Proj') + project.append('') + + project.append('') + for p in self.Platforms: + for c in self.Configurations: + project.append(Vcxproj.ConfigTypePropertyGroup(c, p)) + + project.append('') + project.append('') + project.append('') + project.append(' ') + project.append('') + + for p in self.Platforms: + for c in self.Configurations: + project.append(Vcxproj.ImportGroup(c, p)) + project.append('') + + for p in self.Platforms: + for c in self.Configurations: + project.append(Vcxproj.PIOPropertyGroup(c, p)) + + project.append('') + project.append('') + project.append('') + project.append(' ') + project.append('') + project.append('') + project.append(' ') + project.append(' ') + project.append(' ') + project.append(' ') + project.append('') + return '\n'.join(project) + + def CreateFilters(self): + project = [] + project.append('') + project.append('') + + project.append('') + for f in self.Folders: + project.append(Filters.Filter(f)) + project.append('') + + project.append('') + for f in self.Headers: + project.append(Filters.Item('ClInclude', f)) + project.append('') + + project.append('') + for f in self.Sources: + project.append(Filters.Item('ClCompile', f)) + project.append('') + + project.append('') + for f in self.Others: + project.append(Filters.Item('None', f)) + project.append('') + + project.append('') + return '\n'.join(project) + + def Generate(self): + f = open(self.Name + '.vcxproj', 'w') + f.write(self.CreateProject()) + f.close() + + f = open(self.Name + '.vcxproj.filters', 'w') + f.write(self.CreateFilters()) + f.close() + +def main(paths): + generator = Generator() + for path in paths: + generator.Walk(path) + generator.Generate() + +main(PATHS_TO_SEARCH) diff --git a/platformio.ini b/platformio.ini index 326c7073c..3d9f0c93e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -32,11 +32,15 @@ build_flags = [env] lib_deps = TMCStepper@>=0.7.0 - arduinoWebSockets - ESP32SSPD -platform = espressif32 +; arduinoWebSockets +; ESP32SSPD +;platform = espressif32@3.3.5 +;platform = espressif32 +platform = espressif32@3.0.0 ; temporary fix for lost uart rx characters board = esp32dev framework = arduino +;platform_packages = +; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git#1.0.6 upload_speed = 921600 board_build.partitions = min_spiffs.csv monitor_speed = 115200