mirror of
https://github.com/bdring/Grbl_Esp32.git
synced 2025-08-12 09:34:45 +02:00
Merge remote-tracking branch 'upstream/Devt' into i2s_io_expander
This commit is contained in:
@@ -21,6 +21,8 @@
|
|||||||
#include "grbl.h"
|
#include "grbl.h"
|
||||||
#include "WiFi.h"
|
#include "WiFi.h"
|
||||||
|
|
||||||
|
#include "tools/SpindleClass.cpp"
|
||||||
|
|
||||||
// Declare system global variable structure
|
// Declare system global variable structure
|
||||||
system_t sys;
|
system_t sys;
|
||||||
int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps.
|
int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps.
|
||||||
@@ -34,6 +36,8 @@ volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bit
|
|||||||
volatile uint8_t sys_rt_exec_debug;
|
volatile uint8_t sys_rt_exec_debug;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
Spindle *spindle;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
@@ -52,7 +56,7 @@ void setup() {
|
|||||||
#else
|
#else
|
||||||
#define MACHINE_STRING MACHINE_NAME
|
#define MACHINE_STRING MACHINE_NAME
|
||||||
#endif
|
#endif
|
||||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Using machine:%s", MACHINE_STRING);
|
report_machine_type(CLIENT_SERIAL);
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_I2S_IOEXPANDER
|
#ifdef USE_I2S_IOEXPANDER
|
||||||
// The I2S I/O expander must be initialized before it can access the enhanced GPIO port
|
// The I2S I/O expander must be initialized before it can access the enhanced GPIO port
|
||||||
@@ -124,7 +128,7 @@ void loop() {
|
|||||||
// Reset Grbl primary systems.
|
// Reset Grbl primary systems.
|
||||||
serial_reset_read_buffer(CLIENT_ALL); // Clear serial read buffer
|
serial_reset_read_buffer(CLIENT_ALL); // Clear serial read buffer
|
||||||
gc_init(); // Set g-code parser to default state
|
gc_init(); // Set g-code parser to default state
|
||||||
spindle_init();
|
spindle_select(SPINDLE_TYPE);
|
||||||
coolant_init();
|
coolant_init();
|
||||||
limits_init();
|
limits_init();
|
||||||
probe_init();
|
probe_init();
|
||||||
|
@@ -23,7 +23,7 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_ESP32_V3.5"
|
#define MACHINE_NAME "ESP32_V3.5"
|
||||||
|
|
||||||
#define X_STEP_PIN GPIO_NUM_12
|
#define X_STEP_PIN GPIO_NUM_12
|
||||||
#define X_DIRECTION_PIN GPIO_NUM_26
|
#define X_DIRECTION_PIN GPIO_NUM_26
|
||||||
@@ -40,8 +40,11 @@
|
|||||||
// OK to comment out to use pin for other features
|
// OK to comment out to use pin for other features
|
||||||
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
|
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_17 // labeled SpinPWM
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_17 // labeled SpinPWM
|
||||||
|
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
|
||||||
|
|
||||||
#define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist
|
#define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist
|
||||||
#define COOLANT_FLOOD_PIN GPIO_NUM_16 // labeled Flood
|
#define COOLANT_FLOOD_PIN GPIO_NUM_16 // labeled Flood
|
||||||
#define PROBE_PIN GPIO_NUM_32 // labeled Probe
|
#define PROBE_PIN GPIO_NUM_32 // labeled Probe
|
||||||
|
@@ -23,7 +23,7 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_ESP32_V4"
|
#define MACHINE_NAME "ESP32_V4"
|
||||||
|
|
||||||
#define X_STEP_PIN GPIO_NUM_12
|
#define X_STEP_PIN GPIO_NUM_12
|
||||||
#define X_DIRECTION_PIN GPIO_NUM_14
|
#define X_DIRECTION_PIN GPIO_NUM_14
|
||||||
@@ -40,8 +40,10 @@
|
|||||||
// OK to comment out to use pin for other features
|
// OK to comment out to use pin for other features
|
||||||
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
|
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_2 // labeled SpinPWM
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2 // labeled SpinPWM
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
|
||||||
|
|
||||||
#define MIST_PIN GPIO_NUM_21 // labeled Mist
|
#define MIST_PIN GPIO_NUM_21 // labeled Mist
|
||||||
#define FLOOD_PIN GPIO_NUM_25 // labeled Flood
|
#define FLOOD_PIN GPIO_NUM_25 // labeled Flood
|
||||||
#define PROBE_PIN GPIO_NUM_32 // labeled Probe
|
#define PROBE_PIN GPIO_NUM_32 // labeled Probe
|
||||||
|
@@ -24,7 +24,7 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_ESP32_V4_XYX"
|
#define MACHINE_NAME "ESP32_V4_XYX"
|
||||||
#define X_STEP_PIN GPIO_NUM_26 /* labeled Y */
|
#define X_STEP_PIN GPIO_NUM_26 /* labeled Y */
|
||||||
#define X_DIRECTION_PIN GPIO_NUM_15 /* labeled Y */
|
#define X_DIRECTION_PIN GPIO_NUM_15 /* labeled Y */
|
||||||
#define Y_STEP_PIN GPIO_NUM_12 /* labeled X */
|
#define Y_STEP_PIN GPIO_NUM_12 /* labeled X */
|
||||||
@@ -32,7 +32,9 @@
|
|||||||
#define Y2_STEP_PIN GPIO_NUM_27 /* labeled Z */
|
#define Y2_STEP_PIN GPIO_NUM_27 /* labeled Z */
|
||||||
#define Y2_DIRECTION_PIN GPIO_NUM_33 /* labeled Z */
|
#define Y2_DIRECTION_PIN GPIO_NUM_33 /* labeled Z */
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_2
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2
|
||||||
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_22
|
||||||
|
|
||||||
#define LIMIT_MASK B11
|
#define LIMIT_MASK B11
|
||||||
#define X_LIMIT_PIN GPIO_NUM_17
|
#define X_LIMIT_PIN GPIO_NUM_17
|
||||||
@@ -44,7 +46,7 @@
|
|||||||
#define COOLANT_MIST_PIN GPIO_NUM_21
|
#define COOLANT_MIST_PIN GPIO_NUM_21
|
||||||
#define COOLANT_FLOOD_PIN GPIO_NUM_25
|
#define COOLANT_FLOOD_PIN GPIO_NUM_25
|
||||||
|
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_22
|
|
||||||
|
|
||||||
// see versions for X and Z
|
// see versions for X and Z
|
||||||
#define PROBE_PIN GPIO_NUM_32
|
#define PROBE_PIN GPIO_NUM_32
|
||||||
|
@@ -39,10 +39,14 @@
|
|||||||
#define A_DIRECTION_PIN GPIO_NUM_12
|
#define A_DIRECTION_PIN GPIO_NUM_12
|
||||||
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
|
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
|
||||||
|
|
||||||
|
/*
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_25
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_25
|
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_22
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_22
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG
|
||||||
#define MODBUS_TX GPIO_NUM_17
|
#define MODBUS_TX GPIO_NUM_17
|
||||||
#define MODBUS_RX GPIO_NUM_4
|
#define MODBUS_RX GPIO_NUM_4
|
||||||
#define MODBUS_CTRL GPIO_NUM_16
|
#define MODBUS_CTRL GPIO_NUM_16
|
||||||
|
@@ -1,61 +0,0 @@
|
|||||||
/*
|
|
||||||
add_esc_spindle.h
|
|
||||||
Part of Grbl_ESP32
|
|
||||||
|
|
||||||
This is an additional configuration fragment that can be
|
|
||||||
included after a base configuration file. The base file
|
|
||||||
establishes most settings and the add-on changes a few things.
|
|
||||||
For example, in machines.h, you would write:
|
|
||||||
#include "Machines/3axis_v4.h" // Basic pin assignments
|
|
||||||
#include "Machines/add_esc_spindle.h" // Add-ons for ESC spindle
|
|
||||||
|
|
||||||
This uses a Brushless DC Hobby motor as a spindle motor. See:
|
|
||||||
https://github.com/bdring/Grbl_Esp32/wiki/BESC-Spindle-Feature
|
|
||||||
|
|
||||||
2019 - Bart Dring
|
|
||||||
2020 - Mitch Bradley
|
|
||||||
|
|
||||||
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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
|
|
||||||
// MACHINE_EXTRA is appended to MACHINE_NAME for startup message display
|
|
||||||
#define MACHINE_EXTRA "_ESC_SPINDLE"
|
|
||||||
|
|
||||||
#define SHOW_EXTENDED_SETTINGS
|
|
||||||
|
|
||||||
#define SPINDLE_PWM_BIT_PRECISION 16 // 16 bit recommended for ESC (don't change)
|
|
||||||
|
|
||||||
/*
|
|
||||||
Important ESC Settings
|
|
||||||
$33=50 // Hz this is the typical good frequency for an ESC
|
|
||||||
#define DEFAULT_SPINDLE_FREQ 5000.0 // $33 Hz (extended set)
|
|
||||||
|
|
||||||
Determine the typical min and max pulse length of your ESC
|
|
||||||
min_pulse is typically 1ms (0.001 sec) or less
|
|
||||||
max_pulse is typically 2ms (0.002 sec) or more
|
|
||||||
|
|
||||||
determine PWM_period. It is (1/freq) if freq = 50...period = 0.02
|
|
||||||
|
|
||||||
determine pulse length for min_pulse and max_pulse in percent.
|
|
||||||
|
|
||||||
(pulse / PWM_period)
|
|
||||||
min_pulse = (0.001 / 0.02) = 0.05 = 5% so ... $34 and $35 = 5.0
|
|
||||||
max_pulse = (0.002 / .02) = 0.1 = 10% so ... $36=10
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define DEFAULT_SPINDLE_FREQ 50.0
|
|
||||||
#define DEFAULT_SPINDLE_OFF_VALUE 5.0
|
|
||||||
#define DEFAULT_SPINDLE_MIN_VALUE 5.0
|
|
||||||
#define DEFAULT_SPINDLE_MAX_VALUE 10.0
|
|
@@ -1 +0,0 @@
|
|||||||
// See template.h
|
|
@@ -25,7 +25,7 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_ESPDUINO_32"
|
#define MACHINE_NAME "ESPDUINO_32"
|
||||||
|
|
||||||
#define X_STEP_PIN GPIO_NUM_26
|
#define X_STEP_PIN GPIO_NUM_26
|
||||||
#define X_DIRECTION_PIN GPIO_NUM_16
|
#define X_DIRECTION_PIN GPIO_NUM_16
|
||||||
@@ -39,7 +39,9 @@
|
|||||||
// OK to comment out to use pin for other features
|
// OK to comment out to use pin for other features
|
||||||
#define STEPPERS_DISABLE_PIN GPIO_NUM_12
|
#define STEPPERS_DISABLE_PIN GPIO_NUM_12
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_19
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_19
|
||||||
|
|
||||||
#define SPINDLE_DIR_PIN GPIO_NUM_18
|
#define SPINDLE_DIR_PIN GPIO_NUM_18
|
||||||
|
|
||||||
#define COOLANT_FLOOD_PIN GPIO_NUM_34
|
#define COOLANT_FLOOD_PIN GPIO_NUM_34
|
||||||
|
@@ -22,13 +22,15 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_FOO_6X"
|
#define MACHINE_NAME "FOO_6X"
|
||||||
|
|
||||||
#ifdef N_AXIS
|
#ifdef N_AXIS
|
||||||
#undef N_AXIS
|
#undef N_AXIS
|
||||||
#endif
|
#endif
|
||||||
#define N_AXIS 6
|
#define N_AXIS 6
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
// stepper motors
|
// stepper motors
|
||||||
#define X_STEP_PIN GPIO_NUM_12
|
#define X_STEP_PIN GPIO_NUM_12
|
||||||
#define X_DIRECTION_PIN GPIO_NUM_26
|
#define X_DIRECTION_PIN GPIO_NUM_26
|
||||||
@@ -122,11 +124,6 @@
|
|||||||
#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
|
#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
|
||||||
#define DEFAULT_HOMING_PULLOFF 3.0 // mm
|
#define DEFAULT_HOMING_PULLOFF 3.0 // mm
|
||||||
|
|
||||||
#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm
|
|
||||||
#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm
|
|
||||||
|
|
||||||
#define DEFAULT_LASER_MODE 0 // false
|
|
||||||
|
|
||||||
#define DEFAULT_X_STEPS_PER_MM 400.0
|
#define DEFAULT_X_STEPS_PER_MM 400.0
|
||||||
#define DEFAULT_Y_STEPS_PER_MM 400.0
|
#define DEFAULT_Y_STEPS_PER_MM 400.0
|
||||||
#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
|
#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
|
||||||
|
@@ -50,9 +50,11 @@
|
|||||||
//#define USE_SPINDLE_RELAY
|
//#define USE_SPINDLE_RELAY
|
||||||
|
|
||||||
#ifdef USE_SPINDLE_RELAY
|
#ifdef USE_SPINDLE_RELAY
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_2
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2
|
||||||
#else
|
#else
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_16
|
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_16
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@@ -22,7 +22,9 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_MIDTBOT"
|
#define MACHINE_NAME "MIDTBOT"
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
#define X_STEP_PIN GPIO_NUM_12
|
#define X_STEP_PIN GPIO_NUM_12
|
||||||
#define Y_STEP_PIN GPIO_NUM_14
|
#define Y_STEP_PIN GPIO_NUM_14
|
||||||
@@ -72,6 +74,8 @@
|
|||||||
|
|
||||||
#define SERVO_PEN_PIN GPIO_NUM_27
|
#define SERVO_PEN_PIN GPIO_NUM_27
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
// defaults
|
// defaults
|
||||||
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
||||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on
|
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on
|
||||||
@@ -98,11 +102,6 @@
|
|||||||
#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
|
#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
|
||||||
#define DEFAULT_HOMING_PULLOFF 3.0 // mm
|
#define DEFAULT_HOMING_PULLOFF 3.0 // mm
|
||||||
|
|
||||||
#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm
|
|
||||||
#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm
|
|
||||||
|
|
||||||
#define DEFAULT_LASER_MODE 0 // false
|
|
||||||
|
|
||||||
#define DEFAULT_X_STEPS_PER_MM 200.0
|
#define DEFAULT_X_STEPS_PER_MM 200.0
|
||||||
#define DEFAULT_Y_STEPS_PER_MM 100.0
|
#define DEFAULT_Y_STEPS_PER_MM 100.0
|
||||||
#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
|
#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
|
||||||
|
@@ -25,7 +25,7 @@
|
|||||||
// // Pin assignments for the Buildlog.net MPCNC controller
|
// // Pin assignments for the Buildlog.net MPCNC controller
|
||||||
|
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_MPCNC_V1P1"
|
#define MACHINE_NAME "MPCNC_V1P1"
|
||||||
|
|
||||||
#define USE_GANGED_AXES // allow two motors on an axis
|
#define USE_GANGED_AXES // allow two motors on an axis
|
||||||
|
|
||||||
@@ -50,9 +50,12 @@
|
|||||||
//#define USE_SPINDLE_RELAY
|
//#define USE_SPINDLE_RELAY
|
||||||
|
|
||||||
#ifdef USE_SPINDLE_RELAY
|
#ifdef USE_SPINDLE_RELAY
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_17
|
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_17
|
||||||
#else
|
#else
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_16
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_16
|
||||||
|
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@@ -26,7 +26,7 @@
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_MPCNC_V1P2"
|
#define MACHINE_NAME "MPCNC_V1P2"
|
||||||
|
|
||||||
#define USE_GANGED_AXES // allow two motors on an axis
|
#define USE_GANGED_AXES // allow two motors on an axis
|
||||||
|
|
||||||
@@ -51,9 +51,12 @@
|
|||||||
//#define USE_SPINDLE_RELAY
|
//#define USE_SPINDLE_RELAY
|
||||||
|
|
||||||
#ifdef USE_SPINDLE_RELAY
|
#ifdef USE_SPINDLE_RELAY
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_2
|
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2
|
||||||
#else
|
#else
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_16
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_16
|
||||||
|
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@@ -26,7 +26,7 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_PEN_LASER"
|
#define MACHINE_NAME "PEN_LASER"
|
||||||
|
|
||||||
// Pick a board version
|
// Pick a board version
|
||||||
//#define PEN_LASER_V1
|
//#define PEN_LASER_V1
|
||||||
@@ -49,10 +49,6 @@
|
|||||||
#define Y_LIMIT_PIN GPIO_NUM_4
|
#define Y_LIMIT_PIN GPIO_NUM_4
|
||||||
#define LIMIT_MASK B11
|
#define LIMIT_MASK B11
|
||||||
|
|
||||||
// If SPINDLE_PWM_PIN is commented out, this frees up the pin, but Grbl will still
|
|
||||||
// use a virtual spindle. Do not comment out the other parameters for the spindle.
|
|
||||||
//#define SPINDLE_PWM_PIN GPIO_NUM_17 // Laser PWM
|
|
||||||
|
|
||||||
#define USING_SERVO // uncomment to use this feature
|
#define USING_SERVO // uncomment to use this feature
|
||||||
//#define USING_SOLENOID // uncomment to use this feature
|
//#define USING_SOLENOID // uncomment to use this feature
|
||||||
|
|
||||||
@@ -68,6 +64,8 @@
|
|||||||
#define SOLENOID_PEN_PIN GPIO_NUM_16
|
#define SOLENOID_PEN_PIN GPIO_NUM_16
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
// defaults
|
// defaults
|
||||||
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
||||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // stay on
|
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // stay on
|
||||||
|
@@ -22,12 +22,14 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_POLAR_COASTER"
|
#define MACHINE_NAME "POLAR_COASTER"
|
||||||
|
|
||||||
// This causes the custom code file to be included in the build
|
// This causes the custom code file to be included in the build
|
||||||
// via ../custom_code.cpp
|
// via ../custom_code.cpp
|
||||||
#define CUSTOM_CODE_FILENAME "Custom/polar_coaster.cpp"
|
#define CUSTOM_CODE_FILENAME "Custom/polar_coaster.cpp"
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
#define RADIUS_AXIS 0
|
#define RADIUS_AXIS 0
|
||||||
#define POLAR_AXIS 1
|
#define POLAR_AXIS 1
|
||||||
|
|
||||||
@@ -55,6 +57,8 @@
|
|||||||
#define X_LIMIT_PIN GPIO_NUM_4
|
#define X_LIMIT_PIN GPIO_NUM_4
|
||||||
#define LIMIT_MASK B1
|
#define LIMIT_MASK B1
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
#ifdef IGNORE_CONTROL_PINS // maybe set in config.h
|
#ifdef IGNORE_CONTROL_PINS // maybe set in config.h
|
||||||
#undef IGNORE_CONTROL_PINS
|
#undef IGNORE_CONTROL_PINS
|
||||||
#endif
|
#endif
|
||||||
|
@@ -49,9 +49,9 @@
|
|||||||
#define Y_LIMIT_PIN GPIO_NUM_4
|
#define Y_LIMIT_PIN GPIO_NUM_4
|
||||||
#define LIMIT_MASK B11
|
#define LIMIT_MASK B11
|
||||||
|
|
||||||
// If SPINDLE_PWM_PIN is commented out, this frees up the pin, but Grbl will still
|
// If SPINDLE_OUTPUT_PIN is commented out, this frees up the pin, but Grbl will still
|
||||||
// use a virtual spindle. Do not comment out the other parameters for the spindle.
|
// use a virtual spindle. Do not comment out the other parameters for the spindle.
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_17 // Laser PWM
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_17 // Laser PWM
|
||||||
// PWM Generator is based on 80,000,000 Hz counter
|
// PWM Generator is based on 80,000,000 Hz counter
|
||||||
// Therefor the freq determines the resolution
|
// Therefor the freq determines the resolution
|
||||||
// 80,000,000 / freq = max resolution
|
// 80,000,000 / freq = max resolution
|
||||||
|
@@ -78,7 +78,8 @@
|
|||||||
// Turn on with M7 and off with M9
|
// Turn on with M7 and off with M9
|
||||||
#define COOLANT_MIST_PIN GPIO_NUM_21
|
#define COOLANT_MIST_PIN GPIO_NUM_21
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_25
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_25
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_4
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_4
|
||||||
|
|
||||||
// Relay operation
|
// Relay operation
|
||||||
|
@@ -63,7 +63,8 @@
|
|||||||
// Turn on with M7 and off with M9
|
// Turn on with M7 and off with M9
|
||||||
#define COOLANT_MIST_PIN GPIO_NUM_21
|
#define COOLANT_MIST_PIN GPIO_NUM_21
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_25
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_25
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_4
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_4
|
||||||
|
|
||||||
// Relay operation
|
// Relay operation
|
||||||
|
@@ -70,15 +70,20 @@
|
|||||||
// Turn on with M7 and off with M9
|
// Turn on with M7 and off with M9
|
||||||
#define COOLANT_MIST_PIN GPIO_NUM_21
|
#define COOLANT_MIST_PIN GPIO_NUM_21
|
||||||
|
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_25
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_25
|
||||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_4
|
#define SPINDLE_ENABLE_PIN GPIO_NUM_4
|
||||||
|
|
||||||
// Relay operation
|
// Relay operation
|
||||||
// Install Jumper near relay
|
// Install Jumper near relay
|
||||||
// For spindle Use max RPM of 1
|
// For PWM remove jumper to prevent relay damage
|
||||||
// For PWM remove jumper and set MAX RPM to something higher ($30 setting)
|
|
||||||
// Interlock jumper along top edge needs to be installed for both versions
|
// Interlock jumper along top edge needs to be installed for both versions
|
||||||
#define DEFAULT_SPINDLE_RPM_MAX 1 // Should be 1 for relay operation
|
#define USE_RELAY // comment out to use PWM
|
||||||
|
|
||||||
|
#ifdef USE_RELAY
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||||
|
#else
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#endif
|
||||||
|
|
||||||
#define PROBE_PIN GPIO_NUM_22
|
#define PROBE_PIN GPIO_NUM_22
|
||||||
|
|
||||||
|
@@ -56,7 +56,7 @@
|
|||||||
// will be shown in a Grbl startup message to identify your
|
// will be shown in a Grbl startup message to identify your
|
||||||
// configuration.
|
// configuration.
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_TEMPLATE"
|
#define MACHINE_NAME "TEMPLATE"
|
||||||
|
|
||||||
// If your machine requires custom code as described below in
|
// If your machine requires custom code as described below in
|
||||||
// Special Features, you must copy Custom/custom_code_template.cpp
|
// Special Features, you must copy Custom/custom_code_template.cpp
|
||||||
@@ -102,7 +102,7 @@
|
|||||||
// machine does not support one of these features, you can leave
|
// machine does not support one of these features, you can leave
|
||||||
// the corresponding pin undefined.
|
// the corresponding pin undefined.
|
||||||
|
|
||||||
// #define SPINDLE_PWM_PIN GPIO_NUM_2 // labeled SpinPWM
|
// #define SPINDLE_OUTPUT_PIN GPIO_NUM_2 // labeled SpinPWM
|
||||||
// #define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
|
// #define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
|
||||||
// #define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist
|
// #define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist
|
||||||
// #define COOLANT_FLOOD_PIN GPIO_NUM_25 // labeled Flood
|
// #define COOLANT_FLOOD_PIN GPIO_NUM_25 // labeled Flood
|
||||||
|
@@ -32,10 +32,12 @@
|
|||||||
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
along with Grbl_ESP32. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MACHINE_NAME "MACHINE_DEFAULT - Demo Only No I/O!"
|
#define MACHINE_NAME "Test Drive - Demo Only No I/O!"
|
||||||
|
|
||||||
#define LIMIT_MASK 0 // no limit pins
|
#define LIMIT_MASK 0 // no limit pins
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
#ifdef USE_RMT_STEPS
|
#ifdef USE_RMT_STEPS
|
||||||
#undef USE_RMT_STEPS // Suppress unused variable warning
|
#undef USE_RMT_STEPS // Suppress unused variable warning
|
||||||
#endif
|
#endif
|
||||||
|
@@ -61,6 +61,8 @@
|
|||||||
//#define USE_SPINDLE
|
//#define USE_SPINDLE
|
||||||
|
|
||||||
#ifdef USE_SERVO_AXES
|
#ifdef USE_SERVO_AXES
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||||
|
|
||||||
#define SERVO_Z_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control.
|
#define SERVO_Z_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control.
|
||||||
#define SERVO_Z_RANGE_MIN 0.0
|
#define SERVO_Z_RANGE_MIN 0.0
|
||||||
#define SERVO_Z_RANGE_MAX 5.0
|
#define SERVO_Z_RANGE_MAX 5.0
|
||||||
@@ -68,7 +70,9 @@
|
|||||||
#define SERVO_Z_HOME_POS SERVO_Z_RANGE_MAX // move to max during homing
|
#define SERVO_Z_HOME_POS SERVO_Z_RANGE_MAX // move to max during homing
|
||||||
#define SERVO_Z_MPOS false // will not use mpos, uses work coordinates
|
#define SERVO_Z_MPOS false // will not use mpos, uses work coordinates
|
||||||
#else
|
#else
|
||||||
#define SPINDLE_PWM_PIN GPIO_NUM_27
|
|
||||||
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
|
#define SPINDLE_OUTPUT_PIN GPIO_NUM_27
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// #define X_LIMIT_PIN See version section at beginning of file
|
// #define X_LIMIT_PIN See version section at beginning of file
|
||||||
|
@@ -205,7 +205,7 @@ Some features should not be changed. See notes below.
|
|||||||
#define CMD_RAPID_OVR_LOW 0x97
|
#define CMD_RAPID_OVR_LOW 0x97
|
||||||
// #define CMD_RAPID_OVR_EXTRA_LOW 0x98 // *NOT SUPPORTED*
|
// #define CMD_RAPID_OVR_EXTRA_LOW 0x98 // *NOT SUPPORTED*
|
||||||
#define CMD_SPINDLE_OVR_RESET 0x99 // Restores spindle override value to 100%.
|
#define CMD_SPINDLE_OVR_RESET 0x99 // Restores spindle override value to 100%.
|
||||||
#define CMD_SPINDLE_OVR_COARSE_PLUS 0x9A
|
#define CMD_SPINDLE_OVR_COARSE_PLUS 0x9A // 154
|
||||||
#define CMD_SPINDLE_OVR_COARSE_MINUS 0x9B
|
#define CMD_SPINDLE_OVR_COARSE_MINUS 0x9B
|
||||||
#define CMD_SPINDLE_OVR_FINE_PLUS 0x9C
|
#define CMD_SPINDLE_OVR_FINE_PLUS 0x9C
|
||||||
#define CMD_SPINDLE_OVR_FINE_MINUS 0x9D
|
#define CMD_SPINDLE_OVR_FINE_MINUS 0x9D
|
||||||
@@ -437,12 +437,6 @@ Some features should not be changed. See notes below.
|
|||||||
// tool length offset value is subtracted from the current location.
|
// tool length offset value is subtracted from the current location.
|
||||||
#define TOOL_LENGTH_OFFSET_AXIS Z_AXIS // Default z-axis. Valid values are X_AXIS, Y_AXIS, or Z_AXIS.
|
#define TOOL_LENGTH_OFFSET_AXIS Z_AXIS // Default z-axis. Valid values are X_AXIS, Y_AXIS, or Z_AXIS.
|
||||||
|
|
||||||
// Enables variable spindle output voltage for different RPM values. On the Arduino Uno, the spindle
|
|
||||||
// enable pin will output 5V for maximum RPM with 256 intermediate levels and 0V when disabled.
|
|
||||||
// NOTE: IMPORTANT for Arduino Unos! When enabled, the Z-limit pin D11 and spindle enable pin D12 switch!
|
|
||||||
// The hardware PWM output on pin D11 is required for variable spindle output voltages.
|
|
||||||
#define VARIABLE_SPINDLE // Default enabled. Comment to disable.
|
|
||||||
|
|
||||||
// Alters the behavior of the spindle enable pin. By default Grbl will not disable the enable pin if
|
// Alters the behavior of the spindle enable pin. By default Grbl will not disable the enable pin if
|
||||||
// spindle speed is zero and M3/4 is active, but still sets the PWM output to zero. This allows the users
|
// spindle speed is zero and M3/4 is active, but still sets the PWM output to zero. This allows the users
|
||||||
// to know if the spindle is active and use it as an additional control input.
|
// to know if the spindle is active and use it as an additional control input.
|
||||||
|
@@ -114,7 +114,18 @@
|
|||||||
#define DEFAULT_HOMING_PULLOFF 1.0 // $27 mm
|
#define DEFAULT_HOMING_PULLOFF 1.0 // $27 mm
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// ======== sPINDLE STUFF ====================
|
// ======== SPINDLE STUFF ====================
|
||||||
|
#ifndef DEFAULT_SPINDLE_RPM_MIN // $31
|
||||||
|
#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef DEFAULT_LASER_MODE // $32
|
||||||
|
#define DEFAULT_LASER_MODE 0 // false
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef DEFAULT_SPINDLE_RPM_MAX // $30
|
||||||
|
#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef DEFAULT_SPINDLE_FREQ
|
#ifndef DEFAULT_SPINDLE_FREQ
|
||||||
#define DEFAULT_SPINDLE_FREQ 5000.0 // $33 Hz (extended set)
|
#define DEFAULT_SPINDLE_FREQ 5000.0 // $33 Hz (extended set)
|
||||||
@@ -132,20 +143,7 @@
|
|||||||
#define DEFAULT_SPINDLE_MAX_VALUE 100.0 // $36 Percent (extended set)
|
#define DEFAULT_SPINDLE_MAX_VALUE 100.0 // $36 Percent (extended set)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef DEFAULT_SPINDLE_RPM_MAX
|
// ================ user settings =====================
|
||||||
#define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
#ifndef DEFAULT_SPINDLE_RPM_MIN
|
|
||||||
#define DEFAULT_SPINDLE_RPM_MIN 0.0 // rpm
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef DEFAULT_LASER_MODE
|
|
||||||
#define DEFAULT_LASER_MODE 0 // false
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// user settings
|
|
||||||
#ifndef DEFAULT_USER_INT_80
|
#ifndef DEFAULT_USER_INT_80
|
||||||
#define DEFAULT_USER_INT_80 0 // $80 User integer setting
|
#define DEFAULT_USER_INT_80 0 // $80 User integer setting
|
||||||
#endif
|
#endif
|
||||||
|
@@ -295,7 +295,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
|||||||
case 3:
|
case 3:
|
||||||
case 4:
|
case 4:
|
||||||
case 5:
|
case 5:
|
||||||
#ifndef SPINDLE_PWM_PIN
|
#ifndef SPINDLE_OUTPUT_PIN
|
||||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No spindle pin defined");
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No spindle pin defined");
|
||||||
#endif
|
#endif
|
||||||
word_bit = MODAL_GROUP_M7;
|
word_bit = MODAL_GROUP_M7;
|
||||||
@@ -304,14 +304,10 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
|||||||
gc_block.modal.spindle = SPINDLE_ENABLE_CW;
|
gc_block.modal.spindle = SPINDLE_ENABLE_CW;
|
||||||
break;
|
break;
|
||||||
case 4: // Supported if SPINDLE_DIR_PIN is defined or laser mode is on.
|
case 4: // Supported if SPINDLE_DIR_PIN is defined or laser mode is on.
|
||||||
#ifndef SPINDLE_DIR_PIN
|
if (spindle->is_reversable || bit_istrue(settings.flags, BITFLAG_LASER_MODE))
|
||||||
// if laser mode is not on then this is an unsupported command
|
gc_block.modal.spindle = SPINDLE_ENABLE_CCW;
|
||||||
if bit_isfalse(settings.flags, BITFLAG_LASER_MODE) {
|
else
|
||||||
FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND);
|
FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND);
|
||||||
break;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
gc_block.modal.spindle = SPINDLE_ENABLE_CCW;
|
|
||||||
break;
|
break;
|
||||||
case 5:
|
case 5:
|
||||||
gc_block.modal.spindle = SPINDLE_DISABLE;
|
gc_block.modal.spindle = SPINDLE_DISABLE;
|
||||||
@@ -1088,16 +1084,12 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
|||||||
// [4. Set spindle speed ]:
|
// [4. Set spindle speed ]:
|
||||||
if ((gc_state.spindle_speed != gc_block.values.s) || bit_istrue(gc_parser_flags, GC_PARSER_LASER_FORCE_SYNC)) {
|
if ((gc_state.spindle_speed != gc_block.values.s) || bit_istrue(gc_parser_flags, GC_PARSER_LASER_FORCE_SYNC)) {
|
||||||
if (gc_state.modal.spindle != SPINDLE_DISABLE) {
|
if (gc_state.modal.spindle != SPINDLE_DISABLE) {
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_ISMOTION)) {
|
if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_ISMOTION)) {
|
||||||
if (bit_istrue(gc_parser_flags, GC_PARSER_LASER_DISABLE))
|
if (bit_istrue(gc_parser_flags, GC_PARSER_LASER_DISABLE))
|
||||||
spindle_sync(gc_state.modal.spindle, 0.0);
|
spindle->set_state(gc_state.modal.spindle, 0.0);
|
||||||
else
|
else
|
||||||
spindle_sync(gc_state.modal.spindle, gc_block.values.s);
|
spindle->set_state(gc_state.modal.spindle, gc_block.values.s);
|
||||||
}
|
}
|
||||||
#else
|
|
||||||
spindle_sync(gc_state.modal.spindle, 0.0);
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
gc_state.spindle_speed = gc_block.values.s; // Update spindle speed state.
|
gc_state.spindle_speed = gc_block.values.s; // Update spindle speed state.
|
||||||
}
|
}
|
||||||
@@ -1118,7 +1110,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
|||||||
// Update spindle control and apply spindle speed when enabling it in this block.
|
// Update spindle control and apply spindle speed when enabling it in this block.
|
||||||
// NOTE: All spindle state changes are synced, even in laser mode. Also, pl_data,
|
// NOTE: All spindle state changes are synced, even in laser mode. Also, pl_data,
|
||||||
// rather than gc_state, is used to manage laser state for non-laser motions.
|
// rather than gc_state, is used to manage laser state for non-laser motions.
|
||||||
spindle_sync(gc_block.modal.spindle, pl_data->spindle_speed);
|
spindle->set_state(gc_block.modal.spindle, pl_data->spindle_speed);
|
||||||
gc_state.modal.spindle = gc_block.modal.spindle;
|
gc_state.modal.spindle = gc_block.modal.spindle;
|
||||||
}
|
}
|
||||||
pl_data->condition |= gc_state.modal.spindle; // Set condition flag for planner use.
|
pl_data->condition |= gc_state.modal.spindle; // Set condition flag for planner use.
|
||||||
@@ -1294,7 +1286,7 @@ uint8_t gc_execute_line(char* line, uint8_t client) {
|
|||||||
if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system)))
|
if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system)))
|
||||||
FAIL(STATUS_SETTING_READ_FAIL);
|
FAIL(STATUS_SETTING_READ_FAIL);
|
||||||
system_flag_wco_change(); // Set to refresh immediately just in case something altered.
|
system_flag_wco_change(); // Set to refresh immediately just in case something altered.
|
||||||
spindle_set_state(SPINDLE_DISABLE, 0.0);
|
spindle->set_state(SPINDLE_DISABLE, 0.0);
|
||||||
coolant_set_state(COOLANT_DISABLE);
|
coolant_set_state(COOLANT_DISABLE);
|
||||||
}
|
}
|
||||||
report_feedback_message(MESSAGE_PROGRAM_END);
|
report_feedback_message(MESSAGE_PROGRAM_END);
|
||||||
|
@@ -19,8 +19,9 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
// Grbl versioning system
|
// Grbl versioning system
|
||||||
#define GRBL_VERSION "1.1f"
|
|
||||||
#define GRBL_VERSION_BUILD "20200419"
|
#define GRBL_VERSION "1.2a"
|
||||||
|
#define GRBL_VERSION_BUILD "20200423"
|
||||||
|
|
||||||
//#include <sdkconfig.h>
|
//#include <sdkconfig.h>
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
@@ -28,6 +29,7 @@
|
|||||||
#include <driver/rmt.h>
|
#include <driver/rmt.h>
|
||||||
#include <esp_task_wdt.h>
|
#include <esp_task_wdt.h>
|
||||||
#include <freertos/task.h>
|
#include <freertos/task.h>
|
||||||
|
#include <Preferences.h>
|
||||||
|
|
||||||
#include "driver/timer.h"
|
#include "driver/timer.h"
|
||||||
|
|
||||||
@@ -51,7 +53,7 @@
|
|||||||
#include "protocol.h"
|
#include "protocol.h"
|
||||||
#include "report.h"
|
#include "report.h"
|
||||||
#include "serial.h"
|
#include "serial.h"
|
||||||
#include "spindle_control.h"
|
#include "tools/SpindleClass.h"
|
||||||
#include "stepper.h"
|
#include "stepper.h"
|
||||||
#include "jog.h"
|
#include "jog.h"
|
||||||
#include "inputbuffer.h"
|
#include "inputbuffer.h"
|
||||||
|
@@ -14,12 +14,6 @@
|
|||||||
// from Machines/, for example:
|
// from Machines/, for example:
|
||||||
// #include "Machines/3axis_v4.h"
|
// #include "Machines/3axis_v4.h"
|
||||||
|
|
||||||
// Some configurations use two files, the first establishing a base
|
|
||||||
// configuration and the second providing additional customization,
|
|
||||||
// for example:
|
|
||||||
// #include "Machines/3axis_v4.h"
|
|
||||||
// #include "Machines/add_esc_spindle.h"
|
|
||||||
|
|
||||||
// === OEM Single File Configuration Option
|
// === OEM Single File Configuration Option
|
||||||
// OEMs that wish to publish source code that is configured for a
|
// OEMs that wish to publish source code that is configured for a
|
||||||
// specific machine may put all of their configuration definitions
|
// specific machine may put all of their configuration definitions
|
||||||
@@ -49,14 +43,6 @@
|
|||||||
|
|
||||||
#include MACHINE_PATHNAME_QUOTED(MACHINE_FILENAME)
|
#include MACHINE_PATHNAME_QUOTED(MACHINE_FILENAME)
|
||||||
|
|
||||||
// You can choose two-file configurations by also defining MACHINE_FILENAME2,
|
|
||||||
// for example:
|
|
||||||
// $env:PLATFORMIO_BUILD_FLAGS='-DMACHINE_FILENAME=3axis_v4.h -DMACHINE_FILENAME2=add_esc_spindle.h'; platformio run
|
|
||||||
|
|
||||||
#ifdef MACHINE_FILENAME2
|
|
||||||
#include MACHINE_PATHNAME_QUOTED(MACHINE_FILENAME2)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif // MACHINE_FILENAME
|
#endif // MACHINE_FILENAME
|
||||||
|
|
||||||
#endif // _machine_h
|
#endif // _machine_h
|
||||||
|
@@ -1,12 +1,10 @@
|
|||||||
#ifndef _machine_common_h
|
#ifndef _machine_common_h
|
||||||
#define _machine_common_h
|
#define _machine_common_h
|
||||||
|
|
||||||
#ifndef SPINDLE_PWM_BIT_PRECISION
|
#ifndef SPINDLE_TYPE
|
||||||
#define SPINDLE_PWM_BIT_PRECISION 8
|
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define SPINDLE_PWM_MAX_VALUE ((1<<SPINDLE_PWM_BIT_PRECISION) - 1)
|
|
||||||
|
|
||||||
// Grbl setting that are common to all machines
|
// Grbl setting that are common to all machines
|
||||||
// It should not be necessary to change anything herein
|
// It should not be necessary to change anything herein
|
||||||
|
|
||||||
|
@@ -424,7 +424,7 @@ void mc_reset() {
|
|||||||
if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
|
if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
|
||||||
system_set_exec_state_flag(EXEC_RESET);
|
system_set_exec_state_flag(EXEC_RESET);
|
||||||
// Kill spindle and coolant.
|
// Kill spindle and coolant.
|
||||||
spindle_stop();
|
spindle->stop();
|
||||||
coolant_stop();
|
coolant_stop();
|
||||||
// turn off all digital I/O
|
// turn off all digital I/O
|
||||||
sys_io_control(0xFF, false);
|
sys_io_control(0xFF, false);
|
||||||
|
@@ -149,6 +149,10 @@ float map_float(float x, float in_min, float in_max, float out_min, float out_ma
|
|||||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint32_t map_uint32_t(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max) {
|
||||||
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
|
}
|
||||||
|
|
||||||
float constrain_float(float in, float min, float max) { // DrawBot_Badge
|
float constrain_float(float in, float min, float max) { // DrawBot_Badge
|
||||||
if (in < min)
|
if (in < min)
|
||||||
return min;
|
return min;
|
||||||
@@ -162,4 +166,8 @@ float mapConstrain(float x, float in_min, float in_max, float out_min, float out
|
|||||||
return map_float(x, in_min, in_max, out_min, out_max);
|
return map_float(x, in_min, in_max, out_min, out_max);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool char_is_numeric(char value) {
|
||||||
|
return (value >= '0' && value <='9');
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@@ -89,7 +89,9 @@ float limit_value_by_axis_maximum(float* max_value, float* unit_vec);
|
|||||||
|
|
||||||
float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max);
|
float mapConstrain(float x, float in_min, float in_max, float out_min, float out_max);
|
||||||
float map_float(float x, float in_min, float in_max, float out_min, float out_max);
|
float map_float(float x, float in_min, float in_max, float out_min, float out_max);
|
||||||
|
uint32_t map_uint32_t(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max);
|
||||||
float constrain_float(float in, float min, float max);
|
float constrain_float(float in, float min, float max);
|
||||||
|
bool char_is_numeric(char value);
|
||||||
|
|
||||||
template <class T> void swap(T& a, T& b) {
|
template <class T> void swap(T& a, T& b) {
|
||||||
T c(a); a = b; b = c;
|
T c(a); a = b; b = c;
|
||||||
|
@@ -284,9 +284,8 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
|
|||||||
plan_block_t* block = &block_buffer[block_buffer_head];
|
plan_block_t* block = &block_buffer[block_buffer_head];
|
||||||
memset(block, 0, sizeof(plan_block_t)); // Zero all block values.
|
memset(block, 0, sizeof(plan_block_t)); // Zero all block values.
|
||||||
block->condition = pl_data->condition;
|
block->condition = pl_data->condition;
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
block->spindle_speed = pl_data->spindle_speed;
|
block->spindle_speed = pl_data->spindle_speed;
|
||||||
#endif
|
|
||||||
#ifdef USE_LINE_NUMBERS
|
#ifdef USE_LINE_NUMBERS
|
||||||
block->line_number = pl_data->line_number;
|
block->line_number = pl_data->line_number;
|
||||||
#endif
|
#endif
|
||||||
|
@@ -81,7 +81,7 @@ typedef struct {
|
|||||||
float rapid_rate; // Axis-limit adjusted maximum rate for this block direction in (mm/min)
|
float rapid_rate; // Axis-limit adjusted maximum rate for this block direction in (mm/min)
|
||||||
float programmed_rate; // Programmed rate of this block (mm/min).
|
float programmed_rate; // Programmed rate of this block (mm/min).
|
||||||
|
|
||||||
//#ifdef VARIABLE_SPINDLE
|
|
||||||
// Stored spindle speed data used by spindle overrides and resuming methods.
|
// Stored spindle speed data used by spindle overrides and resuming methods.
|
||||||
float spindle_speed; // Block spindle speed. Copied from pl_line_data.
|
float spindle_speed; // Block spindle speed. Copied from pl_line_data.
|
||||||
//#endif
|
//#endif
|
||||||
|
@@ -464,7 +464,7 @@ void protocol_exec_rt_system() {
|
|||||||
last_s_override = MIN(last_s_override, MAX_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);
|
last_s_override = MAX(last_s_override, MIN_SPINDLE_SPEED_OVERRIDE);
|
||||||
if (last_s_override != sys.spindle_speed_ovr) {
|
if (last_s_override != sys.spindle_speed_ovr) {
|
||||||
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM);
|
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
|
||||||
sys.spindle_speed_ovr = last_s_override;
|
sys.spindle_speed_ovr = last_s_override;
|
||||||
sys.report_ovr_counter = 0; // Set to report change immediately
|
sys.report_ovr_counter = 0; // Set to report change immediately
|
||||||
}
|
}
|
||||||
@@ -535,7 +535,6 @@ static void protocol_exec_rt_suspend() {
|
|||||||
#endif
|
#endif
|
||||||
plan_block_t* block = plan_get_current_block();
|
plan_block_t* block = plan_get_current_block();
|
||||||
uint8_t restore_condition;
|
uint8_t restore_condition;
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
float restore_spindle_speed;
|
float restore_spindle_speed;
|
||||||
if (block == NULL) {
|
if (block == NULL) {
|
||||||
restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant);
|
restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant);
|
||||||
@@ -548,10 +547,7 @@ static void protocol_exec_rt_suspend() {
|
|||||||
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE))
|
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE))
|
||||||
system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP);
|
system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP);
|
||||||
#endif
|
#endif
|
||||||
#else
|
|
||||||
if (block == NULL) restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant);
|
|
||||||
else restore_condition = block->condition;
|
|
||||||
#endif
|
|
||||||
while (sys.suspend) {
|
while (sys.suspend) {
|
||||||
if (sys.abort) return;
|
if (sys.abort) return;
|
||||||
// Block until initial hold is complete and the machine has stopped motion.
|
// Block until initial hold is complete and the machine has stopped motion.
|
||||||
@@ -564,7 +560,7 @@ static void protocol_exec_rt_suspend() {
|
|||||||
// Ensure any prior spindle stop override is disabled at start of safety door routine.
|
// Ensure any prior spindle stop override is disabled at start of safety door routine.
|
||||||
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED;
|
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED;
|
||||||
#ifndef PARKING_ENABLE
|
#ifndef PARKING_ENABLE
|
||||||
spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
spindle->set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
||||||
coolant_set_state(COOLANT_DISABLE); // De-energize
|
coolant_set_state(COOLANT_DISABLE); // De-energize
|
||||||
#else
|
#else
|
||||||
// Get current position and store restore location and spindle retract waypoint.
|
// Get current position and store restore location and spindle retract waypoint.
|
||||||
@@ -599,20 +595,20 @@ static void protocol_exec_rt_suspend() {
|
|||||||
// NOTE: Clear accessory state after retract and after an aborted restore motion.
|
// 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->condition = (PL_COND_FLAG_SYSTEM_MOTION | PL_COND_FLAG_NO_FEED_OVERRIDE);
|
||||||
pl_data->spindle_speed = 0.0;
|
pl_data->spindle_speed = 0.0;
|
||||||
spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
spindle->set_state((SPINDLE_DISABLE, 0.0); // De-energize
|
||||||
coolant_set_state(COOLANT_DISABLE); // De-energize
|
coolant_set_state(COOLANT_DISABLE); // De-energize
|
||||||
// Execute fast parking retract motion to parking target location.
|
// Execute fast parking retract motion to parking target location.
|
||||||
if (parking_target[PARKING_AXIS] < PARKING_TARGET) {
|
if (parking_target[PARKING_AXIS] < PARKING_TARGET) {
|
||||||
parking_target[PARKING_AXIS] = PARKING_TARGET;
|
parking_target[PARKING_AXIS] = PARKING_TARGET;
|
||||||
pl_data->feed_rate = PARKING_RATE;
|
pl_data->feed_rate = PARKING_RATE;
|
||||||
mc_parking_motion(parking_target, pl_data);
|
mc_parking_motion(parking_target, pl_data);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// Parking motion not possible. Just disable the spindle and coolant.
|
// 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.
|
// NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately.
|
||||||
spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
->set_state((SPINDLE_DISABLE, 0.0); // De-energize
|
||||||
coolant_set_state(COOLANT_DISABLE); // De-energize
|
coolant_set_state(COOLANT_DISABLE); // De-energize
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
sys.suspend &= ~(SUSPEND_RESTART_RETRACT);
|
sys.suspend &= ~(SUSPEND_RESTART_RETRACT);
|
||||||
sys.suspend |= SUSPEND_RETRACT_COMPLETE;
|
sys.suspend |= SUSPEND_RETRACT_COMPLETE;
|
||||||
@@ -620,7 +616,7 @@ static void protocol_exec_rt_suspend() {
|
|||||||
if (sys.state == STATE_SLEEP) {
|
if (sys.state == STATE_SLEEP) {
|
||||||
report_feedback_message(MESSAGE_SLEEP_MODE);
|
report_feedback_message(MESSAGE_SLEEP_MODE);
|
||||||
// Spindle and coolant should already be stopped, but do it again just to be sure.
|
// Spindle and coolant should already be stopped, but do it again just to be sure.
|
||||||
spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
spindle->set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
||||||
coolant_set_state(COOLANT_DISABLE); // De-energize
|
coolant_set_state(COOLANT_DISABLE); // De-energize
|
||||||
st_go_idle(); // Disable steppers
|
st_go_idle(); // Disable steppers
|
||||||
while (!(sys.abort)) protocol_exec_rt_system(); // Do nothing until reset.
|
while (!(sys.abort)) protocol_exec_rt_system(); // Do nothing until reset.
|
||||||
@@ -657,9 +653,9 @@ static void protocol_exec_rt_suspend() {
|
|||||||
if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
|
if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
|
||||||
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
|
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
|
||||||
// When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
|
// 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_PWM);
|
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
|
||||||
} else {
|
} else {
|
||||||
spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed);
|
spindle->set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed);
|
||||||
delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND);
|
delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -705,7 +701,7 @@ static void protocol_exec_rt_suspend() {
|
|||||||
// Handles beginning of spindle stop
|
// Handles beginning of spindle stop
|
||||||
if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_INITIATE) {
|
if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_INITIATE) {
|
||||||
if (gc_state.modal.spindle != SPINDLE_DISABLE) {
|
if (gc_state.modal.spindle != SPINDLE_DISABLE) {
|
||||||
spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
spindle->set_state(SPINDLE_DISABLE, 0.0); // De-energize
|
||||||
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_ENABLED; // Set stop override state to enabled, if de-energized.
|
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_ENABLED; // Set stop override state to enabled, if de-energized.
|
||||||
} else {
|
} else {
|
||||||
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state
|
sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state
|
||||||
@@ -716,9 +712,9 @@ static void protocol_exec_rt_suspend() {
|
|||||||
report_feedback_message(MESSAGE_SPINDLE_RESTORE);
|
report_feedback_message(MESSAGE_SPINDLE_RESTORE);
|
||||||
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
|
if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
|
||||||
// When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
|
// 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_PWM);
|
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
|
||||||
} else
|
} else
|
||||||
spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed);
|
spindle->set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed);
|
||||||
}
|
}
|
||||||
if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_RESTORE_CYCLE) {
|
if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_RESTORE_CYCLE) {
|
||||||
system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program.
|
system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program.
|
||||||
@@ -727,10 +723,10 @@ static void protocol_exec_rt_suspend() {
|
|||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state.
|
// Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state.
|
||||||
// NOTE: STEP_CONTROL_UPDATE_SPINDLE_PWM is automatically reset upon resume in step generator.
|
// NOTE: STEP_CONTROL_UPDATE_SPINDLE_RPM is automatically reset upon resume in step generator.
|
||||||
if (bit_istrue(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM)) {
|
if (bit_istrue(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM)) {
|
||||||
spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed);
|
spindle->set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed);
|
||||||
bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM);
|
bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@@ -308,11 +308,8 @@ void report_grbl_settings(uint8_t client, uint8_t show_extended) {
|
|||||||
sprintf(setting, "$27=%4.3f\r\n", settings.homing_pulloff); strcat(rpt, setting);
|
sprintf(setting, "$27=%4.3f\r\n", settings.homing_pulloff); strcat(rpt, setting);
|
||||||
sprintf(setting, "$30=%4.3f\r\n", settings.rpm_max); strcat(rpt, setting);
|
sprintf(setting, "$30=%4.3f\r\n", settings.rpm_max); strcat(rpt, setting);
|
||||||
sprintf(setting, "$31=%4.3f\r\n", settings.rpm_min); strcat(rpt, setting);
|
sprintf(setting, "$31=%4.3f\r\n", settings.rpm_min); strcat(rpt, setting);
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
sprintf(setting, "$32=%d\r\n", bit_istrue(settings.flags, BITFLAG_LASER_MODE)); strcat(rpt, setting);
|
sprintf(setting, "$32=%d\r\n", bit_istrue(settings.flags, BITFLAG_LASER_MODE)); strcat(rpt, setting);
|
||||||
#else
|
|
||||||
strcat(rpt, "$32=0\r\n");
|
|
||||||
#endif
|
|
||||||
if (show_extended) {
|
if (show_extended) {
|
||||||
sprintf(setting, "$33=%5.3f\r\n", settings.spindle_pwm_freq); strcat(rpt, setting);
|
sprintf(setting, "$33=%5.3f\r\n", settings.spindle_pwm_freq); strcat(rpt, setting);
|
||||||
sprintf(setting, "$34=%3.3f\r\n", settings.spindle_pwm_off_value); strcat(rpt, setting);
|
sprintf(setting, "$34=%3.3f\r\n", settings.spindle_pwm_off_value); strcat(rpt, setting);
|
||||||
@@ -471,10 +468,8 @@ void report_gcode_modes(uint8_t client) {
|
|||||||
else
|
else
|
||||||
sprintf(temp, " F%.0f", gc_state.feed_rate);
|
sprintf(temp, " F%.0f", gc_state.feed_rate);
|
||||||
strcat(modes_rpt, temp);
|
strcat(modes_rpt, temp);
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
sprintf(temp, " S%4.3f", gc_state.spindle_speed);
|
sprintf(temp, " S%4.3f", gc_state.spindle_speed);
|
||||||
strcat(modes_rpt, temp);
|
strcat(modes_rpt, temp);
|
||||||
#endif
|
|
||||||
strcat(modes_rpt, "]\r\n");
|
strcat(modes_rpt, "]\r\n");
|
||||||
grbl_send(client, modes_rpt);
|
grbl_send(client, modes_rpt);
|
||||||
}
|
}
|
||||||
@@ -497,12 +492,8 @@ void report_build_info(char* line, uint8_t client) {
|
|||||||
strcpy(build_info, "[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":");
|
strcpy(build_info, "[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":");
|
||||||
strcat(build_info, line);
|
strcat(build_info, line);
|
||||||
strcat(build_info, "]\r\n[OPT:");
|
strcat(build_info, "]\r\n[OPT:");
|
||||||
#ifdef VARIABLE_SPINDLE
|
strcat(build_info, "V"); // variable spindle..always on now
|
||||||
strcat(build_info, "V");
|
|
||||||
#endif
|
|
||||||
#ifdef USE_LINE_NUMBERS
|
|
||||||
strcat(build_info, "N");
|
strcat(build_info, "N");
|
||||||
#endif
|
|
||||||
#ifdef COOLANT_MIST_PIN
|
#ifdef COOLANT_MIST_PIN
|
||||||
strcat(build_info, "M"); // TODO Need to deal with M8...it could be disabled
|
strcat(build_info, "M"); // TODO Need to deal with M8...it could be disabled
|
||||||
#endif
|
#endif
|
||||||
@@ -558,6 +549,7 @@ void report_build_info(char* line, uint8_t client) {
|
|||||||
// These will likely have a comma delimiter to separate them.
|
// These will likely have a comma delimiter to separate them.
|
||||||
strcat(build_info, "]\r\n");
|
strcat(build_info, "]\r\n");
|
||||||
grbl_send(client, build_info); // ok to send to all
|
grbl_send(client, build_info); // ok to send to all
|
||||||
|
report_machine_type(client);
|
||||||
#if defined (ENABLE_WIFI)
|
#if defined (ENABLE_WIFI)
|
||||||
grbl_send(client, (char*)wifi_config.info());
|
grbl_send(client, (char*)wifi_config.info());
|
||||||
#endif
|
#endif
|
||||||
@@ -679,19 +671,11 @@ void report_realtime_status(uint8_t client) {
|
|||||||
#endif
|
#endif
|
||||||
// Report realtime feed speed
|
// Report realtime feed speed
|
||||||
#ifdef REPORT_FIELD_CURRENT_FEED_SPEED
|
#ifdef REPORT_FIELD_CURRENT_FEED_SPEED
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES))
|
if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES))
|
||||||
sprintf(temp, "|FS:%.1f,%.0f", st_get_realtime_rate(), sys.spindle_speed / MM_PER_INCH);
|
sprintf(temp, "|FS:%.1f,%.0f", st_get_realtime_rate(), sys.spindle_speed / MM_PER_INCH);
|
||||||
else
|
else
|
||||||
sprintf(temp, "|FS:%.0f,%.0f", st_get_realtime_rate(), sys.spindle_speed);
|
sprintf(temp, "|FS:%.0f,%.0f", st_get_realtime_rate(), sys.spindle_speed);
|
||||||
strcat(status, temp);
|
strcat(status, temp);
|
||||||
#else
|
|
||||||
if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES))
|
|
||||||
sprintf(temp, "|F:%.1f", st_get_realtime_rate() / MM_PER_INCH);
|
|
||||||
else
|
|
||||||
sprintf(temp, "|F:%.0f", st_get_realtime_rate());
|
|
||||||
strcat(status, temp);
|
|
||||||
#endif
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef REPORT_FIELD_PIN_STATE
|
#ifdef REPORT_FIELD_PIN_STATE
|
||||||
uint8_t lim_pin_state = limits_get_state();
|
uint8_t lim_pin_state = limits_get_state();
|
||||||
@@ -744,7 +728,7 @@ void report_realtime_status(uint8_t client) {
|
|||||||
} else sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT - 1);
|
} else sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT - 1);
|
||||||
sprintf(temp, "|Ov:%d,%d,%d", sys.f_override, sys.r_override, sys.spindle_speed_ovr);
|
sprintf(temp, "|Ov:%d,%d,%d", sys.f_override, sys.r_override, sys.spindle_speed_ovr);
|
||||||
strcat(status, temp);
|
strcat(status, temp);
|
||||||
uint8_t sp_state = spindle_get_state();
|
uint8_t sp_state = spindle->get_state();
|
||||||
uint8_t cl_state = coolant_get_state();
|
uint8_t cl_state = coolant_get_state();
|
||||||
if (sp_state || cl_state) {
|
if (sp_state || cl_state) {
|
||||||
strcat(status, "|A:");
|
strcat(status, "|A:");
|
||||||
@@ -790,4 +774,46 @@ void report_gcode_comment(char* comment) {
|
|||||||
msg[index - offset] = 0; // null terminate
|
msg[index - offset] = 0; // null terminate
|
||||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "GCode Comment...%s", msg);
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "GCode Comment...%s", msg);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void report_machine_type(uint8_t client) {
|
||||||
|
grbl_msg_sendf(client, MSG_LEVEL_INFO, "Using machine:%s", MACHINE_NAME);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
Print a message in hex format
|
||||||
|
Ex: report_hex_msg(msg, "Rx:", 6);
|
||||||
|
Would would print something like ... [MSG Rx: 0x01 0x03 0x01 0x08 0x31 0xbf]
|
||||||
|
*/
|
||||||
|
void report_hex_msg(char* buf, const char* prefix, int len) {
|
||||||
|
char report[200];
|
||||||
|
char temp[20];
|
||||||
|
sprintf(report, "%s", prefix);
|
||||||
|
for (int i = 0; i < len; i++) {
|
||||||
|
sprintf(temp, " 0x%02X", buf[i]);
|
||||||
|
strcat(report, temp);
|
||||||
|
}
|
||||||
|
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s", report);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
char report_get_axis_letter(uint8_t axis) {
|
||||||
|
switch (axis) {
|
||||||
|
case X_AXIS:
|
||||||
|
return 'X';
|
||||||
|
case Y_AXIS:
|
||||||
|
return 'Y';
|
||||||
|
case Z_AXIS:
|
||||||
|
return 'Z';
|
||||||
|
case A_AXIS:
|
||||||
|
return 'A';
|
||||||
|
case B_AXIS:
|
||||||
|
return 'B';
|
||||||
|
case C_AXIS:
|
||||||
|
return 'C';
|
||||||
|
default:
|
||||||
|
return '?';
|
||||||
|
}
|
||||||
}
|
}
|
@@ -170,6 +170,10 @@ void report_gcode_comment(char* comment);
|
|||||||
void report_realtime_debug();
|
void report_realtime_debug();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
void report_machine_type(uint8_t client);
|
||||||
|
|
||||||
|
void report_hex_msg(char* buf, const char *prefix, int len);
|
||||||
|
|
||||||
|
char report_get_axis_letter(uint8_t axis);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@@ -40,7 +40,7 @@ void settings_init() {
|
|||||||
if (!read_global_settings()) {
|
if (!read_global_settings()) {
|
||||||
report_status_message(STATUS_SETTING_READ_FAIL, CLIENT_SERIAL);
|
report_status_message(STATUS_SETTING_READ_FAIL, CLIENT_SERIAL);
|
||||||
settings_restore(SETTINGS_RESTORE_ALL); // Force restore all EEPROM data.
|
settings_restore(SETTINGS_RESTORE_ALL); // Force restore all EEPROM data.
|
||||||
report_grbl_settings(CLIENT_SERIAL, false); // only the serial could be working at this point
|
report_grbl_settings(CLIENT_SERIAL, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -64,12 +64,14 @@ void settings_restore(uint8_t restore_flag) {
|
|||||||
settings.status_report_mask = DEFAULT_STATUS_REPORT_MASK;
|
settings.status_report_mask = DEFAULT_STATUS_REPORT_MASK;
|
||||||
settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION;
|
settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION;
|
||||||
settings.arc_tolerance = DEFAULT_ARC_TOLERANCE;
|
settings.arc_tolerance = DEFAULT_ARC_TOLERANCE;
|
||||||
|
|
||||||
|
settings.rpm_max = DEFAULT_SPINDLE_RPM_MAX;
|
||||||
|
settings.rpm_min = DEFAULT_SPINDLE_RPM_MIN;
|
||||||
settings.spindle_pwm_freq = DEFAULT_SPINDLE_FREQ; // $33 Hz (extended set)
|
settings.spindle_pwm_freq = DEFAULT_SPINDLE_FREQ; // $33 Hz (extended set)
|
||||||
settings.spindle_pwm_off_value = DEFAULT_SPINDLE_OFF_VALUE; // $34 Percent (extended set)
|
settings.spindle_pwm_off_value = DEFAULT_SPINDLE_OFF_VALUE; // $34 Percent (extended set)
|
||||||
settings.spindle_pwm_min_value = DEFAULT_SPINDLE_MIN_VALUE; // $35 Percent (extended set)
|
settings.spindle_pwm_min_value = DEFAULT_SPINDLE_MIN_VALUE; // $35 Percent (extended set)
|
||||||
settings.spindle_pwm_max_value = DEFAULT_SPINDLE_MAX_VALUE; // $36 Percent (extended set)
|
settings.spindle_pwm_max_value = DEFAULT_SPINDLE_MAX_VALUE; // $36 Percent (extended set)
|
||||||
settings.rpm_max = DEFAULT_SPINDLE_RPM_MAX;
|
|
||||||
settings.rpm_min = DEFAULT_SPINDLE_RPM_MIN;
|
|
||||||
settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK;
|
settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK;
|
||||||
settings.homing_feed_rate = DEFAULT_HOMING_FEED_RATE;
|
settings.homing_feed_rate = DEFAULT_HOMING_FEED_RATE;
|
||||||
settings.homing_seek_rate = DEFAULT_HOMING_SEEK_RATE;
|
settings.homing_seek_rate = DEFAULT_HOMING_SEEK_RATE;
|
||||||
@@ -366,20 +368,19 @@ uint8_t settings_store_global_setting(uint8_t parameter, float value) {
|
|||||||
case 25: settings.homing_seek_rate = value; break;
|
case 25: settings.homing_seek_rate = value; break;
|
||||||
case 26: settings.homing_debounce_delay = int_value; break;
|
case 26: settings.homing_debounce_delay = int_value; break;
|
||||||
case 27: settings.homing_pulloff = value; break;
|
case 27: settings.homing_pulloff = value; break;
|
||||||
case 30: settings.rpm_max = value; spindle_init(); break; // Re-initialize spindle rpm calibration
|
case 30: settings.rpm_max = std::max(value, 1.0f); spindle->init(); break; // Re-initialize spindle rpm calibration (min of 1)
|
||||||
case 31: settings.rpm_min = value; spindle_init(); break; // Re-initialize spindle rpm calibration
|
case 31: settings.rpm_min = value; spindle->init(); break; // Re-initialize spindle rpm calibration
|
||||||
case 32:
|
case 32:
|
||||||
#ifdef VARIABLE_SPINDLE
|
if (int_value)
|
||||||
if (int_value) settings.flags |= BITFLAG_LASER_MODE;
|
settings.flags |= BITFLAG_LASER_MODE;
|
||||||
else settings.flags &= ~BITFLAG_LASER_MODE;
|
else
|
||||||
#else
|
settings.flags &= ~BITFLAG_LASER_MODE;
|
||||||
return (STATUS_SETTING_DISABLED_LASER);
|
spindle->init(); // update the spindle class
|
||||||
#endif
|
|
||||||
break;
|
break;
|
||||||
case 33: settings.spindle_pwm_freq = value; spindle_init(); break; // Re-initialize spindle pwm calibration
|
case 33: settings.spindle_pwm_freq = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration
|
||||||
case 34: settings.spindle_pwm_off_value = value; spindle_init(); break; // Re-initialize spindle pwm calibration
|
case 34: settings.spindle_pwm_off_value = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration
|
||||||
case 35: settings.spindle_pwm_min_value = value; spindle_init(); break; // Re-initialize spindle pwm calibration
|
case 35: settings.spindle_pwm_min_value = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration
|
||||||
case 36: settings.spindle_pwm_max_value = value; spindle_init(); break; // Re-initialize spindle pwm calibration
|
case 36: settings.spindle_pwm_max_value = value; spindle_select(SPINDLE_TYPE); break; // Re-initialize spindle pwm calibration
|
||||||
case 80:
|
case 80:
|
||||||
case 81:
|
case 81:
|
||||||
case 82:
|
case 82:
|
||||||
|
@@ -1,226 +0,0 @@
|
|||||||
/*
|
|
||||||
spindle_control.cpp - Header for system level commands and real-time processes
|
|
||||||
Part of Grbl
|
|
||||||
Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC
|
|
||||||
|
|
||||||
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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "grbl.h"
|
|
||||||
|
|
||||||
int8_t spindle_pwm_chan_num;
|
|
||||||
|
|
||||||
#ifdef SPINDLE_PWM_PIN
|
|
||||||
static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
|
|
||||||
uint32_t spindle_pwm_period; // how many counts in 1 period
|
|
||||||
uint32_t spindle_pwm_off_value;
|
|
||||||
uint32_t spindle_pwm_min_value;
|
|
||||||
uint32_t spindle_pwm_max_value;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void spindle_init() {
|
|
||||||
#ifdef SPINDLE_PWM_PIN
|
|
||||||
#ifdef INVERT_SPINDLE_PWM
|
|
||||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "INVERT_SPINDLE_PWM");
|
|
||||||
#endif
|
|
||||||
#ifdef INVERT_SPINDLE_ENABLE_PIN
|
|
||||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "INVERT_SPINDLE_ENABLE_PIN");
|
|
||||||
#endif
|
|
||||||
// determine how many PWM counts are in eqach PWM cycle
|
|
||||||
spindle_pwm_period = ((1 << SPINDLE_PWM_BIT_PRECISION) - 1);
|
|
||||||
if (settings.spindle_pwm_min_value > settings.spindle_pwm_min_value)
|
|
||||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning spindle min pwm is greater than max. Check $35 and $36");
|
|
||||||
// pre-caculate some PWM count values
|
|
||||||
spindle_pwm_off_value = (spindle_pwm_period * settings.spindle_pwm_off_value / 100.0);
|
|
||||||
spindle_pwm_min_value = (spindle_pwm_period * settings.spindle_pwm_min_value / 100.0);
|
|
||||||
spindle_pwm_max_value = (spindle_pwm_period * settings.spindle_pwm_max_value / 100.0);
|
|
||||||
// The pwm_gradient is the pwm duty cycle units per rpm
|
|
||||||
pwm_gradient = (spindle_pwm_max_value - spindle_pwm_min_value) / (settings.rpm_max - settings.rpm_min);
|
|
||||||
// Use DIR and Enable if pins are defined
|
|
||||||
#ifdef SPINDLE_ENABLE_PIN
|
|
||||||
pinMode(SPINDLE_ENABLE_PIN, OUTPUT);
|
|
||||||
#endif
|
|
||||||
#ifdef SPINDLE_DIR_PIN
|
|
||||||
pinMode(SPINDLE_DIR_PIN, OUTPUT);
|
|
||||||
#endif
|
|
||||||
// use the LED control feature to setup PWM https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/peripherals/ledc.html
|
|
||||||
spindle_pwm_chan_num = 0; // spindle always uses channel 0
|
|
||||||
ledcSetup(spindle_pwm_chan_num, (double)settings.spindle_pwm_freq, SPINDLE_PWM_BIT_PRECISION); // setup the channel
|
|
||||||
ledcAttachPin(SPINDLE_PWM_PIN, spindle_pwm_chan_num); // attach the PWM to the pin
|
|
||||||
// Start with spindle off off
|
|
||||||
spindle_stop();
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void spindle_stop() {
|
|
||||||
spindle_set_enable(false);
|
|
||||||
#ifdef SPINDLE_PWM_PIN
|
|
||||||
#ifndef INVERT_SPINDLE_PWM
|
|
||||||
grbl_analogWrite(spindle_pwm_chan_num, spindle_pwm_off_value);
|
|
||||||
#else
|
|
||||||
grbl_analogWrite(spindle_pwm_chan_num, (1 << SPINDLE_PWM_BIT_PRECISION)); // TO DO...wrong for min_pwm
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t spindle_get_state() { // returns SPINDLE_STATE_DISABLE, SPINDLE_STATE_CW or SPINDLE_STATE_CCW
|
|
||||||
// TODO Update this when direction and enable pin are added
|
|
||||||
#ifndef SPINDLE_PWM_PIN
|
|
||||||
return (SPINDLE_STATE_DISABLE);
|
|
||||||
#else
|
|
||||||
if (ledcRead(spindle_pwm_chan_num) == 0) // Check the PWM value
|
|
||||||
return (SPINDLE_STATE_DISABLE);
|
|
||||||
else {
|
|
||||||
#ifdef SPINDLE_DIR_PIN
|
|
||||||
if (digitalRead(SPINDLE_DIR_PIN))
|
|
||||||
return (SPINDLE_STATE_CW);
|
|
||||||
else
|
|
||||||
return (SPINDLE_STATE_CCW);
|
|
||||||
#else
|
|
||||||
return (SPINDLE_STATE_CW);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void spindle_set_speed(uint32_t pwm_value) {
|
|
||||||
#ifndef SPINDLE_PWM_PIN
|
|
||||||
//grbl_sendf(CLIENT_SERIAL, "[MSG: set speed...no pin defined]\r\n");
|
|
||||||
return;
|
|
||||||
#else
|
|
||||||
#ifndef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
|
|
||||||
spindle_set_enable(true);
|
|
||||||
#else
|
|
||||||
spindle_set_enable(pwm_value != 0);
|
|
||||||
#endif
|
|
||||||
#ifndef INVERT_SPINDLE_PWM
|
|
||||||
grbl_analogWrite(spindle_pwm_chan_num, pwm_value);
|
|
||||||
#else
|
|
||||||
grbl_analogWrite(spindle_pwm_chan_num, (1 << SPINDLE_PWM_BIT_PRECISION) - pwm_value);
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t spindle_compute_pwm_value(float rpm) {
|
|
||||||
#ifdef SPINDLE_PWM_PIN
|
|
||||||
uint32_t pwm_value;
|
|
||||||
rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value.
|
|
||||||
// Calculate PWM register value based on rpm max/min settings and programmed rpm.
|
|
||||||
if ((settings.rpm_min >= settings.rpm_max) || (rpm >= settings.rpm_max)) {
|
|
||||||
// No PWM range possible. Set simple on/off spindle control pin state.
|
|
||||||
sys.spindle_speed = settings.rpm_max;
|
|
||||||
pwm_value = spindle_pwm_max_value;
|
|
||||||
} else if (rpm <= settings.rpm_min) {
|
|
||||||
if (rpm == 0.0) { // S0 disables spindle
|
|
||||||
sys.spindle_speed = 0.0;
|
|
||||||
pwm_value = spindle_pwm_off_value;
|
|
||||||
} else { // Set minimum PWM output
|
|
||||||
sys.spindle_speed = settings.rpm_min;
|
|
||||||
pwm_value = spindle_pwm_min_value;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// Compute intermediate PWM value with linear spindle speed model.
|
|
||||||
// NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight.
|
|
||||||
sys.spindle_speed = rpm;
|
|
||||||
#ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE
|
|
||||||
pwm_value = piecewise_linear_fit(rpm);
|
|
||||||
#else
|
|
||||||
pwm_value = floor((rpm - settings.rpm_min) * pwm_gradient) + spindle_pwm_min_value;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
return (pwm_value);
|
|
||||||
#else
|
|
||||||
return (0); // no SPINDLE_PWM_PIN
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// Called by spindle_set_state() and step segment generator. Keep routine small and efficient.
|
|
||||||
void spindle_set_state(uint8_t state, float rpm) {
|
|
||||||
#ifdef SPINDLE_PWM_PIN
|
|
||||||
if (sys.abort) return; // Block during abort.
|
|
||||||
if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm.
|
|
||||||
sys.spindle_speed = 0.0;
|
|
||||||
spindle_stop();
|
|
||||||
} else {
|
|
||||||
// TODO ESP32 Enable and direction control
|
|
||||||
#ifdef SPINDLE_DIR_PIN
|
|
||||||
digitalWrite(SPINDLE_DIR_PIN, state == SPINDLE_ENABLE_CW);
|
|
||||||
#endif
|
|
||||||
// NOTE: Assumes all calls to this function is when Grbl is not moving or must remain off.
|
|
||||||
if (settings.flags & BITFLAG_LASER_MODE) {
|
|
||||||
if (state == SPINDLE_ENABLE_CCW) rpm = 0.0; // TODO: May need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE);
|
|
||||||
}
|
|
||||||
spindle_set_speed(spindle_compute_pwm_value(rpm));
|
|
||||||
}
|
|
||||||
sys.report_ovr_counter = 0; // Set to report change immediately
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void spindle_sync(uint8_t state, float rpm) {
|
|
||||||
if (sys.state == STATE_CHECK_MODE)
|
|
||||||
return;
|
|
||||||
protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
|
|
||||||
spindle_set_state(state, rpm);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void grbl_analogWrite(uint8_t chan, uint32_t duty) {
|
|
||||||
// Remember the old duty cycle in memory instead of reading
|
|
||||||
// it from the I/O peripheral because I/O reads are quite
|
|
||||||
// a bit slower than memory reads.
|
|
||||||
static uint32_t old_duty = 0;
|
|
||||||
if (old_duty != duty) { // reduce unnecessary calls to ledcWrite()
|
|
||||||
old_duty = duty;
|
|
||||||
ledcWrite(chan, duty);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void spindle_set_enable(bool enable) {
|
|
||||||
#ifdef SPINDLE_ENABLE_PIN
|
|
||||||
#ifndef INVERT_SPINDLE_ENABLE_PIN
|
|
||||||
digitalWrite(SPINDLE_ENABLE_PIN, enable); // turn off (low) with zero speed
|
|
||||||
#else
|
|
||||||
digitalWrite(SPINDLE_ENABLE_PIN, !enable); // turn off (high) with zero speed
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t piecewise_linear_fit(float rpm) {
|
|
||||||
uint32_t pwm_value;
|
|
||||||
#if (N_PIECES > 3)
|
|
||||||
if (rpm > RPM_POINT34)
|
|
||||||
pwm_value = floor(RPM_LINE_A4 * rpm - RPM_LINE_B4);
|
|
||||||
else
|
|
||||||
#endif
|
|
||||||
#if (N_PIECES > 2)
|
|
||||||
if (rpm > RPM_POINT23)
|
|
||||||
pwm_value = floor(RPM_LINE_A3 * rpm - RPM_LINE_B3);
|
|
||||||
else
|
|
||||||
#endif
|
|
||||||
#if (N_PIECES > 1)
|
|
||||||
if (rpm > RPM_POINT12)
|
|
||||||
pwm_value = floor(RPM_LINE_A2 * rpm - RPM_LINE_B2);
|
|
||||||
else
|
|
||||||
#endif
|
|
||||||
{
|
|
||||||
pwm_value = floor(RPM_LINE_A1 * rpm - RPM_LINE_B1);
|
|
||||||
}
|
|
||||||
return pwm_value;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@@ -1,47 +0,0 @@
|
|||||||
/*
|
|
||||||
spindle.h - Header for system level commands and real-time processes
|
|
||||||
Part of Grbl
|
|
||||||
Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC
|
|
||||||
|
|
||||||
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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef spindle_control_h
|
|
||||||
#define spindle_control_h
|
|
||||||
|
|
||||||
#include "grbl.h"
|
|
||||||
|
|
||||||
|
|
||||||
#define SPINDLE_NO_SYNC false
|
|
||||||
#define SPINDLE_FORCE_SYNC true
|
|
||||||
|
|
||||||
#define SPINDLE_STATE_DISABLE 0 // Must be zero.
|
|
||||||
#define SPINDLE_STATE_CW bit(0)
|
|
||||||
#define SPINDLE_STATE_CCW bit(1)
|
|
||||||
|
|
||||||
extern uint32_t spindle_pwm_off_value;
|
|
||||||
|
|
||||||
void spindle_init();
|
|
||||||
void spindle_stop();
|
|
||||||
uint8_t spindle_get_state();
|
|
||||||
void spindle_set_speed(uint32_t pwm_value);
|
|
||||||
uint32_t spindle_compute_pwm_value(float rpm);
|
|
||||||
void spindle_set_state(uint8_t state, float rpm);
|
|
||||||
void spindle_sync(uint8_t state, float rpm);
|
|
||||||
void grbl_analogWrite(uint8_t chan, uint32_t duty);
|
|
||||||
void spindle_set_enable(bool enable);
|
|
||||||
uint32_t piecewise_linear_fit(float rpm);
|
|
||||||
|
|
||||||
#endif
|
|
@@ -37,9 +37,7 @@ typedef struct {
|
|||||||
uint32_t steps[N_AXIS];
|
uint32_t steps[N_AXIS];
|
||||||
uint32_t step_event_count;
|
uint32_t step_event_count;
|
||||||
uint8_t direction_bits;
|
uint8_t direction_bits;
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
uint8_t is_pwm_rate_adjusted; // Tracks motions that require constant laser power/rate
|
uint8_t is_pwm_rate_adjusted; // Tracks motions that require constant laser power/rate
|
||||||
#endif
|
|
||||||
} st_block_t;
|
} st_block_t;
|
||||||
static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1];
|
static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1];
|
||||||
|
|
||||||
@@ -56,9 +54,7 @@ typedef struct {
|
|||||||
#else
|
#else
|
||||||
uint8_t prescaler; // Without AMASS, a prescaler is required to adjust for slow timing.
|
uint8_t prescaler; // Without AMASS, a prescaler is required to adjust for slow timing.
|
||||||
#endif
|
#endif
|
||||||
#ifdef VARIABLE_SPINDLE
|
uint16_t spindle_rpm; // TODO get rid of this.
|
||||||
uint16_t spindle_pwm;
|
|
||||||
#endif
|
|
||||||
} segment_t;
|
} segment_t;
|
||||||
static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
|
static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
|
||||||
|
|
||||||
@@ -145,10 +141,11 @@ typedef struct {
|
|||||||
float accelerate_until; // Acceleration ramp end measured from end of block (mm)
|
float accelerate_until; // Acceleration ramp end measured from end of block (mm)
|
||||||
float decelerate_after; // Deceleration ramp start measured from end of block (mm)
|
float decelerate_after; // Deceleration ramp start measured from end of block (mm)
|
||||||
|
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
float inv_rate; // Used by PWM laser mode to speed up segment calculations.
|
float inv_rate; // Used by PWM laser mode to speed up segment calculations.
|
||||||
uint16_t current_spindle_pwm;
|
//uint16_t current_spindle_pwm; // todo remove
|
||||||
#endif
|
float current_spindle_rpm;
|
||||||
|
|
||||||
} st_prep_t;
|
} st_prep_t;
|
||||||
static st_prep_t prep;
|
static st_prep_t prep;
|
||||||
|
|
||||||
@@ -302,20 +299,26 @@ static void stepper_pulse_phase_func() {
|
|||||||
st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level;
|
st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level;
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
// Set real-time spindle output as segment is loaded, just prior to the first step.
|
// Set real-time spindle output as segment is loaded, just prior to the first step.
|
||||||
spindle_set_speed(st.exec_segment->spindle_pwm);
|
//spindle_set_speed(st.exec_segment->spindle_rpm);
|
||||||
#endif
|
//grbl_send(CLIENT_SERIAL, "A_");
|
||||||
|
spindle->set_rpm(prep.current_spindle_rpm);
|
||||||
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// Segment buffer empty. Shutdown.
|
// Segment buffer empty. Shutdown.
|
||||||
st_go_idle();
|
st_go_idle();
|
||||||
#if ( (defined VARIABLE_SPINDLE) && (defined SPINDLE_PWM_PIN) )
|
|
||||||
if (!(sys.state & STATE_JOG)) { // added to prevent ... jog after probing crash
|
if (!(sys.state & STATE_JOG)) { // added to prevent ... jog after probing crash
|
||||||
// Ensure pwm is set properly upon completion of rate-controlled motion.
|
// Ensure pwm is set properly upon completion of rate-controlled motion.
|
||||||
if (st.exec_block->is_pwm_rate_adjusted)
|
if (st.exec_block->is_pwm_rate_adjusted) {
|
||||||
spindle_set_speed(spindle_pwm_off_value);
|
//spindle_set_speed(spindle_pwm_off_value);
|
||||||
|
//grbl_send(CLIENT_SERIAL, "B_");
|
||||||
|
spindle->set_rpm(0.0);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end
|
system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end
|
||||||
return; // Nothing to do but exit.
|
return; // Nothing to do but exit.
|
||||||
}
|
}
|
||||||
@@ -835,37 +838,37 @@ void set_direction_pins_on(uint8_t onMask) {
|
|||||||
#ifdef X_DIRECTION_PIN
|
#ifdef X_DIRECTION_PIN
|
||||||
digitalWrite(X_DIRECTION_PIN, (onMask & (1 << X_AXIS)));
|
digitalWrite(X_DIRECTION_PIN, (onMask & (1 << X_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef X2_DIRECTION_PIN // optional ganged axis
|
#ifdef X2_DIRECTION_PIN // optional ganged axis
|
||||||
digitalWrite(X2_DIRECTION_PIN, (onMask & (1 << X_AXIS)));
|
digitalWrite(X2_DIRECTION_PIN, (onMask & (1 << X_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef Y_DIRECTION_PIN
|
#ifdef Y_DIRECTION_PIN
|
||||||
digitalWrite(Y_DIRECTION_PIN, (onMask & (1 << Y_AXIS)));
|
digitalWrite(Y_DIRECTION_PIN, (onMask & (1 << Y_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef Y2_DIRECTION_PIN // optional ganged axis
|
#ifdef Y2_DIRECTION_PIN // optional ganged axis
|
||||||
digitalWrite(Y2_DIRECTION_PIN, (onMask & (1 << Y_AXIS)));
|
digitalWrite(Y2_DIRECTION_PIN, (onMask & (1 << Y_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef Z_DIRECTION_PIN
|
#ifdef Z_DIRECTION_PIN
|
||||||
digitalWrite(Z_DIRECTION_PIN, (onMask & (1 << Z_AXIS)));
|
digitalWrite(Z_DIRECTION_PIN, (onMask & (1 << Z_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef Z2_DIRECTION_PIN // optional ganged axis
|
#ifdef Z2_DIRECTION_PIN // optional ganged axis
|
||||||
digitalWrite(Z2_DIRECTION_PIN, (onMask & (1 << Z_AXIS)));
|
digitalWrite(Z2_DIRECTION_PIN, (onMask & (1 << Z_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef A_DIRECTION_PIN
|
#ifdef A_DIRECTION_PIN
|
||||||
digitalWrite(A_DIRECTION_PIN, (onMask & (1 << A_AXIS)));
|
digitalWrite(A_DIRECTION_PIN, (onMask & (1 << A_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef A2_DIRECTION_PIN // optional ganged axis
|
#ifdef A2_DIRECTION_PIN // optional ganged axis
|
||||||
digitalWrite(A2_DIRECTION_PIN, (onMask & (1 << A_AXIS)));
|
digitalWrite(A2_DIRECTION_PIN, (onMask & (1 << A_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef B_DIRECTION_PIN
|
#ifdef B_DIRECTION_PIN
|
||||||
digitalWrite(B_DIRECTION_PIN, (onMask & (1 << B_AXIS)));
|
digitalWrite(B_DIRECTION_PIN, (onMask & (1 << B_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef B2_DIRECTION_PIN // optional ganged axis
|
#ifdef B2_DIRECTION_PIN // optional ganged axis
|
||||||
digitalWrite(B2_DIRECTION_PIN, (onMask & (1 << B_AXIS)));
|
digitalWrite(B2_DIRECTION_PIN, (onMask & (1 << B_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef C_DIRECTION_PIN
|
#ifdef C_DIRECTION_PIN
|
||||||
digitalWrite(C_DIRECTION_PIN, (onMask & (1 << C_AXIS)));
|
digitalWrite(C_DIRECTION_PIN, (onMask & (1 << C_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
#ifdef C2_DIRECTION_PIN // optional ganged axis
|
#ifdef C2_DIRECTION_PIN // optional ganged axis
|
||||||
digitalWrite(C2_DIRECTION_PIN, (onMask & (1 << C_AXIS)));
|
digitalWrite(C2_DIRECTION_PIN, (onMask & (1 << C_AXIS)));
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
@@ -1213,18 +1216,16 @@ void st_prep_buffer() {
|
|||||||
prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE);
|
prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE);
|
||||||
} else
|
} else
|
||||||
prep.current_speed = sqrt(pl_block->entry_speed_sqr);
|
prep.current_speed = sqrt(pl_block->entry_speed_sqr);
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
// Setup laser mode variables. PWM rate adjusted motions will always complete a motion with the
|
|
||||||
// spindle off.
|
if (spindle->isRateAdjusted() ){ // settings.flags & BITFLAG_LASER_MODE) {
|
||||||
st_prep_block->is_pwm_rate_adjusted = false;
|
|
||||||
if (settings.flags & BITFLAG_LASER_MODE) {
|
|
||||||
if (pl_block->condition & PL_COND_FLAG_SPINDLE_CCW) {
|
if (pl_block->condition & PL_COND_FLAG_SPINDLE_CCW) {
|
||||||
// Pre-compute inverse programmed rate to speed up PWM updating per step segment.
|
// Pre-compute inverse programmed rate to speed up PWM updating per step segment.
|
||||||
prep.inv_rate = 1.0 / pl_block->programmed_rate;
|
prep.inv_rate = 1.0 / pl_block->programmed_rate;
|
||||||
st_prep_block->is_pwm_rate_adjusted = true;
|
st_prep_block->is_pwm_rate_adjusted = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
/* ---------------------------------------------------------------------------------
|
/* ---------------------------------------------------------------------------------
|
||||||
Compute the velocity profile of a new planner block based on its entry and exit
|
Compute the velocity profile of a new planner block based on its entry and exit
|
||||||
@@ -1310,9 +1311,9 @@ void st_prep_buffer() {
|
|||||||
prep.maximum_speed = prep.exit_speed;
|
prep.maximum_speed = prep.exit_speed;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); // Force update whenever updating block.
|
bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); // Force update whenever updating block.
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
// Initialize new segment
|
// Initialize new segment
|
||||||
segment_t* prep_segment = &segment_buffer[segment_buffer_head];
|
segment_t* prep_segment = &segment_buffer[segment_buffer_head];
|
||||||
@@ -1416,29 +1417,32 @@ void st_prep_buffer() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
} while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete.
|
} while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete.
|
||||||
#ifdef VARIABLE_SPINDLE
|
|
||||||
/* -----------------------------------------------------------------------------------
|
/* -----------------------------------------------------------------------------------
|
||||||
Compute spindle speed PWM output for step segment
|
Compute spindle speed PWM output for step segment
|
||||||
*/
|
*/
|
||||||
if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_PWM)) {
|
if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_RPM)) {
|
||||||
if (pl_block->condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)) {
|
if (pl_block->condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)) {
|
||||||
float rpm = pl_block->spindle_speed;
|
float rpm = pl_block->spindle_speed;
|
||||||
// NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate.
|
// NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate.
|
||||||
if (st_prep_block->is_pwm_rate_adjusted)
|
if (st_prep_block->is_pwm_rate_adjusted) {
|
||||||
rpm *= (prep.current_speed * prep.inv_rate);
|
rpm *= (prep.current_speed * prep.inv_rate);
|
||||||
|
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "RPM %.2f", rpm);
|
||||||
|
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Rates CV %.2f IV %.2f RPM %.2f", prep.current_speed, prep.inv_rate, rpm);
|
||||||
|
}
|
||||||
// If current_speed is zero, then may need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE)
|
// If current_speed is zero, then may need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE)
|
||||||
// but this would be instantaneous only and during a motion. May not matter at all.
|
// but this would be instantaneous only and during a motion. May not matter at all.
|
||||||
prep.current_spindle_pwm = spindle_compute_pwm_value(rpm);
|
|
||||||
|
prep.current_spindle_rpm = rpm;
|
||||||
} else {
|
} else {
|
||||||
sys.spindle_speed = 0.0;
|
sys.spindle_speed = 0.0;
|
||||||
#if ( (defined VARIABLE_SPINDLE) && (defined SPINDLE_PWM_PIN) )
|
prep.current_spindle_rpm = 0.0;
|
||||||
prep.current_spindle_pwm = spindle_pwm_off_value ;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM);
|
bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
|
||||||
}
|
}
|
||||||
prep_segment->spindle_pwm = prep.current_spindle_pwm; // Reload segment PWM value
|
prep_segment->spindle_rpm = prep.current_spindle_rpm; // Reload segment PWM value
|
||||||
#endif
|
|
||||||
/* -----------------------------------------------------------------------------------
|
/* -----------------------------------------------------------------------------------
|
||||||
Compute segment step rate, steps to execute, and apply necessary rate corrections.
|
Compute segment step rate, steps to execute, and apply necessary rate corrections.
|
||||||
NOTE: Steps are computed by direct scalar conversion of the millimeter distance
|
NOTE: Steps are computed by direct scalar conversion of the millimeter distance
|
||||||
|
@@ -589,11 +589,13 @@ int8_t sys_get_next_RMT_chan_num() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
This returns an unused pwm channel.
|
This returns an unused pwm channel.
|
||||||
The 8 channels share 4 timers, so pairs 0,1 & 2,3 , etc
|
The 8 channels share 4 timers, so pairs 0,1 & 2,3 , etc
|
||||||
have to be the same frequency. The spindle always uses channel 0
|
have to be the same frequency. The spindle always uses channel 0
|
||||||
so we start counting from 2.
|
so we start counting from 2.
|
||||||
|
|
||||||
There are still possible issues if requested channels use different frequencies
|
There are still possible issues if requested channels use different frequencies
|
||||||
TODO: Make this more robust.
|
TODO: Make this more robust.
|
||||||
*/
|
*/
|
||||||
|
@@ -74,6 +74,7 @@ extern system_t sys;
|
|||||||
#define EXEC_ALARM_HOMING_FAIL_DOOR 7
|
#define EXEC_ALARM_HOMING_FAIL_DOOR 7
|
||||||
#define EXEC_ALARM_HOMING_FAIL_PULLOFF 8
|
#define EXEC_ALARM_HOMING_FAIL_PULLOFF 8
|
||||||
#define EXEC_ALARM_HOMING_FAIL_APPROACH 9
|
#define EXEC_ALARM_HOMING_FAIL_APPROACH 9
|
||||||
|
#define EXEC_ALARM_SPINDLE_CONTROL 10
|
||||||
|
|
||||||
// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides.
|
// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides.
|
||||||
// Spindle/coolant and feed/rapids are separated into two controlling flag variables.
|
// Spindle/coolant and feed/rapids are separated into two controlling flag variables.
|
||||||
@@ -125,7 +126,7 @@ extern system_t sys;
|
|||||||
#define STEP_CONTROL_END_MOTION bit(0)
|
#define STEP_CONTROL_END_MOTION bit(0)
|
||||||
#define STEP_CONTROL_EXECUTE_HOLD bit(1)
|
#define STEP_CONTROL_EXECUTE_HOLD bit(1)
|
||||||
#define STEP_CONTROL_EXECUTE_SYS_MOTION bit(2)
|
#define STEP_CONTROL_EXECUTE_SYS_MOTION bit(2)
|
||||||
#define STEP_CONTROL_UPDATE_SPINDLE_PWM bit(3)
|
#define STEP_CONTROL_UPDATE_SPINDLE_RPM bit(3)
|
||||||
|
|
||||||
// Define control pin index for Grbl internal use. Pin maps may change, but these values don't.
|
// Define control pin index for Grbl internal use. Pin maps may change, but these values don't.
|
||||||
//#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
//#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
|
||||||
@@ -152,8 +153,7 @@ extern system_t sys;
|
|||||||
#define SPINDLE_STOP_OVR_RESTORE bit(2)
|
#define SPINDLE_STOP_OVR_RESTORE bit(2)
|
||||||
#define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3)
|
#define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3)
|
||||||
|
|
||||||
|
#define UNDEFINED_PIN 255 // Can be used to show a pin has no i/O assigned
|
||||||
|
|
||||||
|
|
||||||
// NOTE: These position variables may need to be declared as volatiles, if problems arise.
|
// NOTE: These position variables may need to be declared as volatiles, if problems arise.
|
||||||
extern int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps.
|
extern int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps.
|
||||||
|
121
Grbl_Esp32/tools/BESCSpindle.cpp
Normal file
121
Grbl_Esp32/tools/BESCSpindle.cpp
Normal file
@@ -0,0 +1,121 @@
|
|||||||
|
/*
|
||||||
|
BESCSpindle.cpp
|
||||||
|
|
||||||
|
This a special type of PWM spindle for RC type Brushless DC Speed
|
||||||
|
controllers.
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Important ESC Settings
|
||||||
|
$33=50 // Hz this is the typical good frequency for an ESC
|
||||||
|
#define DEFAULT_SPINDLE_FREQ 5000.0 // $33 Hz (extended set)
|
||||||
|
|
||||||
|
Determine the typical min and max pulse length of your ESC
|
||||||
|
min_pulse is typically 1ms (0.001 sec) or less
|
||||||
|
max_pulse is typically 2ms (0.002 sec) or more
|
||||||
|
|
||||||
|
determine PWM_period. It is (1/freq) if freq = 50...period = 0.02
|
||||||
|
|
||||||
|
determine pulse length for min_pulse and max_pulse in percent.
|
||||||
|
|
||||||
|
(pulse / PWM_period)
|
||||||
|
min_pulse = (0.001 / 0.02) = 0.05 = 5%
|
||||||
|
max_pulse = (0.002 / .02) = 0.1 = 10%
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
|
||||||
|
// don't change these
|
||||||
|
#define BESC_PWM_FREQ 50.0f // Hz
|
||||||
|
#define BESC_PWM_BIT_PRECISION 16 // bits
|
||||||
|
#define BESC_PULSE_PERIOD (1.0 / BESC_PWM_FREQ)
|
||||||
|
// ok to tweak
|
||||||
|
#define BESC_MIN_PULSE_SECS 0.001f
|
||||||
|
#define BESC_MAX_PULSE_SECS 0.002f
|
||||||
|
//don't change
|
||||||
|
#define BESC_MIN_PULSE_CNT (uint16_t)(BESC_MIN_PULSE_SECS / BESC_PULSE_PERIOD * 65535.0)
|
||||||
|
#define BESC_MAX_PULSE_CNT (uint16_t)(BESC_MAX_PULSE_SECS / BESC_PULSE_PERIOD * 65535.0)
|
||||||
|
|
||||||
|
void BESCSpindle :: init() {
|
||||||
|
|
||||||
|
get_pins_and_settings();
|
||||||
|
|
||||||
|
if (_output_pin == UNDEFINED_PIN) {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: BESC output pin not defined");
|
||||||
|
return; // We cannot continue without the output pin
|
||||||
|
}
|
||||||
|
|
||||||
|
// override some settings to what is required for a BESC
|
||||||
|
_pwm_freq = BESC_PWM_FREQ;
|
||||||
|
_pwm_precision = 16;
|
||||||
|
|
||||||
|
// override these settings
|
||||||
|
// to do make these tweakable
|
||||||
|
_pwm_off_value = BESC_MIN_PULSE_CNT;
|
||||||
|
_pwm_min_value = BESC_MIN_PULSE_CNT;
|
||||||
|
_pwm_max_value = BESC_MAX_PULSE_CNT;
|
||||||
|
|
||||||
|
ledcSetup(_spindle_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel
|
||||||
|
ledcAttachPin(_output_pin, _spindle_pwm_chan_num); // attach the PWM to the pin
|
||||||
|
|
||||||
|
if (_enable_pin != UNDEFINED_PIN)
|
||||||
|
pinMode(_enable_pin, OUTPUT);
|
||||||
|
|
||||||
|
set_rpm(0);
|
||||||
|
|
||||||
|
config_message();
|
||||||
|
}
|
||||||
|
|
||||||
|
// prints the startup message of the spindle config
|
||||||
|
void BESCSpindle :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "BESC spindle on Pin:%d", _output_pin);
|
||||||
|
}
|
||||||
|
|
||||||
|
float BESCSpindle::set_rpm(float rpm) {
|
||||||
|
uint32_t pwm_value;
|
||||||
|
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return rpm;
|
||||||
|
|
||||||
|
// apply speed overrides
|
||||||
|
rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value (percent)
|
||||||
|
|
||||||
|
// apply limits limits
|
||||||
|
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
|
||||||
|
rpm = _max_rpm;
|
||||||
|
} else if (rpm != 0.0 && rpm <= _min_rpm) {
|
||||||
|
rpm = _min_rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
sys.spindle_speed = rpm;
|
||||||
|
|
||||||
|
// determine the pwm value
|
||||||
|
if (rpm == 0.0) {
|
||||||
|
pwm_value = _pwm_off_value;
|
||||||
|
} else {
|
||||||
|
pwm_value = (uint16_t)map_float(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
|
||||||
|
set_enable_pin(rpm != 0);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
set_output(pwm_value);
|
||||||
|
return rpm;
|
||||||
|
}
|
110
Grbl_Esp32/tools/DacSpindle.cpp
Normal file
110
Grbl_Esp32/tools/DacSpindle.cpp
Normal file
@@ -0,0 +1,110 @@
|
|||||||
|
/*
|
||||||
|
DacSpindle.cpp
|
||||||
|
|
||||||
|
This uses the Analog DAC in the ESP32 to generate a voltage
|
||||||
|
proportional to the GCode S value desired. Some spindle uses
|
||||||
|
a 0-5V or 0-10V value to control the spindle. You would use
|
||||||
|
an Op Amp type circuit to get from the 0.3.3V of the ESP32 to that voltage.
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
|
||||||
|
// ======================================== DacSpindle ======================================
|
||||||
|
void DacSpindle :: init() {
|
||||||
|
get_pins_and_settings();
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return;
|
||||||
|
|
||||||
|
_min_rpm = settings.rpm_min;
|
||||||
|
_max_rpm = settings.rpm_max;
|
||||||
|
_pwm_min_value = 0; // not actually PWM...DAC counts
|
||||||
|
_pwm_max_value = 255; // not actually PWM...DAC counts
|
||||||
|
_gpio_ok = true;
|
||||||
|
|
||||||
|
if (_output_pin != GPIO_NUM_25 && _output_pin != GPIO_NUM_26) { // DAC can only be used on these pins
|
||||||
|
_gpio_ok = false;
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "DAC spindle pin invalid GPIO_NUM_%d (pin 25 or 26 only)", _output_pin);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_enable_pin != UNDEFINED_PIN)
|
||||||
|
pinMode(_enable_pin, OUTPUT);
|
||||||
|
|
||||||
|
if (_direction_pin != UNDEFINED_PIN) {
|
||||||
|
pinMode(_direction_pin, OUTPUT);
|
||||||
|
}
|
||||||
|
is_reversable = (_direction_pin != UNDEFINED_PIN);
|
||||||
|
|
||||||
|
|
||||||
|
config_message();
|
||||||
|
}
|
||||||
|
|
||||||
|
void DacSpindle :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "DAC spindle on Pin:%d", _output_pin);
|
||||||
|
}
|
||||||
|
|
||||||
|
float DacSpindle::set_rpm(float rpm) {
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return rpm;
|
||||||
|
|
||||||
|
uint32_t pwm_value;
|
||||||
|
|
||||||
|
//grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Set RPM %5.1f", rpm);
|
||||||
|
|
||||||
|
// apply overrides and limits
|
||||||
|
rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value (percent)
|
||||||
|
|
||||||
|
// Calculate PWM register value based on rpm max/min settings and programmed rpm.
|
||||||
|
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
|
||||||
|
// No PWM range possible. Set simple on/off spindle control pin state.
|
||||||
|
sys.spindle_speed = _max_rpm;
|
||||||
|
pwm_value = 255;
|
||||||
|
} else if (rpm <= _min_rpm) {
|
||||||
|
if (rpm == 0.0) { // S0 disables spindle
|
||||||
|
sys.spindle_speed = 0.0;
|
||||||
|
pwm_value = 0;
|
||||||
|
} else { // Set minimum PWM output
|
||||||
|
rpm = _min_rpm;
|
||||||
|
sys.spindle_speed = rpm;
|
||||||
|
pwm_value = 0;
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// Compute intermediate PWM value with linear spindle speed model.
|
||||||
|
// NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight.
|
||||||
|
sys.spindle_speed = rpm;
|
||||||
|
|
||||||
|
pwm_value = (uint32_t)map_float(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
|
||||||
|
set_enable_pin(rpm != 0);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
set_output(pwm_value);
|
||||||
|
|
||||||
|
return rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void DacSpindle :: set_output(uint32_t duty) {
|
||||||
|
if (_gpio_ok) {
|
||||||
|
dacWrite(_output_pin, (uint8_t)duty);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
246
Grbl_Esp32/tools/HuanyangSpindle.cpp
Normal file
246
Grbl_Esp32/tools/HuanyangSpindle.cpp
Normal file
@@ -0,0 +1,246 @@
|
|||||||
|
/*
|
||||||
|
HuanyangSpindle.cpp
|
||||||
|
|
||||||
|
This is for a Huanyang VFD based spindle via RS485 Modbus.
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
// VFD frequencies are in Hz. Multiple by 60 for RPM
|
||||||
|
|
||||||
|
// before using spindle, VFD must be setup for RS485 and match your spindle
|
||||||
|
PD001 2 RS485 Control of run commands
|
||||||
|
PD002 2 RS485 Control of operating frequency
|
||||||
|
PD005 400 Maximum frequency Hz
|
||||||
|
PD011 120 Min Speed (Recommend Aircooled=120 Water=100)
|
||||||
|
PD014 10 Acceleration time (Test to optimize)
|
||||||
|
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)
|
||||||
|
PD163 1 RS485 Address: 1
|
||||||
|
PD164 1 RS485 Baud rate: 9600
|
||||||
|
PD165 3 RS485 Mode: RTU, 8N1
|
||||||
|
|
||||||
|
Some references....
|
||||||
|
Manual: http://www.hy-electrical.com/bf/inverter.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
|
||||||
|
|
||||||
|
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 "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
#include "driver/uart.h"
|
||||||
|
|
||||||
|
#define HUANYANG_ADDR 0x01
|
||||||
|
#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_BAUD_RATE 9600 // PD164 setting
|
||||||
|
#define HUANYANG_BUF_SIZE 127
|
||||||
|
#define RESPONSE_WAIT_TICKS 80 // how long to wait for a response
|
||||||
|
|
||||||
|
void HuanyangSpindle :: init() {
|
||||||
|
|
||||||
|
// fail if numbers are not defined
|
||||||
|
if (!get_pins_and_settings()) {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle errors");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
uart_driver_delete(HUANYANG_UART_PORT); // this allows us to init() more than once if settings have changed.
|
||||||
|
|
||||||
|
uart_config_t uart_config = {
|
||||||
|
.baud_rate = HUANYANG_BAUD_RATE,
|
||||||
|
.data_bits = UART_DATA_8_BITS,
|
||||||
|
.parity = UART_PARITY_DISABLE,
|
||||||
|
.stop_bits = UART_STOP_BITS_1,
|
||||||
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||||
|
.rx_flow_ctrl_thresh = 122,
|
||||||
|
};
|
||||||
|
|
||||||
|
uart_param_config(HUANYANG_UART_PORT, &uart_config);
|
||||||
|
|
||||||
|
uart_set_pin(HUANYANG_UART_PORT,
|
||||||
|
_txd_pin,
|
||||||
|
_rxd_pin,
|
||||||
|
_rts_pin,
|
||||||
|
UART_PIN_NO_CHANGE);
|
||||||
|
|
||||||
|
uart_driver_install(HUANYANG_UART_PORT,
|
||||||
|
HUANYANG_BUF_SIZE * 2,
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
NULL,
|
||||||
|
0);
|
||||||
|
|
||||||
|
uart_set_mode(HUANYANG_UART_PORT, UART_MODE_RS485_HALF_DUPLEX);
|
||||||
|
|
||||||
|
is_reversable = true;
|
||||||
|
|
||||||
|
config_message();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Checks for all the required pin definitions
|
||||||
|
// 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;
|
||||||
|
#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;
|
||||||
|
#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;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return pins_ok;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HuanyangSpindle :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang Spindle Tx:%d Rx:%d RTS:%d", _txd_pin, _rxd_pin, _rts_pin);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
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
|
||||||
|
*/
|
||||||
|
void HuanyangSpindle :: set_state(uint8_t state, float rpm) {
|
||||||
|
if (sys.abort)
|
||||||
|
return; // Block during abort.
|
||||||
|
|
||||||
|
_state = state; // store locally for faster get_state()
|
||||||
|
|
||||||
|
if (!set_mode(state)) { // try to set state. If it fails there is no need to try to set RPM
|
||||||
|
system_set_exec_alarm(EXEC_ALARM_SPINDLE_CONTROL);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (state == SPINDLE_DISABLE) {
|
||||||
|
sys.spindle_speed = 0.0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
set_rpm(rpm);
|
||||||
|
sys.report_ovr_counter = 0; // Set to report change immediately
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HuanyangSpindle :: set_mode(uint8_t mode) {
|
||||||
|
char msg[6] = {HUANYANG_ADDR, 0x03, 0x01, 0x00, 0x00, 0x00};
|
||||||
|
|
||||||
|
if (mode == SPINDLE_ENABLE_CW)
|
||||||
|
msg[3] = 0x01;
|
||||||
|
else if (mode == SPINDLE_ENABLE_CCW)
|
||||||
|
msg[3] = 0x11;
|
||||||
|
else //SPINDLE_DISABLE
|
||||||
|
msg[3] = 0x08;
|
||||||
|
|
||||||
|
add_ModRTU_CRC(msg, sizeof(msg));
|
||||||
|
|
||||||
|
//report_hex_msg(msg, "To VFD:", sizeof(msg)); // TODO for debugging comment out
|
||||||
|
|
||||||
|
uart_write_bytes(HUANYANG_UART_PORT, msg, sizeof(msg));
|
||||||
|
|
||||||
|
return get_response(6);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool HuanyangSpindle :: get_response(uint16_t length) {
|
||||||
|
uint8_t rx_message[10];
|
||||||
|
|
||||||
|
uint16_t read_length = uart_read_bytes(HUANYANG_UART_PORT, rx_message, length, RESPONSE_WAIT_TICKS);
|
||||||
|
if (read_length < length) {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RS485 Unresponsive");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
// check CRC?
|
||||||
|
// Check address?
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
ADDR CMD LEN DATA CRC
|
||||||
|
0x01 0x05 0x02 0x09 0xC4 0xBF 0x0F Write Frequency (0x9C4 = 2500 = 25.00HZ)
|
||||||
|
*/
|
||||||
|
float HuanyangSpindle :: set_rpm(float rpm) {
|
||||||
|
|
||||||
|
// TODO add in all the speed modifiers, like override and linearization
|
||||||
|
|
||||||
|
char msg[7] = {HUANYANG_ADDR, 0x05, 0x02, 0x00, 0x00, 0x00, 0x00};
|
||||||
|
|
||||||
|
// add data (rpm) bytes
|
||||||
|
uint16_t data = uint16_t(rpm / 60.0 * 100.0); // send Hz * 10 (Ex:1500 RPM = 25Hz .... Send 2500)
|
||||||
|
msg[3] = (data & 0xFF00) >> 8;
|
||||||
|
msg[4] = (data & 0xFF);
|
||||||
|
|
||||||
|
add_ModRTU_CRC(msg, sizeof(msg));
|
||||||
|
|
||||||
|
//report_hex_msg(msg, "To VFD:", sizeof(msg)); // TODO for debugging comment out
|
||||||
|
|
||||||
|
uart_write_bytes(HUANYANG_UART_PORT, msg, sizeof(msg));
|
||||||
|
get_response(6);
|
||||||
|
|
||||||
|
return rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HuanyangSpindle ::stop() {
|
||||||
|
set_mode(SPINDLE_DISABLE);
|
||||||
|
}
|
||||||
|
|
||||||
|
// state is cached rather than read right now to prevent delays
|
||||||
|
uint8_t HuanyangSpindle :: get_state() {
|
||||||
|
return _state;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculate the CRC on all of the byte except the last 2
|
||||||
|
// It then added the CRC to those last 2 bytes
|
||||||
|
// full_msg_len This is the length of the message including the 2 crc bytes
|
||||||
|
// Source: https://ctlsys.com/support/how_to_compute_the_modbus_rtu_message_crc/
|
||||||
|
void HuanyangSpindle :: add_ModRTU_CRC(char* buf, int full_msg_len) {
|
||||||
|
uint16_t crc = 0xFFFF;
|
||||||
|
for (int pos = 0; pos < full_msg_len - 2; pos++) {
|
||||||
|
crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc
|
||||||
|
for (int i = 8; i != 0; i--) { // Loop over each bit
|
||||||
|
if ((crc & 0x0001) != 0) { // If the LSB is set
|
||||||
|
crc >>= 1; // Shift right and XOR 0xA001
|
||||||
|
crc ^= 0xA001;
|
||||||
|
} else // Else LSB is not set
|
||||||
|
crc >>= 1; // Just shift right
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// add the calculated Crc to the message
|
||||||
|
buf[full_msg_len - 1] = (crc & 0xFF00) >> 8;
|
||||||
|
buf[full_msg_len - 2] = (crc & 0xFF);
|
||||||
|
}
|
41
Grbl_Esp32/tools/Laser.cpp
Normal file
41
Grbl_Esp32/tools/Laser.cpp
Normal file
@@ -0,0 +1,41 @@
|
|||||||
|
/*
|
||||||
|
Laser.cpp
|
||||||
|
|
||||||
|
This is similar the the PWM Spindle except that it allows the
|
||||||
|
M4 speed vs. power copensation.
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
|
||||||
|
// ===================================== Laser ==============================================
|
||||||
|
|
||||||
|
|
||||||
|
bool Laser :: isRateAdjusted() {
|
||||||
|
return true; // can use M4 (CCW) laser mode.
|
||||||
|
}
|
||||||
|
|
||||||
|
void Laser :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL,
|
||||||
|
MSG_LEVEL_INFO,
|
||||||
|
"Laser spindle on GPIO:%d, Freq:%.2fHz, Res:%dbits Laser mode:$32=%d",
|
||||||
|
_output_pin,
|
||||||
|
_pwm_freq,
|
||||||
|
_pwm_precision,
|
||||||
|
isRateAdjusted()); // the current mode
|
||||||
|
}
|
41
Grbl_Esp32/tools/NullSpindle.cpp
Normal file
41
Grbl_Esp32/tools/NullSpindle.cpp
Normal file
@@ -0,0 +1,41 @@
|
|||||||
|
/*
|
||||||
|
NullSpindle.cpp
|
||||||
|
|
||||||
|
This is used when you don't want to use a spindle. No I/O will be used
|
||||||
|
and most methods don't do anything
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
*/
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
|
||||||
|
// ======================= NullSpindle ==============================
|
||||||
|
// NullSpindle is just bunch of do nothing (ignore) methods to be used when you don't want a spindle
|
||||||
|
void NullSpindle :: init() {
|
||||||
|
is_reversable = false;
|
||||||
|
config_message();
|
||||||
|
}
|
||||||
|
float NullSpindle :: set_rpm(float rpm) {
|
||||||
|
return rpm;
|
||||||
|
}
|
||||||
|
void NullSpindle :: set_state(uint8_t state, float rpm) {}
|
||||||
|
uint8_t NullSpindle :: get_state() {
|
||||||
|
return (SPINDLE_STATE_DISABLE);
|
||||||
|
}
|
||||||
|
void NullSpindle :: stop() {}
|
||||||
|
void NullSpindle :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "No spindle");
|
||||||
|
}
|
228
Grbl_Esp32/tools/PWMSpindle.cpp
Normal file
228
Grbl_Esp32/tools/PWMSpindle.cpp
Normal file
@@ -0,0 +1,228 @@
|
|||||||
|
/*
|
||||||
|
PWMSpindle.cpp
|
||||||
|
|
||||||
|
This is a full featured TTL PWM spindle. This does not include speed/power
|
||||||
|
compensation. Use the Laser class for that.
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
*/
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
|
||||||
|
// ======================= PWMSpindle ==============================
|
||||||
|
/*
|
||||||
|
This gets called at startup or whenever a spindle setting changes
|
||||||
|
If the spindle is running it will stop and need to be restarted with M3Snnnn
|
||||||
|
*/
|
||||||
|
void PWMSpindle::init() {
|
||||||
|
|
||||||
|
get_pins_and_settings();
|
||||||
|
|
||||||
|
if (_output_pin == UNDEFINED_PIN) {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: Spindle output pin not defined");
|
||||||
|
return; // We cannot continue without the output pin
|
||||||
|
}
|
||||||
|
|
||||||
|
ledcSetup(_spindle_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel
|
||||||
|
ledcAttachPin(_output_pin, _spindle_pwm_chan_num); // attach the PWM to the pin
|
||||||
|
|
||||||
|
if (_enable_pin != UNDEFINED_PIN)
|
||||||
|
pinMode(_enable_pin, OUTPUT);
|
||||||
|
|
||||||
|
if (_direction_pin != UNDEFINED_PIN)
|
||||||
|
pinMode(_direction_pin, OUTPUT);
|
||||||
|
|
||||||
|
config_message();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the GPIO from the machine definition
|
||||||
|
void PWMSpindle :: get_pins_and_settings() {
|
||||||
|
// setup all the pins
|
||||||
|
|
||||||
|
#ifdef SPINDLE_OUTPUT_PIN
|
||||||
|
_output_pin = SPINDLE_OUTPUT_PIN;
|
||||||
|
#else
|
||||||
|
_output_pin = UNDEFINED_PIN;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef SPINDLE_ENABLE_PIN
|
||||||
|
_enable_pin = SPINDLE_ENABLE_PIN;
|
||||||
|
#else
|
||||||
|
_enable_pin = UNDEFINED_PIN;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef SPINDLE_DIR_PIN
|
||||||
|
_direction_pin = SPINDLE_DIR_PIN;
|
||||||
|
#else
|
||||||
|
_direction_pin = UNDEFINED_PIN;
|
||||||
|
#endif
|
||||||
|
is_reversable = (_direction_pin != UNDEFINED_PIN);
|
||||||
|
|
||||||
|
_pwm_freq = settings.spindle_pwm_freq;
|
||||||
|
_pwm_precision = calc_pwm_precision(_pwm_freq); // detewrmine the best precision
|
||||||
|
_pwm_period = (1 << _pwm_precision);
|
||||||
|
|
||||||
|
if (settings.spindle_pwm_min_value > settings.spindle_pwm_min_value)
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: Spindle min pwm is greater than max. Check $35 and $36");
|
||||||
|
|
||||||
|
// pre-caculate some PWM count values
|
||||||
|
_pwm_off_value = (_pwm_period * settings.spindle_pwm_off_value / 100.0);
|
||||||
|
_pwm_min_value = (_pwm_period * settings.spindle_pwm_min_value / 100.0);
|
||||||
|
_pwm_max_value = (_pwm_period * settings.spindle_pwm_max_value / 100.0);
|
||||||
|
|
||||||
|
#ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE
|
||||||
|
_min_rpm = RPM_MIN;
|
||||||
|
_max_rpm = RPM_MAX;
|
||||||
|
#else
|
||||||
|
_min_rpm = settings.rpm_min;
|
||||||
|
_max_rpm = settings.rpm_max;
|
||||||
|
#endif
|
||||||
|
// The pwm_gradient is the pwm duty cycle units per rpm
|
||||||
|
_pwm_gradient = (_pwm_max_value - _pwm_min_value) / (_max_rpm - _min_rpm);
|
||||||
|
|
||||||
|
_spindle_pwm_chan_num = 0; // Channel 0 is reserved for spindle use
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
float PWMSpindle::set_rpm(float rpm) {
|
||||||
|
uint32_t pwm_value;
|
||||||
|
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return rpm;
|
||||||
|
|
||||||
|
// apply override
|
||||||
|
rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value (percent)
|
||||||
|
|
||||||
|
// apply limits
|
||||||
|
if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) {
|
||||||
|
rpm = _max_rpm;
|
||||||
|
} else if (rpm != 0.0 && rpm <= _min_rpm) {
|
||||||
|
rpm = _min_rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
sys.spindle_speed = rpm;
|
||||||
|
|
||||||
|
#ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE
|
||||||
|
pwm_value = piecewise_linear_fit(rpm);
|
||||||
|
#else
|
||||||
|
// Calculate PWM register value based on rpm max/min settings and programmed rpm.
|
||||||
|
if (rpm == 0.0) {
|
||||||
|
pwm_value = _pwm_off_value;
|
||||||
|
} else {
|
||||||
|
pwm_value = (uint16_t)map_float(rpm, _min_rpm, _max_rpm, _pwm_min_value, _pwm_max_value);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
|
||||||
|
set_enable_pin(rpm != 0);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
set_output(pwm_value);
|
||||||
|
|
||||||
|
return rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PWMSpindle::set_state(uint8_t state, float rpm) {
|
||||||
|
if (sys.abort)
|
||||||
|
return; // Block during abort.
|
||||||
|
|
||||||
|
if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm.
|
||||||
|
sys.spindle_speed = 0.0;
|
||||||
|
stop();
|
||||||
|
} else {
|
||||||
|
set_spindle_dir_pin(state == SPINDLE_ENABLE_CW);
|
||||||
|
set_enable_pin(true);
|
||||||
|
set_rpm(rpm);
|
||||||
|
}
|
||||||
|
sys.report_ovr_counter = 0; // Set to report change immediately
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t PWMSpindle::get_state() {
|
||||||
|
|
||||||
|
|
||||||
|
if (_current_pwm_duty == 0 || _output_pin == UNDEFINED_PIN)
|
||||||
|
return (SPINDLE_STATE_DISABLE);
|
||||||
|
else {
|
||||||
|
if (_direction_pin != UNDEFINED_PIN) {
|
||||||
|
if (digitalRead(_direction_pin))
|
||||||
|
return (SPINDLE_STATE_CW);
|
||||||
|
else
|
||||||
|
return (SPINDLE_STATE_CCW);
|
||||||
|
} else
|
||||||
|
return (SPINDLE_STATE_CW);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void PWMSpindle::stop() {
|
||||||
|
// inverts are delt with in methods
|
||||||
|
set_enable_pin(false);
|
||||||
|
set_output(_pwm_off_value);
|
||||||
|
}
|
||||||
|
|
||||||
|
// prints the startup message of the spindle config
|
||||||
|
void PWMSpindle :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "PWM spindle on Pin:%d, Freq:%.2fHz, Res:%dbits", _output_pin, _pwm_freq, _pwm_precision);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void PWMSpindle::set_output(uint32_t duty) {
|
||||||
|
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return;
|
||||||
|
|
||||||
|
// to prevent excessive calls to ledcWrite, make sure duty hass changed
|
||||||
|
if (duty == _current_pwm_duty)
|
||||||
|
return;
|
||||||
|
|
||||||
|
_current_pwm_duty = duty;
|
||||||
|
|
||||||
|
#ifdef INVERT_SPINDLE_PWM
|
||||||
|
duty = (1 << settings.spindle_pwm_precision_bits) - duty;
|
||||||
|
#endif
|
||||||
|
ledcWrite(_spindle_pwm_chan_num, duty);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PWMSpindle::set_enable_pin(bool enable) {
|
||||||
|
if (_enable_pin == UNDEFINED_PIN)
|
||||||
|
return;
|
||||||
|
#ifndef INVERT_SPINDLE_ENABLE_PIN
|
||||||
|
digitalWrite(_enable_pin, enable);
|
||||||
|
#else
|
||||||
|
digitalWrite(_enable_pin, !enable);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void PWMSpindle::set_spindle_dir_pin(bool Clockwise) {
|
||||||
|
if (_direction_pin != UNDEFINED_PIN)
|
||||||
|
digitalWrite(_direction_pin, Clockwise);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
Calculate the highest precision of a PWM based on the frequency in bits
|
||||||
|
|
||||||
|
80,000,000 / freq = period
|
||||||
|
determine the highest precision where (1 << precision) < period
|
||||||
|
*/
|
||||||
|
uint8_t PWMSpindle :: calc_pwm_precision(float freq) {
|
||||||
|
uint8_t precision = 0;
|
||||||
|
|
||||||
|
while ((1 << precision) < (uint32_t)(80000000.0 / freq) && precision <= 16)
|
||||||
|
precision++;
|
||||||
|
|
||||||
|
return precision - 1;
|
||||||
|
}
|
75
Grbl_Esp32/tools/RelaySpindle.cpp
Normal file
75
Grbl_Esp32/tools/RelaySpindle.cpp
Normal file
@@ -0,0 +1,75 @@
|
|||||||
|
/*
|
||||||
|
RelaySpindle.cpp
|
||||||
|
|
||||||
|
This is used for a basic on/off spindle. All S Values about 1
|
||||||
|
will turn the spindle on.
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
*/
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
|
||||||
|
// ========================= RelaySpindle ==================================
|
||||||
|
/*
|
||||||
|
This is the same as a PWM spindle, but is a digital rather than PWM output
|
||||||
|
*/
|
||||||
|
void RelaySpindle::init() {
|
||||||
|
get_pins_and_settings();
|
||||||
|
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return;
|
||||||
|
|
||||||
|
pinMode(_output_pin, OUTPUT);
|
||||||
|
|
||||||
|
if (_enable_pin != UNDEFINED_PIN)
|
||||||
|
pinMode(_enable_pin, OUTPUT);
|
||||||
|
|
||||||
|
if (_direction_pin != UNDEFINED_PIN)
|
||||||
|
pinMode(_direction_pin, OUTPUT);
|
||||||
|
|
||||||
|
is_reversable = (_direction_pin != UNDEFINED_PIN);
|
||||||
|
|
||||||
|
config_message();
|
||||||
|
}
|
||||||
|
|
||||||
|
// prints the startup message of the spindle config
|
||||||
|
void RelaySpindle :: config_message() {
|
||||||
|
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Relay spindle on Pin:%d", _output_pin);
|
||||||
|
}
|
||||||
|
|
||||||
|
float RelaySpindle::set_rpm(float rpm) {
|
||||||
|
if (_output_pin == UNDEFINED_PIN)
|
||||||
|
return rpm;
|
||||||
|
|
||||||
|
sys.spindle_speed = rpm;
|
||||||
|
|
||||||
|
if (rpm == 0) {
|
||||||
|
sys.spindle_speed = 0.0;
|
||||||
|
set_output(0);
|
||||||
|
} else {
|
||||||
|
sys.spindle_speed = rpm;
|
||||||
|
set_output(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
return rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RelaySpindle::set_output(uint32_t duty) {
|
||||||
|
#ifdef INVERT_SPINDLE_PWM
|
||||||
|
duty = (duty == 0); // flip duty
|
||||||
|
#endif
|
||||||
|
digitalWrite(_output_pin, duty > 0); // anything greater
|
||||||
|
}
|
99
Grbl_Esp32/tools/SpindleClass.cpp
Normal file
99
Grbl_Esp32/tools/SpindleClass.cpp
Normal file
@@ -0,0 +1,99 @@
|
|||||||
|
/*
|
||||||
|
SpindleClass.cpp
|
||||||
|
|
||||||
|
A Spindle Class
|
||||||
|
Spindle - A base class. Do not use
|
||||||
|
PWMSpindle - A spindle with a PWM output
|
||||||
|
RelaySpindle - An on/off only spindle
|
||||||
|
Laser - Output is PWM, but the M4 laser power mode can be used
|
||||||
|
DacSpindle - Uses the DAC to output a 0-3.3V output
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
|
||||||
|
2020 - Bart Dring
|
||||||
|
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
TODO
|
||||||
|
Consider breaking into one file per class.
|
||||||
|
|
||||||
|
Get rid of dependance on machine definition #defines
|
||||||
|
SPINDLE_OUTPUT_PIN
|
||||||
|
SPINDLE_ENABLE_PIN
|
||||||
|
SPINDLE_DIR_PIN
|
||||||
|
|
||||||
|
*/
|
||||||
|
#include "grbl.h"
|
||||||
|
#include "SpindleClass.h"
|
||||||
|
#include "NullSpindle.cpp"
|
||||||
|
#include "PWMSpindle.cpp"
|
||||||
|
#include "DacSpindle.cpp"
|
||||||
|
#include "RelaySpindle.cpp"
|
||||||
|
#include "Laser.cpp"
|
||||||
|
#include "HuanyangSpindle.cpp"
|
||||||
|
#include "BESCSpindle.cpp"
|
||||||
|
|
||||||
|
NullSpindle null_spindle;
|
||||||
|
PWMSpindle pwm_spindle;
|
||||||
|
RelaySpindle relay_spindle;
|
||||||
|
Laser laser;
|
||||||
|
DacSpindle dac_spindle;
|
||||||
|
HuanyangSpindle huanyang_spindle;
|
||||||
|
BESCSpindle besc_spindle;
|
||||||
|
|
||||||
|
void spindle_select(uint8_t spindle_type) {
|
||||||
|
|
||||||
|
switch (spindle_type) {
|
||||||
|
case SPINDLE_TYPE_PWM:
|
||||||
|
spindle = &pwm_spindle;
|
||||||
|
break;
|
||||||
|
case SPINDLE_TYPE_RELAY:
|
||||||
|
spindle = &relay_spindle;
|
||||||
|
break;
|
||||||
|
case SPINDLE_TYPE_LASER:
|
||||||
|
spindle = &laser;
|
||||||
|
break;
|
||||||
|
case SPINDLE_TYPE_DAC:
|
||||||
|
spindle = &dac_spindle;
|
||||||
|
break;
|
||||||
|
case SPINDLE_TYPE_HUANYANG:
|
||||||
|
spindle = &huanyang_spindle;
|
||||||
|
break;
|
||||||
|
case SPINDLE_TYPE_BESC:
|
||||||
|
spindle = &besc_spindle;
|
||||||
|
break;
|
||||||
|
case SPINDLE_TYPE_NONE:
|
||||||
|
default:
|
||||||
|
spindle = &null_spindle;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
spindle->init();
|
||||||
|
}
|
||||||
|
|
||||||
|
void spindle_read_prefs(Preferences& prefs) {
|
||||||
|
uint8_t foo = prefs.getUChar("SPIN_TYPE", SPINDLE_TYPE_PWM);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
bool Spindle::isRateAdjusted() {
|
||||||
|
return false; // default for basic spindles is false
|
||||||
|
}
|
||||||
|
|
||||||
|
void Spindle :: spindle_sync(uint8_t state, float rpm) {
|
||||||
|
if (sys.state == STATE_CHECK_MODE)
|
||||||
|
return;
|
||||||
|
protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
|
||||||
|
set_state(state, rpm);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
181
Grbl_Esp32/tools/SpindleClass.h
Normal file
181
Grbl_Esp32/tools/SpindleClass.h
Normal file
@@ -0,0 +1,181 @@
|
|||||||
|
/*
|
||||||
|
SpindleClass.h
|
||||||
|
|
||||||
|
Header file for a Spindle Class
|
||||||
|
See SpindleClass.cpp for more details
|
||||||
|
|
||||||
|
Part of Grbl_ESP32
|
||||||
|
|
||||||
|
2020 - 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 <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define SPINDLE_STATE_DISABLE 0 // Must be zero.
|
||||||
|
#define SPINDLE_STATE_CW bit(0)
|
||||||
|
#define SPINDLE_STATE_CCW bit(1)
|
||||||
|
|
||||||
|
|
||||||
|
#define SPINDLE_TYPE_NONE 0
|
||||||
|
#define SPINDLE_TYPE_PWM 1
|
||||||
|
#define SPINDLE_TYPE_RELAY 2
|
||||||
|
#define SPINDLE_TYPE_LASER 3
|
||||||
|
#define SPINDLE_TYPE_DAC 4
|
||||||
|
#define SPINDLE_TYPE_HUANYANG 5
|
||||||
|
#define SPINDLE_TYPE_BESC 6
|
||||||
|
|
||||||
|
#ifndef SPINDLE_CLASS_H
|
||||||
|
#define SPINDLE_CLASS_H
|
||||||
|
|
||||||
|
#include "grbl.h"
|
||||||
|
#include <driver/dac.h>
|
||||||
|
#include "driver/uart.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// This is the base class. Do not use this as your spindle
|
||||||
|
class Spindle {
|
||||||
|
public:
|
||||||
|
virtual void init(); // not in constructor because this also gets called when $$ settings change
|
||||||
|
virtual float set_rpm(float rpm);
|
||||||
|
virtual void set_state(uint8_t state, float rpm);
|
||||||
|
virtual uint8_t get_state();
|
||||||
|
virtual void stop();
|
||||||
|
virtual void config_message();
|
||||||
|
virtual bool isRateAdjusted();
|
||||||
|
virtual void spindle_sync(uint8_t state, float rpm);
|
||||||
|
|
||||||
|
bool is_reversable;
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is a dummy spindle that has no I/O.
|
||||||
|
// It is used to ignore spindle commands when no spinde is desired
|
||||||
|
class NullSpindle : public Spindle {
|
||||||
|
public:
|
||||||
|
void init();
|
||||||
|
float set_rpm(float rpm);
|
||||||
|
void set_state(uint8_t state, float rpm);
|
||||||
|
uint8_t get_state();
|
||||||
|
void stop();
|
||||||
|
void config_message();
|
||||||
|
};
|
||||||
|
|
||||||
|
// This adds support for PWM
|
||||||
|
class PWMSpindle : public Spindle {
|
||||||
|
public:
|
||||||
|
void init();
|
||||||
|
virtual float set_rpm(float rpm);
|
||||||
|
void set_state(uint8_t state, float rpm);
|
||||||
|
uint8_t get_state();
|
||||||
|
void stop();
|
||||||
|
void config_message();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
int32_t _current_pwm_duty;
|
||||||
|
void set_spindle_dir_pin(bool Clockwise);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
float _min_rpm;
|
||||||
|
float _max_rpm;
|
||||||
|
uint32_t _pwm_off_value;
|
||||||
|
uint32_t _pwm_min_value;
|
||||||
|
uint32_t _pwm_max_value;
|
||||||
|
uint8_t _output_pin;
|
||||||
|
uint8_t _enable_pin;
|
||||||
|
uint8_t _direction_pin;
|
||||||
|
int8_t _spindle_pwm_chan_num;
|
||||||
|
float _pwm_freq;
|
||||||
|
uint32_t _pwm_period; // how many counts in 1 period
|
||||||
|
uint8_t _pwm_precision;
|
||||||
|
float _pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
|
||||||
|
|
||||||
|
virtual void set_output(uint32_t duty);
|
||||||
|
void set_enable_pin(bool enable_pin);
|
||||||
|
void get_pins_and_settings();
|
||||||
|
uint8_t calc_pwm_precision(float freq);
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is for an on/off spindle all RPMs above 0 are on
|
||||||
|
class RelaySpindle : public PWMSpindle {
|
||||||
|
public:
|
||||||
|
void init();
|
||||||
|
void config_message();
|
||||||
|
float set_rpm(float rpm);
|
||||||
|
protected:
|
||||||
|
void set_output(uint32_t duty);
|
||||||
|
};
|
||||||
|
|
||||||
|
// this is the same as a PWM spindle, but the M4 compensation is supported.
|
||||||
|
class Laser : public PWMSpindle {
|
||||||
|
public:
|
||||||
|
bool isRateAdjusted();
|
||||||
|
void config_message();
|
||||||
|
};
|
||||||
|
|
||||||
|
// This uses one of the (2) DAC pins on ESP32 to output a voltage
|
||||||
|
class DacSpindle : public PWMSpindle {
|
||||||
|
public:
|
||||||
|
void init();
|
||||||
|
void config_message();
|
||||||
|
float set_rpm(float rpm);
|
||||||
|
private:
|
||||||
|
bool _gpio_ok; // DAC is on a valid pin
|
||||||
|
protected:
|
||||||
|
void set_output(uint32_t duty); // sets DAC instead of PWM
|
||||||
|
};
|
||||||
|
|
||||||
|
class HuanyangSpindle : public Spindle {
|
||||||
|
public:
|
||||||
|
void init();
|
||||||
|
void config_message();
|
||||||
|
virtual void set_state(uint8_t state, float rpm);
|
||||||
|
uint8_t get_state();
|
||||||
|
float set_rpm(float rpm);
|
||||||
|
void stop();
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool get_response(uint16_t length);
|
||||||
|
uint16_t ModRTU_CRC(char* buf, int len);
|
||||||
|
void add_ModRTU_CRC(char* buf, int full_msg_len);
|
||||||
|
bool set_mode(uint8_t mode);
|
||||||
|
bool get_pins_and_settings();
|
||||||
|
|
||||||
|
uint8_t _txd_pin;
|
||||||
|
uint8_t _rxd_pin;
|
||||||
|
uint8_t _rts_pin;
|
||||||
|
uint8_t _state;
|
||||||
|
};
|
||||||
|
|
||||||
|
class BESCSpindle : public PWMSpindle {
|
||||||
|
public:
|
||||||
|
void init();
|
||||||
|
void config_message();
|
||||||
|
float set_rpm(float rpm);
|
||||||
|
};
|
||||||
|
|
||||||
|
extern Spindle* spindle;
|
||||||
|
|
||||||
|
extern NullSpindle null_spindle;
|
||||||
|
extern PWMSpindle pwm_spindle;
|
||||||
|
extern RelaySpindle relay_spindle;
|
||||||
|
extern Laser laser;
|
||||||
|
extern DacSpindle dac_spindle;
|
||||||
|
extern HuanyangSpindle huanyang_spindle;
|
||||||
|
extern BESCSpindle besc_spindle;
|
||||||
|
|
||||||
|
void spindle_select(uint8_t spindle_type);
|
||||||
|
void spindle_read_prefs(Preferences& prefs);
|
||||||
|
|
||||||
|
#endif
|
@@ -7,26 +7,16 @@ $env:PYTHONIOENCODING="utf-8"
|
|||||||
|
|
||||||
Function BuildMachine($names) {
|
Function BuildMachine($names) {
|
||||||
$basename = $names[0]
|
$basename = $names[0]
|
||||||
$addname = $names[1]
|
|
||||||
$env:PLATFORMIO_BUILD_FLAGS = "-DMACHINE_FILENAME=$basename"
|
$env:PLATFORMIO_BUILD_FLAGS = "-DMACHINE_FILENAME=$basename"
|
||||||
$displayname = $basename
|
$displayname = $basename
|
||||||
if ($addname -ne "") {
|
|
||||||
$env:PLATFORMIO_BUILD_FLAGS += " -DMACHINE_FILENAME2=$addname"
|
|
||||||
$displayname += " + $addname"
|
|
||||||
}
|
|
||||||
Write-Output "Building machine $displayname"
|
Write-Output "Building machine $displayname"
|
||||||
platformio run 2>&1 | Select-String error,Took
|
platformio run 2>&1 | Select-String error,Took
|
||||||
Write-Output " "
|
Write-Output " "
|
||||||
}
|
}
|
||||||
|
|
||||||
# First build all the base configurations with names that do not start with add_
|
# Build all the machines
|
||||||
foreach ($filepath in Get-ChildItem -file .\Grbl_Esp32\Machines\* -Exclude add_*) {
|
foreach ($filepath in Get-ChildItem -file .\Grbl_Esp32\Machines\*) {
|
||||||
BuildMachine($filepath.name, "")
|
BuildMachine($filepath.name, "")
|
||||||
}
|
}
|
||||||
|
|
||||||
# Then build all of the add-ons on top of a single base
|
|
||||||
$base="3axis_v4.h"
|
|
||||||
foreach ($filepath in Get-ChildItem -file .\Grbl_Esp32\Machines\add_*) {
|
|
||||||
BuildMachine($base, $filepath.name)
|
|
||||||
}
|
|
||||||
Remove-Item env:PLATFORMIO_BUILD_FLAGS
|
Remove-Item env:PLATFORMIO_BUILD_FLAGS
|
||||||
|
@@ -22,12 +22,8 @@ cmd = ['platformio','run']
|
|||||||
verbose = '-v' in sys.argv
|
verbose = '-v' in sys.argv
|
||||||
|
|
||||||
numErrors = 0
|
numErrors = 0
|
||||||
adderBase = '3axis_v4.h'
|
|
||||||
for name in os.listdir('Grbl_Esp32/Machines'):
|
for name in os.listdir('Grbl_Esp32/Machines'):
|
||||||
if name.startswith('add_'):
|
exitCode = buildMachine(name, verbose=verbose)
|
||||||
exitCode = buildMachine(adderBase, addName=name, verbose=verbose)
|
|
||||||
else:
|
|
||||||
exitCode = buildMachine(name, verbose=verbose)
|
|
||||||
if exitCode != 0:
|
if exitCode != 0:
|
||||||
numErrors += 1
|
numErrors += 1
|
||||||
|
|
||||||
|
18
build-all.sh
18
build-all.sh
@@ -17,14 +17,8 @@ NUM_ERRORS=0
|
|||||||
|
|
||||||
BuildMachine () {
|
BuildMachine () {
|
||||||
basename=$1
|
basename=$1
|
||||||
addname=$2
|
|
||||||
BF="\"-DMACHINE_FILENAME=$basename\""
|
BF="\"-DMACHINE_FILENAME=$basename\""
|
||||||
displayname=$basename
|
displayname=$basename
|
||||||
if [ "$addname" != "" ]
|
|
||||||
then
|
|
||||||
BF="$BF \"-DMACHINE_FILENAME2=$addname\""
|
|
||||||
displayname="$basename + $addname"
|
|
||||||
fi
|
|
||||||
echo "Building machine $displayname"
|
echo "Building machine $displayname"
|
||||||
PLATFORMIO_BUILD_FLAGS=$BF platformio run 2>&1 | $FILTER
|
PLATFORMIO_BUILD_FLAGS=$BF platformio run 2>&1 | $FILTER
|
||||||
local re=$?
|
local re=$?
|
||||||
@@ -38,18 +32,10 @@ BuildMachine () {
|
|||||||
echo
|
echo
|
||||||
}
|
}
|
||||||
|
|
||||||
# First build all the base configurations with names that do not start with add_
|
# Build all the machines
|
||||||
for file in `ls ./Grbl_Esp32/Machines/* | grep -v add_\*`; do
|
for file in `ls ./Grbl_Esp32/Machines/*`; do
|
||||||
base=`basename $file`
|
base=`basename $file`
|
||||||
BuildMachine $base ""
|
BuildMachine $base ""
|
||||||
done
|
done
|
||||||
|
|
||||||
# Then build all of the add-ons on top of a single base
|
|
||||||
base="3axis_v4.h"
|
|
||||||
for file in `ls ./Grbl_Esp32/Machines/add_*`
|
|
||||||
do
|
|
||||||
adder=`basename $file`
|
|
||||||
BuildMachine $base $adder
|
|
||||||
done
|
|
||||||
|
|
||||||
exit $NUM_ERRORS
|
exit $NUM_ERRORS
|
||||||
|
@@ -26,13 +26,11 @@ if '-u' in sys.argv:
|
|||||||
|
|
||||||
exitCode = 255
|
exitCode = 255
|
||||||
if len(sys.argv) == 2:
|
if len(sys.argv) == 2:
|
||||||
exitCode = buildMachine(sys.argv[1], addName=None, verbose=verbose, extraArgs=extraArgs)
|
exitCode = buildMachine(sys.argv[1], verbose=verbose, extraArgs=extraArgs)
|
||||||
elif len(sys.argv) == 3:
|
|
||||||
exitCode = buildMachine(sys.argv[1], addName=sys.argv[2], verbose=verbose, extraArgs=extraArgs)
|
|
||||||
else:
|
else:
|
||||||
print("Usage: ./build-machine.py [-q] [-u] machine_name.h [add_name.h]")
|
print("Usage: ./build-machine.py [-q] [-u] machine_name.h")
|
||||||
print(' Build for the given machine and optional add-on regardless of machine.h')
|
print(' Build for the given machine regardless of machine.h')
|
||||||
print(' -q suppresses most messages')
|
print(' -q suppresses most messages')
|
||||||
print(' -u uploads to the target after compilation')
|
print(' -u uploads to the target after compilation')
|
||||||
|
|
||||||
sys.exit(exitCode)
|
sys.exit(exitCode)
|
||||||
|
16
builder.py
16
builder.py
@@ -1,24 +1,20 @@
|
|||||||
# This script is imported by build-machine.py and build-all.py
|
# This script is imported by build-machine.py and build-all.py
|
||||||
# It performs a platformio build with a given base machine file
|
# It performs a platformio build with a given machine file.
|
||||||
# and an optional add-on file. The verbose argument controls
|
# The verbose argument controls whether the full output is
|
||||||
# whether the full output is displayed, or filtered to show
|
# displayed, or filtered to show only summary information.
|
||||||
# only summary information. extraArgs can be used to perform
|
# extraArgs can be used to perform uploading after compilation.
|
||||||
# uploading after compilation.
|
|
||||||
|
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
import subprocess, os
|
import subprocess, os
|
||||||
|
|
||||||
env = dict(os.environ)
|
env = dict(os.environ)
|
||||||
|
|
||||||
def buildMachine(baseName, addName=None, verbose=True, extraArgs=None):
|
def buildMachine(baseName, verbose=True, extraArgs=None):
|
||||||
cmd = ['platformio','run']
|
cmd = ['platformio','run']
|
||||||
if extraArgs:
|
if extraArgs:
|
||||||
cmd.append(extraArgs)
|
cmd.append(extraArgs)
|
||||||
displayName = baseName
|
displayName = baseName
|
||||||
flags = '-DMACHINE_FILENAME=' + baseName
|
flags = '-DMACHINE_FILENAME=' + baseName
|
||||||
if addName:
|
|
||||||
displayName += ' + ' + addName
|
|
||||||
flags += ' -DMACHINE_FILENAME2=' + addName
|
|
||||||
print('Building machine ' + displayName)
|
print('Building machine ' + displayName)
|
||||||
env['PLATFORMIO_BUILD_FLAGS'] = flags
|
env['PLATFORMIO_BUILD_FLAGS'] = flags
|
||||||
if verbose:
|
if verbose:
|
||||||
@@ -30,4 +26,4 @@ def buildMachine(baseName, addName=None, verbose=True, extraArgs=None):
|
|||||||
print(line, end='')
|
print(line, end='')
|
||||||
app.wait()
|
app.wait()
|
||||||
print()
|
print()
|
||||||
return app.returncode
|
return app.returncode
|
||||||
|
Reference in New Issue
Block a user