diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index fe1c341f..b35d99ce 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -40,7 +40,7 @@ Some features should not be changed. See notes below. #include //#define ESP_DEBUG -#define N_AXIS 3 // Number of axes defined +#define N_AXIS 3 // Number of axes defined (valid range: 3 to 6) // Define CPU pin map and default settings. // NOTE: OEMs can avoid the need to maintain/update the defaults.h and cpu_map.h files and use only diff --git a/Grbl_Esp32/cpu_map.h b/Grbl_Esp32/cpu_map.h index dcf2239b..13f32607 100644 --- a/Grbl_Esp32/cpu_map.h +++ b/Grbl_Esp32/cpu_map.h @@ -379,11 +379,16 @@ #ifdef CPU_MAP_MIDTBOT // Buildlog.net midtbot #define CPU_MAP_NAME "CPU_MAP_MIDTBOT" + + #define USE_RMT_STEPS #define X_STEP_PIN GPIO_NUM_12 - #define Y_STEP_PIN GPIO_NUM_14 + #define Y_STEP_PIN GPIO_NUM_14 + #define X_RMT_CHANNEL 0 + #define X_DIRECTION_PIN GPIO_NUM_26 - #define Y_DIRECTION_PIN GPIO_NUM_25 + #define Y_DIRECTION_PIN GPIO_NUM_25 + #define Y_RMT_CHANNEL 1 #ifndef COREXY // maybe set in config.h #define COREXY @@ -403,6 +408,7 @@ #define SERVO_Z_CHANNEL_NUM 5 #define SERVO_Z_RANGE_MIN 0.0 #define SERVO_Z_RANGE_MAX 5.0 + #define SERVO_Z_MPOS false #ifndef IGNORE_CONTROL_PINS // maybe set in config.h #define IGNORE_CONTROL_PINS diff --git a/Grbl_Esp32/gcode.cpp b/Grbl_Esp32/gcode.cpp index d3dfa679..6610147e 100644 --- a/Grbl_Esp32/gcode.cpp +++ b/Grbl_Esp32/gcode.cpp @@ -379,21 +379,21 @@ uint8_t gc_execute_line(char *line, uint8_t client) legal g-code words and stores their value. Error-checking is performed later since some words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */ switch(letter) { -#ifdef A_AXIS +#ifdef N_AXIS > A_AXIS case 'A': word_bit = WORD_A; gc_block.values.xyz[A_AXIS] = value; axis_words |= (1< B_AXIS case 'B': word_bit = WORD_B; gc_block.values.xyz[B_AXIS] = value; axis_words |= (1< A_AXIS case 'C': word_bit = WORD_C; gc_block.values.xyz[C_AXIS] = value; diff --git a/Grbl_Esp32/grbl.h b/Grbl_Esp32/grbl.h index 586286b2..c24ad7de 100644 --- a/Grbl_Esp32/grbl.h +++ b/Grbl_Esp32/grbl.h @@ -20,7 +20,7 @@ // Grbl versioning system #define GRBL_VERSION "1.1f" -#define GRBL_VERSION_BUILD "20190829" +#define GRBL_VERSION_BUILD "20190830" //#include #include diff --git a/Grbl_Esp32/servo_axis.cpp b/Grbl_Esp32/servo_axis.cpp index ce4efc1e..6395b40f 100644 --- a/Grbl_Esp32/servo_axis.cpp +++ b/Grbl_Esp32/servo_axis.cpp @@ -52,7 +52,7 @@ void init_servos() { //grbl_send(CLIENT_SERIAL, "[MSG: Init Servos]\r\n"); #ifdef SERVO_X_PIN - grbl_send(CLIENT_SERIAL, "[MSG: Init X Servo]\r\n"); + grbl_send(CLIENT_SERIAL, "[MSG:Init X Servo]\r\n"); X_Servo_Axis.init(); X_Servo_Axis.set_range(SERVO_X_RANGE_MIN, SERVO_X_RANGE_MAX); X_Servo_Axis.set_homing_type(SERVO_HOMING_OFF); @@ -60,20 +60,23 @@ void init_servos() X_Servo_Axis.set_disable_with_steppers(false); #endif #ifdef SERVO_Y_PIN - grbl_send(CLIENT_SERIAL, "[MSG: Init Y Servo]\r\n"); + grbl_send(CLIENT_SERIAL, "[MSG:Init Y Servo]\r\n"); Y_Servo_Axis.init(); Y_Servo_Axis.set_range(SERVO_Y_RANGE_MIN, SERVO_Y_RANGE_MAX); #endif #ifdef SERVO_Z_PIN - grbl_send(CLIENT_SERIAL, "[MSG: Init Z Servo]\r\n"); + grbl_send(CLIENT_SERIAL, "[MSG:Init Z Servo]\r\n"); Z_Servo_Axis.init(); Z_Servo_Axis.set_range(SERVO_Z_RANGE_MIN, SERVO_Z_RANGE_MAX); Z_Servo_Axis.set_homing_type(SERVO_HOMING_TARGET); Z_Servo_Axis.set_homing_position(SERVO_Z_RANGE_MAX); + #ifdef SERVO_Z_MPOS // value should be true or false + Z_Servo_Axis.set_use_mpos(SERVO_Z_MPOS); + #endif #endif #ifdef SERVO_A_PIN - grbl_send(CLIENT_SERIAL, "[MSG: Init A Servo]\r\n"); + grbl_send(CLIENT_SERIAL, "[MSG:Init A Servo]\r\n"); A_Servo_Axis.init(); A_Servo_Axis.set_range(SERVO_A_RANGE_MIN, SERVO_A_RANGE_MAX); A_Servo_Axis.set_homing_type(SERVO_HOMING_OFF); @@ -81,12 +84,12 @@ void init_servos() A_Servo_Axis.set_disable_with_steppers(false); #endif #ifdef SERVO_B_PIN - grbl_send(CLIENT_SERIAL, "[MSG: Init B Servo]\r\n"); + grbl_send(CLIENT_SERIAL, "[MSG:Init B Servo]\r\n"); B_Servo_Axis.init(); B_Servo_Axis.set_range(SERVO_B_RANGE_MIN, SERVO_B_RANGE_MAX); #endif #ifdef SERVO_C_PIN - grbl_send(CLIENT_SERIAL, "[MSG: Init C Servo]\r\n"); + grbl_send(CLIENT_SERIAL, "[MSG:Init C Servo]\r\n"); C_Servo_Axis.init(); C_Servo_Axis.set_range(SERVO_C_RANGE_MIN, SERVO_C_RANGE_MAX); C_Servo_Axis.set_homing_type(SERVO_HOMING_TARGET); @@ -160,13 +163,15 @@ void ServoAxis::init() void ServoAxis::set_location() { - // These are the pulse lengths for the minimum and maximum positions - // Note: Some machines will have the physical max/min inverted with pulse length max/min due to invert setting $3=... - float servo_pulse_min, servo_pulse_max; - float min_pulse_cal, max_pulse_cal; // calibration values in percent 110% = 1.1 - uint32_t servo_pulse_len; + // These are the pulse lengths for the minimum and maximum positions + // Note: Some machines will have the physical max/min inverted with pulse length max/min due to invert setting $3=... + float servo_pulse_min, servo_pulse_max; + float min_pulse_cal, max_pulse_cal; // calibration values in percent 110% = 1.1 + uint32_t servo_pulse_len; float servo_pos, mpos, offset; + + // skip location if we are in alarm mode if (_disable_on_alarm && (sys.state == STATE_ALARM)) { disable(); @@ -177,7 +182,7 @@ void ServoAxis::set_location() if (_disable_with_steppers && get_stepper_disable()) { disable(); return; - } + } if ( (_homing_type == SERVO_HOMING_TARGET) && (sys.state == STATE_HOMING) ) { @@ -189,8 +194,9 @@ void ServoAxis::set_location() servo_pos = mpos; } else { - offset = gc_state.coord_system[_axis]+gc_state.coord_offset[_axis]; // get the current axis work offset - servo_pos = mpos - offset; // determine the current work position + offset = gc_state.coord_system[_axis] + gc_state.coord_offset[_axis]; // get the current axis work offset + servo_pos = mpos - offset; // determine the current work position + } } @@ -319,5 +325,14 @@ void ServoAxis::set_disable_with_steppers(bool disable_with_steppers) { _disable_with_steppers = disable_with_steppers; } +/* + If true, servo position will alway be calculated in machine position + Offsets will not be applied +*/ +void ServoAxis::set_use_mpos(bool use_mpos) { + _use_mpos = use_mpos; +} + + #endif diff --git a/Grbl_Esp32/servo_axis.h b/Grbl_Esp32/servo_axis.h index ce689711..802ab938 100644 --- a/Grbl_Esp32/servo_axis.h +++ b/Grbl_Esp32/servo_axis.h @@ -95,9 +95,9 @@ class ServoAxis{ public: ServoAxis(uint8_t axis, uint8_t pin_num, uint8_t channel_num); // constructor void init(); - void set_location(); - void disable(); // sets PWM to 0% duty cycle. Most servos can be manually moved in this state - void set_range(float min, float max); + void set_location(); + void disable(); // sets PWM to 0% duty cycle. Most servos can be manually moved in this state + void set_range(float min, float max); void set_homing_type(uint8_t homing_type); void set_homing_position(float homing_position); void set_disable_on_alarm (bool disable_on_alarm); diff --git a/Grbl_Esp32/stepper.cpp b/Grbl_Esp32/stepper.cpp index d0a98ae2..5f9dc376 100644 --- a/Grbl_Esp32/stepper.cpp +++ b/Grbl_Esp32/stepper.cpp @@ -262,13 +262,13 @@ void IRAM_ATTR onStepperDriverTimer(void *para) // ISR It is time to take a ste st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; - #ifdef A_AXIS + #if (N_AXIS > A_AXIS) st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.exec_segment->amass_level; #endif - #ifdef B_AXIS + #if (N_AXIS > B_AXIS) st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.exec_segment->amass_level; #endif - #ifdef C_AXIS + #if (N_AXIS > C_AXIS) st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level; #endif diff --git a/Grbl_Esp32/system.cpp b/Grbl_Esp32/system.cpp index da054473..9facd3c8 100644 --- a/Grbl_Esp32/system.cpp +++ b/Grbl_Esp32/system.cpp @@ -440,7 +440,8 @@ uint8_t get_limit_pin_mask(uint8_t axis_idx) if ( axis_idx == Z_AXIS ) { return((1<