Merge pull request #201 from bdring/devt_6x

Devt 6x
2025-08-28 16:49:54 +02:00 · 2019-08-29 16:23:47 -05:00
parent 4a3479bc99 43c8168299
commit 31439bdd33
19 changed files with 703 additions and 97 deletions
--- a/Grbl_Esp32/commands.cpp
+++ b/Grbl_Esp32/commands.cpp
@@ -2035,10 +2035,13 @@ bool COMMANDS::execute_internal_command (int cmd, String cmd_params, level_authe
            resp += " # webcommunication: Sync: ";
            resp += String(web_server.port() + 1);
            #endif
-            resp += "# hostname:";
+            resp += " # hostname:";
            resp += wifi_config.Hostname();
            if (WiFi.getMode() == WIFI_AP)resp += "(AP mode)";
            #endif
+            //to save time in decoding `?`
+            resp += " # axis:";
+            resp += String(N_AXIS);
            if (espresponse)espresponse->println (resp.c_str());
        }   
            break;
--- a/Grbl_Esp32/config.h
+++ b/Grbl_Esp32/config.h
@@ -46,7 +46,7 @@ Some features should not be changed. See notes below.
 // one configuration file by placing their specific defaults and pin map at the bottom of this file.
 // If doing so, simply comment out these two defines and see instructions below.
 #define DEFAULTS_GENERIC
-#define CPU_MAP_ESP32 // these are defined in cpu_map.h
+#define CPU_MAP_FOO_6X // these are defined in cpu_map.h
 #define VERBOSE_HELP // adds addition help info, but could confuse some senders


--- a/Grbl_Esp32/cpu_map.h
+++ b/Grbl_Esp32/cpu_map.h
@@ -1150,7 +1150,159 @@
 		
 #endif

+#ifdef CPU_MAP_FOO_6X
+	#define CPU_MAP_NAME "CPU_MAP_FOO_6X"
+	
+	/*
+	#define USE_KINEMATICS	
+	#include "kinematics.h" 
+	*/
+	
+	// stepper motors
+	#define USE_RMT_STEPS	
+	
+	#define X_STEP_PIN      	GPIO_NUM_12
+	#define X_DIRECTION_PIN   GPIO_NUM_26
+	#define X_RMT_CHANNEL		0		
+	
+	#define Y_STEP_PIN      	GPIO_NUM_14   
+	#define Y_DIRECTION_PIN   GPIO_NUM_25
+	#define Y_RMT_CHANNEL		1

+	// Z is a servo
+	
+	#define A_STEP_PIN      	GPIO_NUM_27   
+	#define A_DIRECTION_PIN   GPIO_NUM_33
+	#define A_RMT_CHANNEL		2	
+	
+	#define B_STEP_PIN      	GPIO_NUM_15
+	#define B_DIRECTION_PIN   GPIO_NUM_32
+	#define B_RMT_CHANNEL		3	
+	
+	// C is a servo
+		
+		// servos
+		#define USE_SERVO_AXES
+		#define SERVO_Z_PIN 			GPIO_NUM_22
+		#define SERVO_Z_CHANNEL_NUM 	6
+		#define SERVO_Z_RANGE_MIN		0.0
+		#define SERVO_Z_RANGE_MAX		5.0
+		
+		#define SERVO_C_PIN 			GPIO_NUM_2
+		#define SERVO_C_CHANNEL_NUM 	7
+		#define SERVO_C_RANGE_MIN		0.0
+		#define SERVO_C_RANGE_MAX		5.0		
+		
+		// limit switches
+		#define X_LIMIT_PIN      	GPIO_NUM_21
+		#define Y_LIMIT_PIN      	GPIO_NUM_17
+		#define A_LIMIT_PIN      	GPIO_NUM_16
+		#define B_LIMIT_PIN      	GPIO_NUM_4
+		#define LIMIT_MASK      	B11011
+		
+		// OK to comment out to use pin for other features
+		#define STEPPERS_DISABLE_PIN GPIO_NUM_13		
+		
+		#ifdef HOMING_CYCLE_0	// undefine from config.h
+			#undef HOMING_CYCLE_0
+		#endif
+		//#define HOMING_CYCLE_0 (1<<X_AXIS)
+		#define HOMING_CYCLE_0 ((1<<X_AXIS)|(1<<Y_AXIS))
+		//#define HOMING_CYCLE_0 ((1<<X_AXIS)|(1<<Y_AXIS) |(1<<A_AXIS)|(1<<B_AXIS))
+		
+		#ifdef HOMING_CYCLE_1	// undefine from config.h
+			#undef HOMING_CYCLE_1
+		#endif
+		//#define HOMING_CYCLE_1 (1<<A_AXIS)  
+		#define HOMING_CYCLE_1 ((1<<A_AXIS)|(1<<B_AXIS))
+				
+		#ifdef HOMING_CYCLE_2	// undefine from config.h
+			#undef HOMING_CYCLE_2
+		#endif
+		/*
+		#define HOMING_CYCLE_2 (1<<Y_AXIS)
+		
+		#ifdef HOMING_CYCLE_3	// undefine from config.h
+			#undef HOMING_CYCLE_3
+		#endif
+		#define HOMING_CYCLE_3 (1<<B_AXIS)
+		*/
+		
+		
+		// required spindle stuff ... fix (remove) this requirement!
+		//#define SPINDLE_PWM_PIN    /
+		#define SPINDLE_PWM_CHANNEL 			0
+		#define SPINDLE_PWM_BASE_FREQ 		5000 // Hz
+		#define SPINDLE_PWM_BIT_PRECISION 	8   // be sure to match this with SPINDLE_PWM_MAX_VALUE
+		#define SPINDLE_PWM_OFF_VALUE     	0
+		#define SPINDLE_PWM_MAX_VALUE     	255 // (2^SPINDLE_PWM_BIT_PRECISION)
+		#define SPINDLE_PWM_MIN_VALUE   		SPINDLE_PWM_OFF_VALUE   // Must be greater than zero.
+		#define SPINDLE_PWM_RANGE         	(SPINDLE_PWM_MAX_VALUE-SPINDLE_PWM_MIN_VALUE)	
+		
+		// defaults  ... <Idle|MPos:-497.000,-3.000,0.000,-3.000,-497.000,0.000|FS:0,0>
+		#ifdef DEFAULTS_GENERIC
+			#undef DEFAULTS_GENERIC  // undefine generic then define each default below
+		#endif
+		#define DEFAULT_STEP_PULSE_MICROSECONDS 3 
+		#define DEFAULT_STEPPER_IDLE_LOCK_TIME 200 // 200ms
+		
+		#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t
+		#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t
+		#define DEFAULT_INVERT_ST_ENABLE 0 // boolean
+		#define DEFAULT_INVERT_LIMIT_PINS 1 // boolean
+		#define DEFAULT_INVERT_PROBE_PIN 0 // boolean 
+		
+		#define DEFAULT_STATUS_REPORT_MASK 1
+		
+		#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
+		#define DEFAULT_ARC_TOLERANCE 0.002 // mm
+		#define DEFAULT_REPORT_INCHES 0 // false
+		
+		#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false
+		#define DEFAULT_HARD_LIMIT_ENABLE 0  // false
+		
+		#define DEFAULT_HOMING_ENABLE 1  
+		#define DEFAULT_HOMING_DIR_MASK 17 
+		#define DEFAULT_HOMING_FEED_RATE 200.0 // mm/min
+		#define DEFAULT_HOMING_SEEK_RATE 2000.0 // mm/min
+		#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k)
+		#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_Y_STEPS_PER_MM 400.0
+		#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode
+		#define DEFAULT_A_STEPS_PER_MM 400.0
+		#define DEFAULT_B_STEPS_PER_MM 400.0
+		#define DEFAULT_C_STEPS_PER_MM 100.0 // This is percent in servo mode
+		
+		
+		#define DEFAULT_X_MAX_RATE 30000.0 // mm/min
+		#define DEFAULT_Y_MAX_RATE 30000.0 // mm/min
+		#define DEFAULT_Z_MAX_RATE 8000.0 // mm/min
+		#define DEFAULT_A_MAX_RATE 30000.0 // mm/min
+		#define DEFAULT_B_MAX_RATE 30000.0 // mm/min
+		#define DEFAULT_C_MAX_RATE 8000.0 // mm/min
+		
+		#define DEFAULT_X_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+		#define DEFAULT_Y_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+		#define DEFAULT_Z_ACCELERATION (100.0*60*60) 
+		#define DEFAULT_A_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+		#define DEFAULT_B_ACCELERATION (1500.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+		#define DEFAULT_C_ACCELERATION (100.0*60*60) 
+		
+		#define DEFAULT_X_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value.
+		#define DEFAULT_Y_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value.
+		#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode	
+		#define DEFAULT_A_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value.
+		#define DEFAULT_B_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value.
+		#define DEFAULT_C_MAX_TRAVEL 100.0 // This is percent in servo mode				
+		
+#endif

 	// ================= common to all machines ================================
 	
@@ -1168,17 +1320,25 @@
 		
 		#define X_STEP_BIT    0  // don't change
 		#define Y_STEP_BIT    1  // don't change
-		#define Z_STEP_BIT    2  // don't change
-		#define STEP_MASK       B111 // don't change
+		#define Z_STEP_BIT    2  // don't change		
+		#define A_STEP_BIT    3  // don't change
+		#define B_STEP_BIT    4  // don't change
+		#define C_STEP_BIT    5  // don't change
+		#define STEP_MASK       B111111 // don't change
 		
 		#define X_DIRECTION_BIT   0 // don't change
 		#define Y_DIRECTION_BIT   1  // don't change
 		#define Z_DIRECTION_BIT   2  // don't change
-		
+		#define A_DIRECTION_BIT   3 // don't change
+		#define B_DIRECTION_BIT   4  // don't change
+		#define C_DIRECTION_BIT   5  // don't change
+
 		#define X_LIMIT_BIT      	0  // don't change
 		#define Y_LIMIT_BIT      	1  // don't change
 		#define Z_LIMIT_BIT     	2  // don't change
-		
+		#define A_LIMIT_BIT      	3  // don't change
+		#define B_LIMIT_BIT      	4  // don't change
+		#define C_LIMIT_BIT     	5  // don't change		
 		
 		#define PROBE_MASK        1 // don't change		
 		
--- a/Grbl_Esp32/data/index.html.gz
+++ b/Grbl_Esp32/data/index.html.gz
--- a/Grbl_Esp32/defaults.h
+++ b/Grbl_Esp32/defaults.h
@@ -66,18 +66,31 @@
  #define DEFAULT_X_STEPS_PER_MM 800.0
  #define DEFAULT_Y_STEPS_PER_MM 800.0
  #define DEFAULT_Z_STEPS_PER_MM 800.0
+  #define DEFAULT_A_STEPS_PER_MM 100.0
+  #define DEFAULT_B_STEPS_PER_MM 100.0
+  #define DEFAULT_C_STEPS_PER_MM 100.0
  
  #define DEFAULT_X_MAX_RATE 5000.0 // mm/min
  #define DEFAULT_Y_MAX_RATE 4000.0 // mm/min
  #define DEFAULT_Z_MAX_RATE 3000.0 // mm/min
+  #define DEFAULT_A_MAX_RATE 1000.0 // mm/min
+  #define DEFAULT_B_MAX_RATE 1000.0 // mm/min
+  #define DEFAULT_C_MAX_RATE 1000.0 // mm/min
  
  #define DEFAULT_X_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
  #define DEFAULT_Y_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
  #define DEFAULT_Z_ACCELERATION (200.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+  #define DEFAULT_A_ACCELERATION (100.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+  #define DEFAULT_B_ACCELERATION (100.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+  #define DEFAULT_C_ACCELERATION (100.0*60*60) // 10*60*60 mm/min^2 = 10 mm/sec^2
+  
  
  #define DEFAULT_X_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value.
  #define DEFAULT_Y_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value.
  #define DEFAULT_Z_MAX_TRAVEL 300.0 // mm NOTE: Must be a positive value.
+  #define DEFAULT_A_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value.
+  #define DEFAULT_B_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value.
+  #define DEFAULT_C_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value.
  

 #endif
--- a/Grbl_Esp32/gcode.cpp
+++ b/Grbl_Esp32/gcode.cpp
@@ -449,7 +449,7 @@ uint8_t gc_execute_line(char *line, uint8_t client)
 					FAIL(STATUS_GCODE_MAX_VALUE_EXCEEDED);
 				}
 				grbl_sendf(CLIENT_ALL, "[MSG:Tool No: %d]\r\n", int_value);
-				gc_block.values.t = int_value; 
+				gc_state.tool = int_value;
 				break;
 			case 'X':
 				word_bit = WORD_X;
@@ -1180,8 +1180,8 @@ uint8_t gc_execute_line(char *line, uint8_t client)
 		pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use.
 	} // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already.

-	// [5. Select tool ]: NOT SUPPORTED. Only tracks tool value.
-	gc_state.tool = gc_block.values.t;
+	// [5. Select tool ]: NOT SUPPORTED. Only tracks tool value.	
+	//	gc_state.tool = gc_block.values.t;

 	// [6. Change tool ]: NOT SUPPORTED

@@ -1270,7 +1270,7 @@ uint8_t gc_execute_line(char *line, uint8_t client)
 		// and absolute and incremental modes.
 		pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag.
 		if (axis_command) {
-			mc_line(gc_block.values.xyz, pl_data);
+			mc_line(gc_block.values.xyz, pl_data);  // kinematics kinematics not used for homing righ now
 		}
 		mc_line(gc_block.values.ijk, pl_data);
 		memcpy(gc_state.position, gc_block.values.ijk, N_AXIS*sizeof(float));
@@ -1300,10 +1300,12 @@ uint8_t gc_execute_line(char *line, uint8_t client)
 		if (axis_command == AXIS_COMMAND_MOTION_MODE) {
 			uint8_t gc_update_pos = GC_UPDATE_POS_TARGET;
 			if (gc_state.modal.motion == MOTION_MODE_LINEAR) {
-				mc_line(gc_block.values.xyz, pl_data);
+				//mc_line(gc_block.values.xyz, pl_data);
+				mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position);
 			} else if (gc_state.modal.motion == MOTION_MODE_SEEK) {
 				pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag.
-				mc_line(gc_block.values.xyz, pl_data);
+				//mc_line(gc_block.values.xyz, pl_data);
+				mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position);
 			} else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) {
 				mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r,
 				       axis_0, axis_1, axis_linear, bit_istrue(gc_parser_flags,GC_PARSER_ARC_IS_CLOCKWISE));
--- a/Grbl_Esp32/grbl_limits.cpp
+++ b/Grbl_Esp32/grbl_limits.cpp
@@ -280,6 +280,15 @@ void limits_init()
 		#ifdef Z_LIMIT_PIN
 			pinMode(Z_LIMIT_PIN, INPUT_PULLUP);
 		#endif
+		#ifdef A_LIMIT_PIN
+			pinMode(A_LIMIT_PIN, INPUT_PULLUP);
+		#endif
+		#ifdef B_LIMIT_PIN
+			pinMode(B_LIMIT_PIN, INPUT_PULLUP);
+		#endif
+		#ifdef C_LIMIT_PIN
+			pinMode(C_LIMIT_PIN, INPUT_PULLUP);
+		#endif
 	#else
 		#ifdef X_LIMIT_PIN
 			pinMode(X_LIMIT_PIN, INPUT); // input no pullup
@@ -290,6 +299,15 @@ void limits_init()
 		#ifdef Z_LIMIT_PIN
 			pinMode(Z_LIMIT_PIN, INPUT);	
 		#endif
+		#ifdef A_LIMIT_PIN
+			pinMode(A_LIMIT_PIN, INPUT); // input no pullup
+		#endif
+		#ifdef B_LIMIT_PIN
+			pinMode(B_LIMIT_PIN, INPUT);
+		#endif
+		#ifdef C_LIMIT_PIN
+			pinMode(C_LIMIT_PIN, INPUT);	
+		#endif
  #endif
   
  if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) {
@@ -303,6 +321,15 @@ void limits_init()
 		#ifdef Z_LIMIT_PIN
 			attachInterrupt(digitalPinToInterrupt(Z_LIMIT_PIN), isr_limit_switches, CHANGE);
 		#endif
+		#ifdef A_LIMIT_PIN
+			attachInterrupt(digitalPinToInterrupt(A_LIMIT_PIN), isr_limit_switches, CHANGE);
+		#endif
+		#ifdef B_LIMIT_PIN
+			attachInterrupt(digitalPinToInterrupt(B_LIMIT_PIN), isr_limit_switches, CHANGE);
+		#endif
+		#ifdef C_LIMIT_PIN
+			attachInterrupt(digitalPinToInterrupt(C_LIMIT_PIN), isr_limit_switches, CHANGE);
+		#endif
  } else {
    limits_disable();
  }
@@ -327,6 +354,9 @@ void limits_disable()
  detachInterrupt(X_LIMIT_BIT);
  detachInterrupt(Y_LIMIT_BIT);
  detachInterrupt(Z_LIMIT_BIT);  
+  detachInterrupt(A_LIMIT_BIT);
+  detachInterrupt(B_LIMIT_BIT);
+  detachInterrupt(C_LIMIT_BIT);  
 }


@@ -349,6 +379,20 @@ uint8_t limits_get_state()
 	#ifdef Z_LIMIT_PIN
 		pin += (digitalRead(Z_LIMIT_PIN) << Z_AXIS);
 	#endif
+	
+	#ifdef A_LIMIT_PIN
+		pin += (digitalRead(A_LIMIT_PIN) << A_AXIS);
+	#endif
+	
+	#ifdef B_LIMIT_PIN
+		pin += (digitalRead(B_LIMIT_PIN) << B_AXIS);
+	#endif
+	
+	#ifdef C_LIMIT_PIN
+		pin += (digitalRead(C_LIMIT_PIN) << C_AXIS);
+	#endif
+	
+	//grbl_sendf(CLIENT_SERIAL, "[MSG: Limit pins %d]\r\n", pin);

 	#ifdef INVERT_LIMIT_PIN_MASK // not normally used..unless you have both normal and inverted switches
 		pin ^= INVERT_LIMIT_PIN_MASK;
@@ -357,14 +401,20 @@ uint8_t limits_get_state()
 	if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { 
 		pin ^= LIMIT_MASK;
 	}
+	
+	//grbl_sendf(CLIENT_SERIAL, "[MSG: Limit Inverted %d]\r\n", pin);
  
 	if (pin) {
 		uint8_t idx;
 		for (idx=0; idx<N_AXIS; idx++) {
-			if (pin & get_limit_pin_mask(idx)) { limit_state |= (1 << idx); }
+			if (pin & get_limit_pin_mask(idx)) {
+				limit_state |= (1 << idx);
+			}
 		}
 	}
 	
+	//grbl_sendf(CLIENT_SERIAL, "[MSG: Limit State %d]\r\n", limit_state);
+	
 	return(limit_state);	
 }

--- a/Grbl_Esp32/kinematics.cpp
+++ b/Grbl_Esp32/kinematics.cpp
@@ -0,0 +1,91 @@
+/*
+  kinematics.cpp - Implements simple inverse kinematics for Grbl_ESP32
+  Part of Grbl_ESP32
+
+  Copyright (c) 2019 Barton Dring @buildlog
+
+
+  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/>.
+
+	Inverse kinematics determine the joint parameters required to get to a position
+	in 3D space. Grbl will still work as 3 axes of steps, but these steps could
+	represent angles, etc instead of linear units.
+
+	Unless forward kinematics are applied to the reporting, Grbl will report raw joint
+	values instead of the normal Cartesian positions
+
+	How it works...
+
+	If you tell it to go to X10 Y10 Z10 in Cartesian space, for example, the equations
+  will convert those values to the required joint values. In the case of a polar machine, X represents the radius,
+	Y represents the polar degrees and Z would be unchanged.
+
+	In most cases, a straight line in Cartesian space could cause a curve in the new system.
+	To fix this, the line is broken into very small segments and each segment is converted
+	to the new space. While each segment is also distorted, the amount is so small it cannot be seen.
+
+	This segmentation is how normal Grbl draws arcs.
+
+	Feed Rate
+
+	Feed rate is given in steps/time. Due to the new coordinate units and non linearity issues, the
+	feed rate may need to be adjusted. The ratio of the step distances in the original coordinate system
+	determined and applied to the feed rate.
+
+	TODO:
+		Add y offset, for completeness
+		Add ZERO_NON_HOMED_AXES option
+
+
+*/
+#include "grbl.h"
+
+#ifdef USE_KINEMATICS
+
+void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position)
+{	
+	// use the current tool as a flag for converting for different machine mode.
+	
+	// target: This is the target location for the move in machine space millimeters.
+	// position: This is the previous position in machine space millimeters
+	
+	float offsets[N_AXIS];    // current work offsets
+	float new_system[N_AXIS]; // target location in the new system
+	uint8_t index;
+	
+	// skip kinematics... not implemented yet.
+	mc_line(target, pl_data);
+	return;
+	
+	// All data is in machine space and axes have different offsets, so we need to apply the offset in corrections
+	for (index = 0; index < N_AXIS; index++) {
+		offsets[index] = gc_state.coord_system[index] + gc_state.coord_offset[index]; // offset from machine coordinate system		
+	}	
+	
+	switch (gc_state.tool) {
+		case 1: // XYZ only ignore all ABC
+		
+		break;
+		case 2: // Convert XYZ to ABC (no motion on ABC)
+			
+		break;
+		default: // tool 0 or above 2 uses normal 6 axis code...no changes
+			
+		break;
+	}		
+	
+		
+}
+
+#endif
--- a/Grbl_Esp32/kinematics.h
+++ b/Grbl_Esp32/kinematics.h
@@ -0,0 +1,27 @@
+/*
+  kinematics.h - Implements simple kinematics for Grbl_ESP32
+  Part of Grbl_ESP32
+
+  Copyright (c) 2019 Barton Dring @buildlog
+      
+	 
+  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 kinematics_h
+  #define kinematics_h    
+		
+void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position);
+
+#endif
--- a/Grbl_Esp32/motion_control.cpp
+++ b/Grbl_Esp32/motion_control.cpp
@@ -26,6 +26,18 @@

 uint8_t ganged_mode = SQUARING_MODE_DUAL;

+
+// this allows kinematics to be used. 
+void mc_line_kins(float *target, plan_line_data_t *pl_data, float *position)
+{		
+	#ifndef USE_KINEMATICS	
+		mc_line(target, pl_data);
+	#else // else use kinematics
+		//grbl_sendf(CLIENT_SERIAL, "[MSG:Kin Tool %d]\r\n", gc_state.tool);
+		inverse_kinematics(target, pl_data, position);
+	#endif
+}
+
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
 // (1 minute)/feed_rate time.
--- a/Grbl_Esp32/motion_control.h
+++ b/Grbl_Esp32/motion_control.h
@@ -46,6 +46,7 @@
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
 // (1 minute)/feed_rate time.
+void mc_line_kins(float *target, plan_line_data_t *pl_data, float *position);
 void mc_line(float *target, plan_line_data_t *pl_data);

 // Execute an arc in offset mode format. position == current xyz, target == target xyz,
--- a/Grbl_Esp32/nuts_bolts.h
+++ b/Grbl_Esp32/nuts_bolts.h
@@ -27,11 +27,16 @@
 #define SOME_LARGE_VALUE 1.0E+38

 // Axis array index values. Must start with 0 and be continuous.
-#define N_AXIS 3 // Number of axes
+// Note: You set the number of axes used by changing N_AXIS.
+// Be sure to define pins or servos in cpu_map.h 
+#define N_AXIS 6 // Number of axes defined
 #define X_AXIS 0 // Axis indexing value.
 #define Y_AXIS 1
 #define Z_AXIS 2
-// #define A_AXIS 3
+#define A_AXIS 3
+#define B_AXIS 4
+#define C_AXIS 5
+

 // CoreXY motor assignments. DO NOT ALTER.
 // NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations.
--- a/Grbl_Esp32/report.cpp
+++ b/Grbl_Esp32/report.cpp
@@ -349,8 +349,8 @@ void report_probe_parameters(uint8_t client)
 {
  // Report in terms of machine position.	
 	float print_position[N_AXIS];
-	char probe_rpt[50];	// the probe report we are building here
-	char temp[30];
+	char probe_rpt[100];	// the probe report we are building here
+	char temp[60];
 	
 	strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters
  
@@ -374,8 +374,8 @@ void report_ngc_parameters(uint8_t client)
 {
  float coord_data[N_AXIS];
  uint8_t coord_select;
-	char temp[50];
-	char ngc_rpt[400];	
+	char temp[60];
+	char ngc_rpt[500];	
 	
 	ngc_rpt[0] = '\0';	
 	
@@ -414,7 +414,7 @@ void report_ngc_parameters(uint8_t client)
 	
 	grbl_send(client, ngc_rpt);
 	
-  report_probe_parameters(client); 
+	report_probe_parameters(client);
 }


--- a/Grbl_Esp32/servo_axis.cpp
+++ b/Grbl_Esp32/servo_axis.cpp
@@ -38,9 +38,19 @@ static TaskHandle_t servosSyncTaskHandle = 0;
 	ServoAxis Z_Servo_Axis(Z_AXIS, SERVO_Z_PIN, SERVO_Z_CHANNEL_NUM);
 #endif

+#ifdef SERVO_A_PIN
+	ServoAxis A_Servo_Axis(A_AXIS, SERVO_A_PIN, SERVO_A_CHANNEL_NUM);
+#endif
+#ifdef SERVO_B_PIN
+	ServoAxis B_Servo_Axis(B_AXIS, SERVO_B_PIN, SERVO_B_CHANNEL_NUM);
+#endif
+#ifdef SERVO_C_PIN
+	ServoAxis C_Servo_Axis(C_AXIS, SERVO_C_PIN, SERVO_C_CHANNEL_NUM);
+#endif
+
 void init_servos()
 {
-	grbl_send(CLIENT_SERIAL, "[MSG: Init Servos]\r\n");
+	//grbl_send(CLIENT_SERIAL, "[MSG: Init Servos]\r\n");
 	#ifdef SERVO_X_PIN
 		grbl_send(CLIENT_SERIAL, "[MSG: Init X Servo]\r\n");
 		X_Servo_Axis.init();
@@ -61,6 +71,28 @@ void init_servos()
 		Z_Servo_Axis.set_homing_type(SERVO_HOMING_TARGET);
 		Z_Servo_Axis.set_homing_position(SERVO_Z_RANGE_MAX);
 	#endif
+	
+	#ifdef SERVO_A_PIN
+		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);
+		A_Servo_Axis.set_disable_on_alarm(false);
+		A_Servo_Axis.set_disable_with_steppers(false);
+	#endif
+	#ifdef SERVO_B_PIN
+		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");
+		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);
+		C_Servo_Axis.set_homing_position(SERVO_C_RANGE_MAX);
+	#endif
+	
  
  // setup a task that will calculate the determine and set the servo positions
  xTaskCreatePinnedToCore(  servosSyncTask,    // task
@@ -93,6 +125,16 @@ void servosSyncTask(void *pvParameters)
 		#ifdef SERVO_Z_PIN
 			Z_Servo_Axis.set_location();
 		#endif
+		#ifdef SERVO_A_PIN
+			A_Servo_Axis.set_location();			
+		#endif
+		#ifdef SERVO_B_PIN
+			B_Servo_Axis.set_location();
+		#endif
+		#ifdef SERVO_C_PIN
+			C_Servo_Axis.set_location();
+		#endif
+		
 		vTaskDelayUntil(&xLastWakeTime, xServoFrequency);
  }
 } 
@@ -104,10 +146,12 @@ ServoAxis::ServoAxis(uint8_t axis, uint8_t pin_num, uint8_t channel_num) // cons
  _axis = axis;
 	_pin_num = pin_num;
 	_channel_num = channel_num;	
+	_showError = true; // this will be used to show error only once
 }

 void ServoAxis::init()
 {
+	_cal_is_valid();
 	ledcSetup(_channel_num, _pwm_freq, _pwm_resolution_bits);
 	ledcAttachPin(_pin_num, _channel_num);
 	disable();
@@ -207,10 +251,9 @@ void ServoAxis::disable()
 bool ServoAxis::_cal_is_valid()
 {
  bool settingsOK = true;
-  static bool showError = true; // this will be used to show error only once
-
+  
 	if ( (settings.steps_per_mm[_axis] < SERVO_CAL_MIN) || (settings.steps_per_mm[_axis] > SERVO_CAL_MAX) ) {
-		if (showError) {
+		if (_showError) {
 			grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($10%d) Error: %4.4f s/b between %.2f and %.2f]\r\n", _axis, settings.steps_per_mm[_axis], SERVO_CAL_MIN, SERVO_CAL_MAX);
 		}
 		settingsOK = false;
@@ -218,13 +261,13 @@ bool ServoAxis::_cal_is_valid()

 	// Note: Max travel is set positive via $$, but stored as a negative number
 	if ( (settings.max_travel[_axis] < -SERVO_CAL_MAX) || (settings.max_travel[_axis] > -SERVO_CAL_MIN) ) {
-		if (showError) {
+		if (_showError) {
 			grbl_sendf(CLIENT_SERIAL, "[MSG:Servo cal ($13%d) Error: %4.4f s/b between %.2f and %.2f]\r\n", _axis, -settings.max_travel[_axis], SERVO_CAL_MIN, SERVO_CAL_MAX);
 		}
 		settingsOK = false;
 	}
 	
-	//showError = false;  // to show error once 
+	_showError = false;  // to show error once 
 	return settingsOK;
 }

--- a/Grbl_Esp32/servo_axis.h
+++ b/Grbl_Esp32/servo_axis.h
@@ -108,6 +108,8 @@ class ServoAxis{
 		int _axis; // these should be assign in constructor using Grbl X_AXIS type values
 		int _pin_num; // The GPIO pin being used
 		int _channel_num; // The PWM channel
+		bool _showError;
+		
 		uint32_t _pwm_freq = SERVO_PULSE_FREQ;
 		uint32_t _pwm_resolution_bits = SERVO_PULSE_RES_BITS;
 		float _pulse_min = SERVO_MIN_PULSE; // in pwm counts
@@ -125,6 +127,7 @@ class ServoAxis{
 		bool _validate_cal_settings();
 		void _write_pwm(uint32_t duty);
 		bool _cal_is_valid(); // checks to see if calibration values are in acceptable range 
+		
 };

 #endif
--- a/Grbl_Esp32/settings.cpp
+++ b/Grbl_Esp32/settings.cpp
@@ -100,6 +100,29 @@ void settings_restore(uint8_t restore_flag) {
    settings.max_travel[X_AXIS] = (-DEFAULT_X_MAX_TRAVEL);
    settings.max_travel[Y_AXIS] = (-DEFAULT_Y_MAX_TRAVEL);
    settings.max_travel[Z_AXIS] = (-DEFAULT_Z_MAX_TRAVEL);
+	
+	#if N_AXIS > A_AXIS
+		 settings.steps_per_mm[A_AXIS] = DEFAULT_A_STEPS_PER_MM;
+		 settings.max_rate[A_AXIS] = DEFAULT_A_MAX_RATE;
+		 settings.acceleration[A_AXIS] = DEFAULT_A_ACCELERATION;
+		 settings.max_travel[A_AXIS] = (-DEFAULT_A_MAX_TRAVEL);
+	#endif
+	
+	#if N_AXIS > B_AXIS
+		 settings.steps_per_mm[B_AXIS] = DEFAULT_B_STEPS_PER_MM;
+		 settings.max_rate[B_AXIS] = DEFAULT_B_MAX_RATE;
+		 settings.acceleration[B_AXIS] = DEFAULT_B_ACCELERATION;
+		 settings.max_travel[B_AXIS] = (-DEFAULT_B_MAX_TRAVEL);
+	#endif
+	
+	#if N_AXIS > C_AXIS
+		 settings.steps_per_mm[C_AXIS] = DEFAULT_C_STEPS_PER_MM;
+		 settings.max_rate[C_AXIS] = DEFAULT_C_MAX_RATE;
+		 settings.acceleration[C_AXIS] = DEFAULT_C_ACCELERATION;
+		 settings.max_travel[C_AXIS] = (-DEFAULT_C_MAX_TRAVEL);
+	#endif
+	
+	

    write_global_settings();
  }
--- a/Grbl_Esp32/stepper.cpp
+++ b/Grbl_Esp32/stepper.cpp
@@ -67,7 +67,17 @@ typedef struct {
 	// Used by the bresenham line algorithm
 	uint32_t counter_x,        // Counter variables for the bresenham line tracer
 	         counter_y,
-	         counter_z;
+	         counter_z
+			 #ifdef A_AXIS
+				, counter_a
+			#endif
+			#ifdef B_AXIS
+				, counter_b
+			#endif
+			#ifdef C_AXIS
+				, counter_c
+            #endif
+			;
 #ifdef STEP_PULSE_DELAY
 	uint8_t step_bits;  // Stores out_bits output to complete the step pulse delay
 #endif
@@ -198,58 +208,6 @@ static st_prep_t prep;


 */
-
-#ifdef USE_RMT_STEPS
-// Set stepper pulse output pins
-inline IRAM_ATTR static void stepperRMT_Outputs()
-{
-
-#ifdef  X_STEP_PIN
-	if(st.step_outbits & (1<<X_AXIS)) {
-	#ifndef X_STEP_B_PIN // if not a ganged axis
-			RMT.conf_ch[X_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-			RMT.conf_ch[X_RMT_CHANNEL].conf1.tx_start = 1;
-	#else // it is a ganged axis
-			if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A) ) {
-				RMT.conf_ch[X_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-				RMT.conf_ch[X_RMT_CHANNEL].conf1.tx_start = 1;			}
-			
-			if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B) ) {
-				RMT.conf_ch[X_B_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-				RMT.conf_ch[X_B_RMT_CHANNEL].conf1.tx_start = 1;
-			}			
-	#endif
-	}
-
-#endif
-
-#ifdef  Y_STEP_PIN
-	if(st.step_outbits & (1<<Y_AXIS)) {
-#ifndef Y_STEP_B_PIN // if not a ganged axis
-		RMT.conf_ch[Y_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-		RMT.conf_ch[Y_RMT_CHANNEL].conf1.tx_start = 1;
-#else // it is a ganged axis
-		if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A) ) {
-			RMT.conf_ch[Y_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-			RMT.conf_ch[Y_RMT_CHANNEL].conf1.tx_start = 1;
-		}		
-		if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B) ) {
-			RMT.conf_ch[Y_B_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-			RMT.conf_ch[Y_B_RMT_CHANNEL].conf1.tx_start = 1;
-		}		
-#endif
-	}
-#endif
-
-#ifdef  Z_STEP_PIN
-	if(st.step_outbits & (1<<Z_AXIS)) {
-		RMT.conf_ch[Z_RMT_CHANNEL].conf1.mem_rd_rst = 1;
-		RMT.conf_ch[Z_RMT_CHANNEL].conf1.tx_start = 1;
-	}
-#endif
-}
-#endif
-
 // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller
 // int8 variables and update position counters only when a segment completes. This can get complicated
 // with probing and homing cycles that require true real-time positions.
@@ -294,6 +252,7 @@ void IRAM_ATTR onStepperDriverTimer(void *para)  // ISR It is time to take a ste

 				// Initialize Bresenham line and distance counters
 				st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1);
+				// TODO ABC
 			}
 			st.dir_outbits = st.exec_block->direction_bits ^ settings.dir_invert_mask;

@@ -302,6 +261,17 @@ void IRAM_ATTR onStepperDriverTimer(void *para)  // ISR It is time to take a ste
 			st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level;
 			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
+			   st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.exec_segment->amass_level;
+			#endif
+			#ifdef B_AXIS
+			   st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.exec_segment->amass_level;
+			#endif
+			#ifdef C_AXIS
+			   st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level;
+			#endif
+			
 #endif

 #ifdef VARIABLE_SPINDLE
@@ -380,6 +350,49 @@ void IRAM_ATTR onStepperDriverTimer(void *para)  // ISR It is time to take a ste
 			sys_position[Z_AXIS]++;
 		}
 	}
+	
+#if N_AXIS > A_AXIS
+   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
+     st.counter_a += st.steps[A_AXIS];
+   #else
+     st.counter_a += st.exec_block->steps[A_AXIS];
+   #endif
+   if (st.counter_a > st.exec_block->step_event_count) {
+     st.step_outbits |= (1<<A_STEP_BIT);
+     st.counter_a -= st.exec_block->step_event_count;
+     if (st.exec_block->direction_bits & (1<<A_DIRECTION_BIT)) { sys_position[A_AXIS]--; }
+     else { sys_position[A_AXIS]++; }
+   }
+#endif
+  
+  
+ #if N_AXIS > B_AXIS
+   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
+     st.counter_b += st.steps[B_AXIS];
+   #else
+     st.counter_b += st.exec_block->steps[B_AXIS];
+   #endif
+   if (st.counter_b > st.exec_block->step_event_count) {
+     st.step_outbits |= (1<<B_STEP_BIT);
+     st.counter_b -= st.exec_block->step_event_count;
+     if (st.exec_block->direction_bits & (1<<B_DIRECTION_BIT)) { sys_position[B_AXIS]--; }
+     else { sys_position[B_AXIS]++; }
+   }
+ #endif
+
+#if N_AXIS > C_AXIS
+   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
+     st.counter_c += st.steps[C_AXIS];
+   #else
+     st.counter_c += st.exec_block->steps[C_AXIS];
+   #endif
+   if (st.counter_c > st.exec_block->step_event_count) {
+     st.step_outbits |= (1<<C_STEP_BIT);
+     st.counter_c -= st.exec_block->step_event_count;
+     if (st.exec_block->direction_bits & (1<<C_DIRECTION_BIT)) { sys_position[C_AXIS]--; }
+     else { sys_position[C_AXIS]++; }
+   }
+ #endif

 	// During a homing cycle, lock out and prevent desired axes from moving.
 	if (sys.state == STATE_HOMING) {
@@ -414,8 +427,14 @@ void IRAM_ATTR onStepperDriverTimer(void *para)  // ISR It is time to take a ste
 void stepper_init()
 {
 	
-	#ifdef USE_TMC2130
-		TMC2130_Init();
+	// make the stepper disable pin an output
+	#ifdef STEPPERS_DISABLE_PIN
+		pinMode(STEPPERS_DISABLE_PIN, OUTPUT);
+		set_stepper_disable(true);
+	#endif
+	
+	#ifdef USE_TRINAMIC
+		Trinamic_Init();
 	#endif
 	
 	#ifdef USE_RMT_STEPS
@@ -444,6 +463,19 @@ void stepper_init()
 		#ifdef Z_STEP_B_PIN
 			pinMode(Z_STEP_B_PIN, OUTPUT);
 		#endif
+		
+		#ifdef A_STEP_PIN
+			pinMode(A_STEP_PIN, OUTPUT);
+		#endif
+		
+		#ifdef B_STEP_PIN
+			pinMode(B_STEP_PIN, OUTPUT);
+		#endif
+		
+		#ifdef C_STEP_PIN
+			pinMode(C_STEP_PIN, OUTPUT);
+		#endif	
+		
 	#endif

 	// make the direction pins outputs
@@ -456,14 +488,18 @@ void stepper_init()
 	#ifdef Z_DIRECTION_PIN
 		pinMode(Z_DIRECTION_PIN, OUTPUT);
 	#endif
-
-
-
-	// make the stepper disable pin an output
-	#ifdef STEPPERS_DISABLE_PIN
-		pinMode(STEPPERS_DISABLE_PIN, OUTPUT);
-		set_stepper_disable(true);
+	#ifdef A_DIRECTION_PIN
+		pinMode(A_DIRECTION_PIN, OUTPUT);
 	#endif
+	#ifdef B_DIRECTION_PIN
+		pinMode(B_DIRECTION_PIN, OUTPUT);
+	#endif
+	#ifdef C_DIRECTION_PIN
+		pinMode(C_DIRECTION_PIN, OUTPUT);
+	#endif
+
+
+	

 // setup stepper timer interrupt

@@ -573,6 +609,38 @@ void initRMT()
 	rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
 #endif

+#ifdef A_STEP_PIN
+	rmt_set_source_clk( (rmt_channel_t)A_RMT_CHANNEL, RMT_BASECLK_APB);
+	rmtConfig.channel = (rmt_channel_t)A_RMT_CHANNEL;
+	rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, A_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW;
+	rmtConfig.gpio_num = A_STEP_PIN;  // TODO
+	rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
+	rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
+	rmt_config(&rmtConfig);
+	rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
+#endif
+
+#ifdef B_STEP_PIN
+	rmt_set_source_clk( (rmt_channel_t)B_RMT_CHANNEL, RMT_BASECLK_APB);
+	rmtConfig.channel = (rmt_channel_t)B_RMT_CHANNEL;
+	rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, B_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW;
+	rmtConfig.gpio_num = B_STEP_PIN;  // TODO
+	rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
+	rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
+	rmt_config(&rmtConfig);
+	rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
+#endif
+
+#ifdef C_STEP_PIN
+	rmt_set_source_clk( (rmt_channel_t)C_RMT_CHANNEL, RMT_BASECLK_APB);
+	rmtConfig.channel = (rmt_channel_t)C_RMT_CHANNEL;
+	rmtConfig.tx_config.idle_level = bit_istrue(settings.step_invert_mask, C_AXIS) ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW;
+	rmtConfig.gpio_num = C_STEP_PIN;  // TODO
+	rmtItem[0].level0 = rmtConfig.tx_config.idle_level;
+	rmtItem[0].level1 = !rmtConfig.tx_config.idle_level;
+	rmt_config(&rmtConfig);
+	rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
+#endif


 }
@@ -651,6 +719,17 @@ void set_direction_pins_on(uint8_t onMask)
 #ifdef Z_DIRECTION_PIN
 	digitalWrite(Z_DIRECTION_PIN, (onMask & (1<<Z_AXIS)));
 #endif
+#ifdef A_DIRECTION_PIN
+	digitalWrite(A_DIRECTION_PIN, (onMask & (1<<A_AXIS)));
+#endif
+#ifdef B_DIRECTION_PIN
+	digitalWrite(B_DIRECTION_PIN, (onMask & (1<<B_AXIS)));
+#endif
+#ifdef C_DIRECTION_PIN
+	digitalWrite(C_DIRECTION_PIN, (onMask & (1<<C_AXIS)));
+#endif
+
+
 }

 #ifndef USE_GANGED_AXES
@@ -670,6 +749,10 @@ void set_stepper_pins_on(uint8_t onMask)
 #ifdef Z_STEP_PIN
 	digitalWrite(Z_STEP_PIN, (onMask & (1<<Z_AXIS)));
 #endif
+
+#ifdef A_STEP_PIN
+	digitalWrite(A_STEP_PIN, (onMask & (1<<A_AXIS)));
+#endif
 }
 #else // we use ganged axes
 void set_stepper_pins_on(uint8_t onMask)
@@ -719,6 +802,82 @@ void set_stepper_pins_on(uint8_t onMask)
 #endif


+}
+#endif
+
+#ifdef USE_RMT_STEPS
+// Set stepper pulse output pins
+inline IRAM_ATTR static void stepperRMT_Outputs()
+{
+	
+#ifdef  X_STEP_PIN
+	if(st.step_outbits & (1<<X_AXIS)) {
+	#ifndef X_STEP_B_PIN // if not a ganged axis
+			RMT.conf_ch[X_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+			RMT.conf_ch[X_RMT_CHANNEL].conf1.tx_start = 1;
+	#else // it is a ganged axis
+			if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A) ) {
+				RMT.conf_ch[X_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+				RMT.conf_ch[X_RMT_CHANNEL].conf1.tx_start = 1;			}
+			
+			if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B) ) {
+				RMT.conf_ch[X_B_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+				RMT.conf_ch[X_B_RMT_CHANNEL].conf1.tx_start = 1;
+			}			
+	#endif
+	}
+
+#endif
+
+#ifdef  Y_STEP_PIN
+	if(st.step_outbits & (1<<Y_AXIS)) {
+#ifndef Y_STEP_B_PIN // if not a ganged axis
+		RMT.conf_ch[Y_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+		RMT.conf_ch[Y_RMT_CHANNEL].conf1.tx_start = 1;
+#else // it is a ganged axis
+		if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_A) ) {
+			RMT.conf_ch[Y_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+			RMT.conf_ch[Y_RMT_CHANNEL].conf1.tx_start = 1;
+		}		
+		if ( (ganged_mode == SQUARING_MODE_DUAL) || (ganged_mode == SQUARING_MODE_B) ) {
+			RMT.conf_ch[Y_B_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+			RMT.conf_ch[Y_B_RMT_CHANNEL].conf1.tx_start = 1;
+		}		
+#endif
+	}
+#endif
+
+#ifdef  Z_STEP_PIN
+	if(st.step_outbits & (1<<Z_AXIS)) {
+		RMT.conf_ch[Z_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+		RMT.conf_ch[Z_RMT_CHANNEL].conf1.tx_start = 1;
+	}
+#endif
+
+#ifdef  A_STEP_PIN
+	if(st.step_outbits & (1<<A_AXIS)) {
+		
+		RMT.conf_ch[A_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+		RMT.conf_ch[A_RMT_CHANNEL].conf1.tx_start = 1;
+	}
+#endif
+
+#ifdef  B_STEP_PIN
+	if(st.step_outbits & (1<<B_AXIS)) {
+		
+		RMT.conf_ch[B_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+		RMT.conf_ch[B_RMT_CHANNEL].conf1.tx_start = 1;
+	}
+#endif
+
+#ifdef  C_STEP_PIN
+	if(st.step_outbits & (1<<C_AXIS)) {
+		
+		RMT.conf_ch[C_RMT_CHANNEL].conf1.mem_rd_rst = 1;
+		RMT.conf_ch[C_RMT_CHANNEL].conf1.tx_start = 1;
+	}
+#endif
+
 }
 #endif

@@ -728,6 +887,8 @@ void st_go_idle()
 	// Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
 	Stepper_Timer_Stop();
 	busy = false;
+	
+	

 	bool pin_state = false;
 	// Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
@@ -1324,11 +1485,15 @@ void IRAM_ATTR Stepper_Timer_Stop()


 void set_stepper_disable(uint8_t isOn)  // isOn = true // to disable
-{
+{	
+	#ifdef TRINAMIC
+		return;
+	#endif
+	
 	if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) {
 		isOn = !isOn;    // Apply pin invert.
 	}
-
+	
 #ifdef STEPPERS_DISABLE_PIN
 	digitalWrite(STEPPERS_DISABLE_PIN, isOn );
 #endif
@@ -1354,4 +1519,3 @@ bool get_stepper_disable()   // returns true if steppers are disabled



-
--- a/Grbl_Esp32/stepper.h
+++ b/Grbl_Esp32/stepper.h
@@ -32,7 +32,7 @@


 #include "grbl.h"
-//#include "config.h"
+#include "config.h"

 // Some useful constants.
 #define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
@@ -81,8 +81,11 @@ extern uint8_t ganged_mode;
 // -- Task handles for use in the notifications
 void IRAM_ATTR onSteppertimer();
 void IRAM_ATTR onStepperOffTimer();
-void stepper_init();
-void initRMT(); 
+
+  void initRMT();
+  inline IRAM_ATTR static void stepperRMT_Outputs();
+
+void stepper_init(); 

 // Enable steppers, but cycle does not start unless called by motion control or realtime command.
 void st_wake_up();
@@ -124,4 +127,4 @@ void Stepper_Timer_WritePeriod(uint64_t alarm_val);
 void Stepper_Timer_Start();
 void Stepper_Timer_Stop();

-#endif
+#endif
--- a/Grbl_Esp32/system.cpp
+++ b/Grbl_Esp32/system.cpp
@@ -160,7 +160,7 @@ uint8_t system_execute_line(char *line, uint8_t client)
          if ( line[2] != 0 ) { return(STATUS_INVALID_STATEMENT); }
          else { report_ngc_parameters(client); }
          break;
-        case 'H' : // Perform homing cycle [IDLE/ALARM]
+        case 'H' : // Perform homing cycle [IDLE/ALARM]  $H
          if (bit_isfalse(settings.flags,BITFLAG_HOMING_ENABLE)) {return(STATUS_SETTING_DISABLED); }
          if (system_check_safety_door_ajar()) { return(STATUS_CHECK_DOOR); } // Block if safety door is ajar.
          sys.state = STATE_HOMING; // Set system state variable
@@ -172,6 +172,9 @@ uint8_t system_execute_line(char *line, uint8_t client)
                case 'X': mc_homing_cycle(HOMING_CYCLE_X); break;
                case 'Y': mc_homing_cycle(HOMING_CYCLE_Y); break;
                case 'Z': mc_homing_cycle(HOMING_CYCLE_Z); break;
+				case 'A': mc_homing_cycle(HOMING_CYCLE_A); break;
+				case 'B': mc_homing_cycle(HOMING_CYCLE_B); break;
+				case 'C': mc_homing_cycle(HOMING_CYCLE_C); break;
                default: return(STATUS_INVALID_STATEMENT);
              }
          #endif
@@ -434,7 +437,10 @@ uint8_t get_limit_pin_mask(uint8_t axis_idx)
 {
  if ( axis_idx == X_AXIS ) { return((1<<X_LIMIT_BIT)); }
  if ( axis_idx == Y_AXIS ) { return((1<<Y_LIMIT_BIT)); }
-  return((1<<Z_LIMIT_BIT));
+  if ( axis_idx == Z_AXIS ) { return((1<<Z_LIMIT_BIT)); }
+  if ( axis_idx == A_AXIS ) { return((1<<A_LIMIT_BIT)); }
+  if ( axis_idx == B_AXIS ) { return((1<<B_LIMIT_BIT)); }
+  return((1<<C_LIMIT_BIT));
 }

 // CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps.