Arc support

dda.c

@@ -830,6 +830,17 @@ void dda_clock() {
     }
     #endif
 
+    #if defined RESUME_PIN
+    if (dda->endstop_check & 0x8) {
+      if (e_min() == dda->endstop_stop_cond)
+        move_state.debounce_count_e++;
+      else
+        move_state.debounce_count_e = 0;
+
+      endstop_stop = move_state.debounce_count_e >= ENDSTOP_STEPS;
+    }
+    #endif
+
     // If an endstop is definitely triggered, stop the movement.
     if (endstop_trigger) {
       #ifdef ACCELERATION_RAMPING

dda.h

@@ -69,6 +69,7 @@ typedef struct {
 	/// Endstop handling.
   uint8_t endstop_stop; ///< Stop due to endstop trigger
   uint8_t debounce_count_x, debounce_count_y, debounce_count_z;
+  uint8_t debounce_count_e;
 } MOVE_STATE;
 
 /**
@@ -160,7 +161,7 @@ typedef struct {
   uint8_t           fast_axis;       ///< number of the fast axis
 
 	/// Endstop homing
-	uint8_t endstop_check; ///< Do we need to check endstops? 0x1=Check X, 0x2=Check Y, 0x4=Check Z
+	uint8_t endstop_check; ///< Do we need to check endstops? 0x1=Check X, 0x2=Check Y, 0x4=Check Z, 0x8=Check E
 	uint8_t endstop_stop_cond; ///< Endstop condition on which to stop motion: 0=Stop on detrigger, 1=Stop on trigger
 } DDA;
 
@@ -202,4 +203,7 @@ void dda_clock(void);
 // update current_position
 void update_current_position(void);
 
+//integer square root algorithm
+uint16_t int_sqrt(uint32_t a);
+
 #endif	/* _DDA_H */

debug.h

@@ -24,6 +24,7 @@
   #define DEBUG_PID       16
   #define DEBUG_DDA       32
   #define DEBUG_POSITION  64
+  #define DEBUG_ARC      128
 #else
 	// by setting these to zero, the compiler should optimise the relevant code out
 	#define		DEBUG_PID				0
@@ -32,6 +33,7 @@
 	#define		DEBUG_ECHO			0
   #define DEBUG_INFO       0
   #define DEBUG_DRYRUN     0
+  #define DEBUG_ARC        0
 #endif
 
 

gcode_parse.c

@@ -165,6 +165,22 @@ uint8_t gcode_parse_char(uint8_t c) {
 					if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
 						serwrite_uint8(next_target.M);
 					break;
+				case 'I':
+					if (next_target.option_inches)
+						next_target.I = decfloat_to_int(&read_digit, 25400);
+					else
+						next_target.I = decfloat_to_int(&read_digit, 1000);
+					if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
+						serwrite_int32(next_target.I);
+					break;
+				case 'J':
+					if (next_target.option_inches)
+						next_target.J = decfloat_to_int(&read_digit, 25400);
+					else
+						next_target.J = decfloat_to_int(&read_digit, 1000);
+					if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
+						serwrite_int32(next_target.J);
+					break;
 				case 'X':
 					if (next_target.option_inches)
             next_target.target.axis[X] = decfloat_to_int(&read_digit, 25400);
@@ -282,6 +298,12 @@ uint8_t gcode_parse_char(uint8_t c) {
           next_target.seen_G = 0;
           next_target.G = 0;
           break;
+			case 'I':
+				next_target.seen_I = 1;
+				break;
+			case 'J':
+				next_target.seen_J = 1;
+				break;
         case 'X':
           next_target.seen_X = 1;
           break;
@@ -340,7 +362,6 @@ uint8_t gcode_parse_char(uint8_t c) {
             // ignore
             break;
         #endif
-
         default:
           #ifdef	DEBUG
             // invalid
@@ -406,6 +427,7 @@ uint8_t gcode_parse_char(uint8_t c) {
 		}
 
 		// reset variables
+		next_target.seen_I = next_target.seen_J = \
 		next_target.seen_X = next_target.seen_Y = next_target.seen_Z = \
 			next_target.seen_E = next_target.seen_F = next_target.seen_S = \
 			next_target.seen_P = next_target.seen_T = next_target.seen_N = \

gcode_parse.h

@@ -31,6 +31,8 @@ typedef struct {
 		uint8_t					seen_Y	:1;
 		uint8_t					seen_Z	:1;
 		uint8_t					seen_E	:1;
+    uint8_t         seen_I :1; ///< ARC center X at startpoint +I.
+    uint8_t         seen_J :1; ///< ARC center Y at startpoint +I.
 		uint8_t					seen_F	:1;
 		uint8_t					seen_S	:1;
 		uint8_t					seen_P	:1;
@@ -54,6 +56,8 @@ typedef struct {
 	uint8_t						G;				///< G command number
 	uint8_t						M;				///< M command number
 	TARGET						target;		///< target position: X, Y, Z, E and F
+  int32_t           I;          ///< In micrometers unless explicitely stated.
+  int32_t           J;          ///< In micrometers unless explicitely stated.
 
 	uint8_t						T;				///< T word (tool index)
 

gcode_process.c

@@ -5,12 +5,14 @@
 */
 
 #include	<string.h>
+#include <stdlib.h>  // For ARC hypot function.
 
 #include	"gcode_parse.h"
 
 #include "cpu.h"
 #include	"dda.h"
 #include	"dda_queue.h"
+#include "dda_maths.h"
 #include	"watchdog.h"
 #include	"delay.h"
 #include	"serial.h"
@@ -24,6 +26,7 @@
 #include	"clock.h"
 #include	"config_wrapper.h"
 #include	"home.h"
+#include "motion_control.h"
 #include "sd.h"
 
 
@@ -130,7 +133,51 @@ void process_gcode_command() {
 
 				//	G3 - Arc Counter-clockwise
 				// unimplemented
-
+			case 2:
+			case 3:
+				//? --- G2: Arc Clockwise ---
+				//? --- G3: Arc Counter-clockwise ---
+				//?
+				//? Example: G02 X47.4 Y13.3 I-21.6 J12.4
+				//?
+				//? Go in an Arc with center (X-21.6, Y+12.4)  from the current (X, Y) point to the point (47.4, 13.3)
+				//?;
+				{
+					uint8_t clockwise = 0;
+					// if we didn't see an I or J word, set it to zero. This is for Incremental Arc Distance Mode (G91.1 the default and currently only supported mode)
+					if (next_target.seen_I == 0){	//ARC support
+						next_target.I = 0;
+						if (next_target.seen_J == 0){
+							//both are 0, this is an error for G2/3 because radius is 0.
+							//we generate an error message and just do a linear motion to the target point
+							sersendf_P(PSTR("E: missing offsets in G-code %d"), next_target.G);
+							enqueue(&next_target.target);
+							break;
+						}
+					}
+					if (next_target.seen_J == 0)
+						next_target.J = 0;
+					if (next_target.G==2) clockwise=1;
+					//for radius_mode implementation see file gcode.c in gbrl sourcecode
+					//for now only offset implementation
+
+					//calculate radius
+					//the integer variant will be off by 8% or so for a radius of 250mm but the radius is
+					//only used to calculate the number of segments the arc is to be drawn in so this
+					//good enough for our use.
+
+					uint32_t r = approx_distance(labs(next_target.I),labs(next_target.J));
+//					float r = hypot(next_target.I/1000.0,next_target.J/1000.0);
+
+//					serial_writestr_P(PSTR("\n\rArc Radius: (float,int) "));
+//					serwrite_uint32(hypot(next_target.I/1000.0,next_target.J/1000.0)*1000.0);
+//					serial_writestr_P(PSTR(", "));
+//					serwrite_uint32(r);
+
+					// Trace the arc
+					mc_arc(r, clockwise);	//motion_control.h
+				}
+				break;
 			case 4:
 				//? --- G4: Dwell ---
 				//?
@@ -370,7 +417,27 @@ void process_gcode_command() {
 				//? --- M6: tool change ---
 				//?
 				//? Undocumented.
-				tool = next_tool;
+				tool = next_tool; //tom: seems the var tool is never used
+				sersendf_P(PSTR("next tool: T%u"),tool);
+				//tom 04-08-2014: after a tool change we need to wait until the user presses
+				//the resume button:
+				//? --- M6: Pause machine until RESUME button pressed ---
+				// Fake Home E command to pause machine until RESUME_PIN=low
+				// This can for example be used to change tools manually
+				// in PCB_Gcode there is an option "Do toolchange with zero step" which calls m6 2 times:
+				// - go to X0, Y0, Z 35 (X,Y & Z options in PCB-Gcode)
+				// - M6 Tx (x is the next tool) then we wait until the resume switch is pressed (user has time to change the tool)
+				// - G01 Z0.0000 F75.00 this moves the head back to Z0, make sure the tool is high up in the spindle and
+				//     make sure that if you have tools of different length the longest one is not lower then the one that started our drilling
+				//	   Todo: use other switches to manualy adjust the Z height (or maybe other software commands)
+				// -M6 second time, we are at Z0 and the user can lower the tool onto the surface so it is at the actual Z0 again.
+				//     Press the resume button to resume drilling after this.
+//				queue_wait();
+				#if defined	RESUME_PIN	//the resume pin is in config.h and could be the same as Z-Min since the tool should be in High (Z++) now
+					home_e_negative();
+				#endif
+
+
 				break;
 
       #ifdef SD
@@ -562,12 +629,13 @@ void process_gcode_command() {
 				//?
 				//? Example: M111 S6
 				//?
-				//? Set the level of debugging information transmitted back to the host to level 6.  The level is the OR of three bits:
+				//? Set the level of debugging information transmitted back to the host to level 6.  The level is the OR of four bits:
 				//?
 				//? <Pre>
 				//? #define         DEBUG_PID       1
 				//? #define         DEBUG_DDA       2
 				//? #define         DEBUG_POSITION  4
+				//? #define         DEBUG_ARC		8
 				//? </pre>
 				//?
 				//? This command is only available in DEBUG builds of Teacup.

home.c

@@ -67,6 +67,18 @@ void home() {
   home_z_positive();
 }
 
+/// find E MIN endstop (Fake, just waits for RESUME_PIN to get low)
+void home_e_negative() {
+	#if defined RESUME_PIN
+		TARGET t = startpoint;
+
+//		t.X = -1000000;				//no change in t.x, we stay in same position as we wait for RESUME_PIN
+		enqueue_home(&t, 0x8, 1);
+
+//		queue_wait(); // we have to wait here, see G92
+	#endif
+}
+
 /// find X MIN endstop
 void home_x_negative() {
 	#if defined X_MIN_PIN

home.h

@@ -3,6 +3,7 @@
 
 void home(void);
 
+void home_e_negative(void);
 void home_x_negative(void);
 void home_x_positive(void);
 void home_y_negative(void);