diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index 39488722..59df1595 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -312,11 +312,6 @@ void kinematics_post_homing() { memcpy(gc_state.position, last_cartesian, n_axis * sizeof(last_cartesian[0])); } -// this is used used by Limits.cpp to see if the range of the machine is exceeded. -bool limitsCheckTravel(float* target) { - return false; -} - void user_m30() {} // ================ Local Helper functions ================= diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 9c5ac586..ace8e55a 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -386,43 +386,43 @@ // ========== Motor current (SPI Drivers ) ============= #ifndef DEFAULT_X_CURRENT -# define DEFAULT_X_CURRENT 0.25 // $140 current in amps (extended set) +# define DEFAULT_X_CURRENT 0.8 // $140 current in amps (extended set) #endif #ifndef DEFAULT_Y_CURRENT -# define DEFAULT_Y_CURRENT 0.25 // $141 current in amps (extended set) +# define DEFAULT_Y_CURRENT 0.8 // $141 current in amps (extended set) #endif #ifndef DEFAULT_Z_CURRENT -# define DEFAULT_Z_CURRENT 0.25 // $142 current in amps (extended set) +# define DEFAULT_Z_CURRENT 0.8 // $142 current in amps (extended set) #endif #ifndef DEFAULT_A_CURRENT -# define DEFAULT_A_CURRENT 0.25 // $143 current in amps (extended set) +# define DEFAULT_A_CURRENT 0.8 // $143 current in amps (extended set) #endif #ifndef DEFAULT_B_CURRENT -# define DEFAULT_B_CURRENT 0.25 // $144 current in amps (extended set) +# define DEFAULT_B_CURRENT 0.8 // $144 current in amps (extended set) #endif #ifndef DEFAULT_C_CURRENT -# define DEFAULT_C_CURRENT 0.25 // $145 current in amps (extended set) +# define DEFAULT_C_CURRENT 0.8 // $145 current in amps (extended set) #endif // ========== Motor hold current (SPI Drivers ) ============= #ifndef DEFAULT_X_HOLD_CURRENT -# define DEFAULT_X_HOLD_CURRENT 0.125 // $150 current in amps (extended set) +# define DEFAULT_X_HOLD_CURRENT 0.4 // $150 current in amps (extended set) #endif #ifndef DEFAULT_Y_HOLD_CURRENT -# define DEFAULT_Y_HOLD_CURRENT 0.125 // $151 current in amps (extended set) +# define DEFAULT_Y_HOLD_CURRENT 0.4 // $151 current in amps (extended set) #endif #ifndef DEFAULT_Z_HOLD_CURRENT -# define DEFAULT_Z_HOLD_CURRENT 0.125 // $152 current in amps (extended set) +# define DEFAULT_Z_HOLD_CURRENT 0.4 // $152 current in amps (extended set) #endif #ifndef DEFAULT_A_HOLD_CURRENT -# define DEFAULT_A_HOLD_CURRENT 0.125 // $153 current in amps (extended set) +# define DEFAULT_A_HOLD_CURRENT 0.4 // $153 current in amps (extended set) #endif #ifndef DEFAULT_B_HOLD_CURRENT -# define DEFAULT_B_HOLD_CURRENT 0.125 // $154 current in amps (extended set) +# define DEFAULT_B_HOLD_CURRENT 0.4 // $154 current in amps (extended set) #endif #ifndef DEFAULT_C_HOLD_CURRENT -# define DEFAULT_C_HOLD_CURRENT 0.125 // $154 current in amps (extended set) +# define DEFAULT_C_HOLD_CURRENT 0.4 // $154 current in amps (extended set) #endif // ========== Microsteps (SPI Drivers ) ================ diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index d55eb1ed..335c4c36 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1483,8 +1483,10 @@ Error gc_execute_line(char* line, uint8_t client) { // and absolute and incremental modes. pl_data->motion.rapidMotion = 1; // Set rapid motion flag. if (axis_command != AxisCommand::None) { + limitsCheckSoft(gc_block.values.xyz); cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } + limitsCheckSoft(coord_data); cartesian_to_motors(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; @@ -1513,9 +1515,11 @@ Error gc_execute_line(char* line, uint8_t client) { if (axis_command == AxisCommand::MotionMode) { GCUpdatePos gc_update_pos = GCUpdatePos::Target; if (gc_state.modal.motion == Motion::Linear) { + limitsCheckSoft(gc_block.values.xyz); cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if (gc_state.modal.motion == Motion::Seek) { pl_data->motion.rapidMotion = 1; // Set rapid motion flag. + limitsCheckSoft(gc_block.values.xyz); cartesian_to_motors(gc_block.values.xyz, pl_data, gc_state.position); } else if ((gc_state.modal.motion == Motion::CwArc) || (gc_state.modal.motion == Motion::CcwArc)) { mc_arc(gc_block.values.xyz, diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index b89b7c4a..4d9ce8ab 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210816"; +const char* const GRBL_VERSION_BUILD = "20211016"; //#include #include diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index fec18cfd..9e172b5b 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -416,3 +416,12 @@ bool limitsSwitchDefined(uint8_t axis, uint8_t gang_index) { bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) { return false; } + +void limitsCheckSoft(float* target) { + if (soft_limits->get()) { + // NOTE: Block jog state. Jogging is a special case and soft limits are handled independently. + if (sys.state != State::Jog && sys.state != State::Homing) { + limits_soft_check(target); + } + } +} diff --git a/Grbl_Esp32/src/Limits.h b/Grbl_Esp32/src/Limits.h index 3697f8e0..a3e52e09 100644 --- a/Grbl_Esp32/src/Limits.h +++ b/Grbl_Esp32/src/Limits.h @@ -57,3 +57,5 @@ bool limitsCheckTravel(float* target); // check if a switch has been defined bool limitsSwitchDefined(uint8_t axis, uint8_t gang_index); + +void limitsCheckSoft(float* target); diff --git a/Grbl_Esp32/src/Machine.h b/Grbl_Esp32/src/Machine.h index 11ab8406..15a146f9 100644 --- a/Grbl_Esp32/src/Machine.h +++ b/Grbl_Esp32/src/Machine.h @@ -8,8 +8,7 @@ // !!! For initial testing, start with test_drive.h which disables // all I/O pins // #include "Machines/atari_1020.h" -# include "Machines/test_drive.h" - +# include "Machines/6_pack_TMC2130_XYZ_Test.h" // !!! For actual use, change the line above to select a board // from Machines/, for example: // #include "Machines/3axis_v4.h" diff --git a/Grbl_Esp32/src/Machines/TMC2209_4x.h b/Grbl_Esp32/src/Machines/TMC2209_4x.h index 87a2ced0..e3d676a9 100644 --- a/Grbl_Esp32/src/Machines/TMC2209_4x.h +++ b/Grbl_Esp32/src/Machines/TMC2209_4x.h @@ -75,12 +75,12 @@ #define STEPPERS_DISABLE_PIN GPIO_NUM_25 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module -#define USER_DIGITAL_PIN_0 GPIO_NUM_4 // M62 M63 -#define USER_DIGITAL_PIN_1 GPIO_NUM_13 // M62 M63 -#define USER_DIGITAL_PIN_2 GPIO_NUM_17 // M62 M63 -#define USER_DIGITAL_PIN_3 GPIO_NUM_12 // M62 M63 +// Built in I/O +#define SPINDLE_TYPE SpindleType::PWM +#define SPINDLE_OUTPUT_PIN GPIO_NUM_4 +#define SPINDLE_ENABLE_PIN GPIO_NUM_13 +#define COOLANT_MIST_PIN GPIO_NUM_17 // M7 on M9 Off +#define COOLANT_FLOOD_PIN GPIO_NUM_12 // M8 on M9 off // ===================== defaults ====================== diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 00762b97..3b0b8fcd 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -45,14 +45,7 @@ bool mc_line(float* target, plan_line_data_t* pl_data) { bool submitted_result = false; // store the plan data so it can be cancelled by the protocol system if needed sys_pl_data_inflight = pl_data; - // If enabled, check for soft limit violations. Placed here all line motions are picked up - // from everywhere in Grbl. - if (soft_limits->get()) { - // NOTE: Block jog state. Jogging is a special case and soft limits are handled independently. - if (sys.state != State::Jog) { - limits_soft_check(target); - } - } + // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. if (sys.state == State::CheckMode) { sys_pl_data_inflight = NULL; @@ -222,6 +215,7 @@ void mc_arc(float* target, position[axis_1] = center_axis1 + r_axis1; position[axis_linear] += linear_per_segment; pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered + limitsCheckSoft(position); cartesian_to_motors(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; @@ -233,6 +227,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. + limitsCheckSoft(target); cartesian_to_motors(target, pl_data, previous_position); } @@ -419,6 +414,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par } // Setup and queue probing motion. Auto cycle-start should not start the cycle. grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Found"); + limitsCheckSoft(target); cartesian_to_motors(target, pl_data, gc_state.position); // Activate the probing state monitor in the stepper module. sys_probe_state = Probe::Active; diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 16e658b8..84e6ce2a 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -54,9 +54,9 @@ namespace Motors { _spi_index(spi_index) { _has_errors = false; if (_driver_part_number == 2130) { - tmcstepper = new TMC2130Stepper(_cs_pin, _r_sense, _spi_index); + tmc2130 = new TMC2130Stepper(_cs_pin, _r_sense, _spi_index); } else if (_driver_part_number == 5160) { - tmcstepper = new TMC5160Stepper(_cs_pin, _r_sense, _spi_index); + tmc5160 = new TMC5160Stepper(_cs_pin, _r_sense, _spi_index); } else { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, @@ -67,14 +67,14 @@ namespace Motors { return; } - _has_errors = false; + _has_errors = false; digitalWrite(_cs_pin, HIGH); pinMode(_cs_pin, OUTPUT); // use slower speed if I2S if (_cs_pin >= I2S_OUT_PIN_BASE) { - tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ); + (tmc2130) ? tmc2130->setSPISpeed(TRINAMIC_SPI_FREQ) : tmc5160->setSPISpeed(TRINAMIC_SPI_FREQ); } link = List; @@ -98,7 +98,7 @@ namespace Motors { SPI.begin(); // this will get called for each motor, but does not seem to hurt anything - tmcstepper->begin(); + (tmc2130) ? tmc2130->begin() : tmc5160->begin(); _has_errors = !test(); // Try communicating with motor. Prints an error if there is a problem. @@ -142,7 +142,10 @@ namespace Motors { if (_has_errors) { return false; } - switch (tmcstepper->test_connection()) { + + uint8_t result = tmc2130 ? tmc2130->test_connection() : tmc5160->test_connection(); + + switch (result) { case 1: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, @@ -159,7 +162,8 @@ namespace Motors { // driver responded, so check for other errors from the DRV_STATUS register TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. - status.sr = tmcstepper->DRV_STATUS(); + + status.sr = tmc2130 ? tmc2130->DRV_STATUS() : tmc5160->DRV_STATUS(); bool err = false; @@ -208,8 +212,13 @@ namespace Motors { if (hold_i_percent > 1.0) hold_i_percent = 1.0; } - tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); - tmcstepper->rms_current(run_i_ma, hold_i_percent); + if (tmc2130) { + tmc2130->microsteps(axis_settings[_axis_index]->microsteps->get()); + tmc2130->rms_current(run_i_ma, hold_i_percent); + } else { + tmc5160->microsteps(axis_settings[_axis_index]->microsteps->get()); + tmc5160->rms_current(run_i_ma, hold_i_percent); + } init_step_dir_pins(); } @@ -236,33 +245,65 @@ namespace Motors { } _mode = newMode; - switch (_mode) { - case TrinamicMode ::StealthChop: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); - tmcstepper->en_pwm_mode(true); - tmcstepper->pwm_autoscale(true); - tmcstepper->diag1_stall(false); - break; - case TrinamicMode ::CoolStep: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); - tmcstepper->en_pwm_mode(false); - tmcstepper->pwm_autoscale(false); - tmcstepper->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep - tmcstepper->THIGH(NORMAL_THIGH); - break; - case TrinamicMode ::StallGuard: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); - tmcstepper->en_pwm_mode(false); - tmcstepper->pwm_autoscale(false); - tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); - tmcstepper->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); - tmcstepper->sfilt(1); - tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 - tmcstepper->sgt(constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); - break; - default: - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_UNDEFINED"); + if (tmc2130) { + switch (_mode) { + case TrinamicMode ::StealthChop: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); + tmc2130->en_pwm_mode(true); + tmc2130->pwm_autoscale(true); + tmc2130->diag1_stall(false); + break; + case TrinamicMode ::CoolStep: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); + tmc2130->en_pwm_mode(false); + tmc2130->pwm_autoscale(false); + tmc2130->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep + tmc2130->THIGH(NORMAL_THIGH); + break; + case TrinamicMode ::StallGuard: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); + tmc2130->en_pwm_mode(false); + tmc2130->pwm_autoscale(false); + tmc2130->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); + tmc2130->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); + tmc2130->sfilt(1); + tmc2130->diag1_stall(true); // stallguard i/o is on diag1 + tmc2130->sgt(constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); + break; + default: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_UNDEFINED"); + } + } else { + switch (_mode) { + case TrinamicMode ::StealthChop: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); + tmc5160->en_pwm_mode(true); + tmc5160->pwm_autoscale(true); + tmc5160->diag1_stall(false); + break; + case TrinamicMode ::CoolStep: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); + tmc5160->en_pwm_mode(false); + tmc5160->pwm_autoscale(false); + tmc5160->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep + tmc5160->THIGH(NORMAL_THIGH); + break; + case TrinamicMode ::StallGuard: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); + tmc5160->en_pwm_mode(false); + tmc5160->pwm_autoscale(false); + tmc5160->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); + tmc5160->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); + tmc5160->sfilt(1); + tmc5160->diag1_stall(true); // stallguard i/o is on diag1 + tmc5160->sgt(constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); + break; + default: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_UNDEFINED"); + } } + + } /* @@ -272,24 +313,27 @@ namespace Motors { if (_has_errors) { return; } - uint32_t tstep = tmcstepper->TSTEP(); + uint32_t tstep = (tmc2130) ? tmc2130->TSTEP() : tmc5160->TSTEP(); if (tstep == 0xFFFFF || tstep < 1) { // if axis is not moving return return; } float feedrate = st_get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz + int32_t st = (tmc2130) ? tmc2130->stallguard() : tmc5160->stallguard(); + int32_t sg = (tmc2130) ? tmc2130->sg_result() : tmc5160->sg_result(); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Stallguard %d SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", reportAxisNameMsg(_axis_index, _dual_axis_index), - tmcstepper->stallguard(), - tmcstepper->sg_result(), + st, + sg, feedrate, constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. - status.sr = tmcstepper->DRV_STATUS(); + status.sr = (tmc2130) ? tmc2130->DRV_STATUS() : tmc5160->DRV_STATUS(); // these only report if there is a fault condition report_open_load(status); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h index facedf0d..7ea59cf7 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h @@ -100,7 +100,9 @@ namespace Motors { private: uint32_t calc_tstep(float speed, float percent); - TMC2130Stepper* tmcstepper; // all other driver types are subclasses of this one + TMC2130Stepper* tmc2130 = nullptr; + TMC2130Stepper* tmc5160 = nullptr; + TrinamicMode _homing_mode; uint8_t _cs_pin = UNDEFINED_PIN; // The chip select pin (can be the same for daisy chain) uint16_t _driver_part_number; // example: use 2130 for TMC2130 diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index 2489920f..633893e9 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -119,7 +119,7 @@ void client_init() { // after WebUI attaches. xTaskCreatePinnedToCore(clientCheckTask, // task "clientCheckTask", // name for task - 4096, // size of task stack + 8192, // size of task stack NULL, // parameters 1, // priority &clientCheckTaskHandle,