mirror of
https://github.com/bdring/Grbl_Esp32.git
synced 2025-09-09 13:50:51 +02:00
refactoring
This commit is contained in:
@@ -1,59 +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
|
||||
|
||||
/*
|
||||
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
|
@@ -50,8 +50,10 @@
|
||||
//#define USE_SPINDLE_RELAY
|
||||
|
||||
#ifdef USE_SPINDLE_RELAY
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2
|
||||
#else
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_16
|
||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
||||
#endif
|
||||
|
@@ -74,6 +74,8 @@
|
||||
|
||||
#define SERVO_PEN_PIN GPIO_NUM_27
|
||||
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||
|
||||
// defaults
|
||||
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on
|
||||
|
@@ -50,8 +50,10 @@
|
||||
//#define USE_SPINDLE_RELAY
|
||||
|
||||
#ifdef USE_SPINDLE_RELAY
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_17
|
||||
#else
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_16
|
||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
||||
#endif
|
||||
|
@@ -51,8 +51,10 @@
|
||||
//#define USE_SPINDLE_RELAY
|
||||
|
||||
#ifdef USE_SPINDLE_RELAY
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2
|
||||
#else
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_PWM
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_16
|
||||
#define SPINDLE_ENABLE_PIN GPIO_NUM_32
|
||||
#endif
|
||||
|
@@ -68,6 +68,8 @@
|
||||
#define SOLENOID_PEN_PIN GPIO_NUM_16
|
||||
#endif
|
||||
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||
|
||||
// defaults
|
||||
#define DEFAULT_STEP_PULSE_MICROSECONDS 3
|
||||
#define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // stay on
|
||||
|
@@ -57,6 +57,8 @@
|
||||
#define X_LIMIT_PIN GPIO_NUM_4
|
||||
#define LIMIT_MASK B1
|
||||
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_NONE
|
||||
|
||||
#ifdef IGNORE_CONTROL_PINS // maybe set in config.h
|
||||
#undef IGNORE_CONTROL_PINS
|
||||
#endif
|
||||
|
@@ -81,10 +81,15 @@
|
||||
|
||||
// Relay operation
|
||||
// Install Jumper near relay
|
||||
// For spindle Use max RPM of 1
|
||||
// For PWM remove jumper and set MAX RPM to something higher ($30 setting)
|
||||
// For PWM remove jumper to prevent relay damage
|
||||
// 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
|
||||
|
||||
|
@@ -65,11 +65,7 @@
|
||||
#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
|
||||
|
||||
|
||||
// Comment out servo pin and uncomment spindle pwm pin to use the servo PWM to control a spindle
|
||||
/*
|
||||
#define SPINDLE_OUTPUT_PIN GPIO_NUM_27
|
||||
*/
|
||||
#define SPINDLE_TYPE SPINDLE_TYPE_RELAY
|
||||
|
||||
// #define X_LIMIT_PIN See version section
|
||||
#define Y_LIMIT_PIN GPIO_NUM_4
|
||||
|
@@ -314,7 +314,7 @@ void report_grbl_settings(uint8_t client, uint8_t show_extended) {
|
||||
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, "$35=%3.3f\r\n", settings.spindle_pwm_min_value); strcat(rpt, setting);
|
||||
sprintf(setting, "$36=%3.3f\r\n", settings.spindle_pwm_max_value); strcat(rpt, setting);
|
||||
sprintf(setting, "$36=%3.3f\r\n", settings.spindle_pwm_max_value); strcat(rpt, setting);
|
||||
for (uint8_t index = 0; index < USER_SETTING_COUNT; index++) {
|
||||
sprintf(setting, "$%d=%d\r\n", 80 + index, settings.machine_int16[index]); strcat(rpt, setting);
|
||||
}
|
||||
@@ -777,11 +777,11 @@ void report_gcode_comment(char* comment) {
|
||||
|
||||
|
||||
/*
|
||||
Print a message in hex formate
|
||||
Print a message in hex format
|
||||
Ex: report_hex_msg(msg, "Rx:", 6);
|
||||
Would could print [MSG Rx: 0x01 0x03 0x01 0x08 0x31 0xbf]
|
||||
Would would print something like ... [MSG Rx: 0x01 0x03 0x01 0x08 0x31 0xbf]
|
||||
*/
|
||||
void report_hex_msg(char* buf, const char *prefix, int len) {
|
||||
void report_hex_msg(char* buf, const char* prefix, int len) {
|
||||
char report[200];
|
||||
char temp[20];
|
||||
sprintf(report, "%s", prefix);
|
||||
@@ -792,4 +792,23 @@ void report_hex_msg(char* buf, const char *prefix, int len) {
|
||||
|
||||
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 '?';
|
||||
}
|
||||
}
|
@@ -172,4 +172,6 @@ void report_gcode_comment(char* comment);
|
||||
|
||||
void report_hex_msg(char* buf, const char *prefix, int len);
|
||||
|
||||
char report_get_axis_letter(uint8_t axis);
|
||||
|
||||
#endif
|
||||
|
@@ -54,7 +54,7 @@
|
||||
|
||||
void BESCSpindle :: init() {
|
||||
|
||||
get_pin_numbers();
|
||||
get_pins_and_settings();
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN) {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Warning: BESC output pin not defined");
|
||||
@@ -116,6 +116,6 @@ float BESCSpindle::set_rpm(float rpm) {
|
||||
set_enable_pin(rpm != 0);
|
||||
#endif
|
||||
|
||||
set_pwm(pwm_value);
|
||||
set_output(pwm_value);
|
||||
return rpm;
|
||||
}
|
@@ -27,7 +27,7 @@
|
||||
|
||||
// ======================================== DacSpindle ======================================
|
||||
void DacSpindle :: init() {
|
||||
get_pin_numbers();
|
||||
get_pins_and_settings();
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
return;
|
||||
|
||||
@@ -97,12 +97,12 @@ float DacSpindle::set_rpm(float rpm) {
|
||||
set_enable_pin(rpm != 0);
|
||||
#endif
|
||||
|
||||
set_pwm(pwm_value);
|
||||
set_output(pwm_value);
|
||||
|
||||
return rpm;
|
||||
}
|
||||
|
||||
void DacSpindle :: set_pwm(uint32_t duty) {
|
||||
void DacSpindle :: set_output(uint32_t duty) {
|
||||
if (_gpio_ok) {
|
||||
dacWrite(_output_pin, (uint8_t)duty);
|
||||
}
|
||||
|
@@ -58,7 +58,7 @@
|
||||
void HuanyangSpindle :: init() {
|
||||
|
||||
// fail if numbers are not defined
|
||||
if (!get_pin_numbers()) {
|
||||
if (!get_pins_and_settings()) {
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Huanyang spindle errors");
|
||||
return;
|
||||
}
|
||||
@@ -99,7 +99,7 @@ void HuanyangSpindle :: init() {
|
||||
// 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_pin_numbers() {
|
||||
bool HuanyangSpindle :: get_pins_and_settings() {
|
||||
bool pins_ok = true;
|
||||
|
||||
#ifdef HUANYANG_TXD_PIN
|
||||
|
@@ -29,14 +29,13 @@
|
||||
*/
|
||||
void PWMSpindle::init() {
|
||||
|
||||
get_pin_numbers();
|
||||
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
|
||||
|
||||
@@ -44,13 +43,13 @@ void PWMSpindle::init() {
|
||||
pinMode(_enable_pin, OUTPUT);
|
||||
|
||||
if (_direction_pin != UNDEFINED_PIN)
|
||||
pinMode(_direction_pin, OUTPUT);
|
||||
pinMode(_direction_pin, OUTPUT);
|
||||
|
||||
config_message();
|
||||
}
|
||||
|
||||
// Get the GPIO from the machine definition
|
||||
void PWMSpindle :: get_pin_numbers() {
|
||||
void PWMSpindle :: get_pins_and_settings() {
|
||||
// setup all the pins
|
||||
|
||||
#ifdef SPINDLE_OUTPUT_PIN
|
||||
@@ -66,7 +65,7 @@ void PWMSpindle :: get_pin_numbers() {
|
||||
#endif
|
||||
|
||||
#ifdef SPINDLE_DIR_PIN
|
||||
_direction_pin = SPINDLE_DIR_PIN;
|
||||
_direction_pin = SPINDLE_DIR_PIN;
|
||||
#else
|
||||
_direction_pin = UNDEFINED_PIN;
|
||||
#endif
|
||||
@@ -77,7 +76,7 @@ void PWMSpindle :: get_pin_numbers() {
|
||||
_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");
|
||||
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);
|
||||
@@ -95,18 +94,18 @@ void PWMSpindle :: get_pin_numbers() {
|
||||
_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) {
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
return rpm;
|
||||
|
||||
uint32_t pwm_value;
|
||||
|
||||
// apply overrides and limits
|
||||
rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value (percent)
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
return rpm;
|
||||
|
||||
// apply override
|
||||
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)) {
|
||||
@@ -118,17 +117,15 @@ float PWMSpindle::set_rpm(float rpm) {
|
||||
sys.spindle_speed = 0.0;
|
||||
pwm_value = _pwm_off_value;
|
||||
} else { // Set minimum PWM output
|
||||
rpm = _min_rpm;
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RPM 2:%5.2f", rpm);
|
||||
rpm = _min_rpm;
|
||||
sys.spindle_speed = rpm;
|
||||
pwm_value = _pwm_min_value;
|
||||
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;
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RPM 3:%5.2f", rpm);
|
||||
|
||||
#ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE
|
||||
pwm_value = piecewise_linear_fit(rpm);
|
||||
#else
|
||||
@@ -139,9 +136,8 @@ float PWMSpindle::set_rpm(float rpm) {
|
||||
#ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
|
||||
set_enable_pin(rpm != 0);
|
||||
#endif
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle RPM Final:%5.2f", rpm);
|
||||
set_pwm(pwm_value);
|
||||
|
||||
set_output(pwm_value);
|
||||
|
||||
return rpm;
|
||||
}
|
||||
@@ -180,7 +176,7 @@ uint8_t PWMSpindle::get_state() {
|
||||
void PWMSpindle::stop() {
|
||||
// inverts are delt with in methods
|
||||
set_enable_pin(false);
|
||||
set_pwm(_pwm_off_value);
|
||||
set_output(_pwm_off_value);
|
||||
}
|
||||
|
||||
// prints the startup message of the spindle config
|
||||
@@ -189,9 +185,7 @@ void PWMSpindle :: config_message() {
|
||||
}
|
||||
|
||||
|
||||
void PWMSpindle::set_pwm(uint32_t duty) {
|
||||
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Spindle set duty:%d", duty);
|
||||
void PWMSpindle::set_output(uint32_t duty) {
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
return;
|
||||
@@ -225,15 +219,15 @@ void PWMSpindle::set_spindle_dir_pin(bool Clockwise) {
|
||||
|
||||
|
||||
/*
|
||||
Calculate the best precision of a PWM base on the frequency in bits
|
||||
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
|
||||
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)
|
||||
|
||||
while ((1 << precision) < (uint32_t)(80000000.0 / freq) && precision <= 16)
|
||||
precision++;
|
||||
|
||||
return precision - 1;
|
||||
|
@@ -27,7 +27,8 @@
|
||||
This is the same as a PWM spindle, but is a digital rather than PWM output
|
||||
*/
|
||||
void RelaySpindle::init() {
|
||||
get_pin_numbers();
|
||||
get_pins_and_settings();
|
||||
|
||||
if (_output_pin == UNDEFINED_PIN)
|
||||
return;
|
||||
|
||||
@@ -57,19 +58,18 @@ float RelaySpindle::set_rpm(float rpm) {
|
||||
|
||||
if (rpm == 0) {
|
||||
sys.spindle_speed = 0.0;
|
||||
set_pwm(0);
|
||||
set_output(0);
|
||||
} else {
|
||||
sys.spindle_speed = rpm;
|
||||
set_pwm(1);
|
||||
set_output(1);
|
||||
}
|
||||
|
||||
return rpm;
|
||||
}
|
||||
|
||||
void RelaySpindle::set_pwm(uint32_t duty) {
|
||||
void RelaySpindle::set_output(uint32_t duty) {
|
||||
#ifdef INVERT_SPINDLE_PWM
|
||||
duty = (duty == 0); // flip duty
|
||||
#endif
|
||||
grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Relay output %d", duty > 0);
|
||||
digitalWrite(_output_pin, duty > 0); // anything greater
|
||||
}
|
@@ -101,9 +101,9 @@ class PWMSpindle : public Spindle {
|
||||
uint8_t _pwm_precision;
|
||||
float _pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
|
||||
|
||||
virtual void set_pwm(uint32_t duty);
|
||||
virtual void set_output(uint32_t duty);
|
||||
void set_enable_pin(bool enable_pin);
|
||||
void get_pin_numbers();
|
||||
void get_pins_and_settings();
|
||||
uint8_t calc_pwm_precision(float freq);
|
||||
};
|
||||
|
||||
@@ -114,7 +114,7 @@ class RelaySpindle : public PWMSpindle {
|
||||
void config_message();
|
||||
float set_rpm(float rpm);
|
||||
protected:
|
||||
void set_pwm(uint32_t duty);
|
||||
void set_output(uint32_t duty);
|
||||
};
|
||||
|
||||
// this is the same as a PWM spindle, but the M4 compensation is supported.
|
||||
@@ -133,7 +133,7 @@ class DacSpindle : public PWMSpindle {
|
||||
private:
|
||||
bool _gpio_ok; // DAC is on a valid pin
|
||||
protected:
|
||||
void set_pwm(uint32_t duty); // sets DAC instead of PWM
|
||||
void set_output(uint32_t duty); // sets DAC instead of PWM
|
||||
};
|
||||
|
||||
class HuanyangSpindle : public Spindle {
|
||||
@@ -150,7 +150,7 @@ class HuanyangSpindle : public Spindle {
|
||||
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_pin_numbers();
|
||||
bool get_pins_and_settings();
|
||||
|
||||
uint8_t _txd_pin;
|
||||
uint8_t _rxd_pin;
|
||||
|
Reference in New Issue
Block a user