Updated limit/homing routine. Works, but needs more TLC.

- Added acceleration to the homing routine.

- Homing now accounts for different step rates when moving multiple
axes without exceeding acceleration limits.

- Homing now updates all internal positioning variables to machine zero
after completion.

- "Poor-man's" debounce delay added.

- Updated the delay_us() function to perform faster and more accurate
microsecond delays. Previously, the single increments would add
noticeable time drift for larger delays.

- Fix a bug in the stepper.c prescalar calculations that was changed in
the last commit.

- Other minor fixes.

config.h

@@ -45,6 +45,9 @@
 #define X_LIMIT_BIT     1  // Uno Digital Pin 9
 #define Y_LIMIT_BIT     2  // Uno Digital Pin 10
 #define Z_LIMIT_BIT     3  // Uno Digital Pin 11
+// #define LIMIT_INT       PCIE0  // Pin change interrupt settings
+// #define LIMIT_INT_vect  PCINT0_vect
+// #define LIMIT_PCMSK     PCMSK0
 
 #define SPINDLE_ENABLE_DDR   DDRB
 #define SPINDLE_ENABLE_PORT  PORTB
@@ -86,7 +89,7 @@
 // entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift,
 // grbl has no way to know this has happened, since stepper motors are open-loop control. Depending
 // on the machine, this parameter may need to be larger or smaller than the default time.
-// NOTE: If the define commented, the delay will not be compiled.
+// NOTE: If the define commented, the stepper lock will be disabled upon compiling.
 #define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer > 0
 
 // The temporal resolution of the acceleration management subsystem. Higher number give smoother
@@ -150,14 +153,15 @@
 // of your successes or difficulties, as we will monitor this and possibly integrate this as a 
 // standard feature for future releases. However, we suggest to first try our direction delay
 // hack/solution posted in the Wiki involving inverting the stepper pin mask.
-// NOTE: Uncomment to enable. The recommended delay should be > 3us but not exceed a total
+// NOTE: Uncomment to enable. The recommended delay should be > 3us and the total step pulse
-// time of 127us when added with the Grbl settings pulse microsecond.
+// time, which includes the Grbl settings pulse microseconds, should not exceed 127us.
 // #define STEP_PULSE_DELAY 5 // Step pulse delay in microseconds. Default disabled.
 
 // ---------------------------------------------------------------------------------------
 
 // TODO: The following options are set as compile-time options for now, until the next EEPROM 
-// settings version has solidified. 
+// settings version has solidified. This is to prevent having to support dozens of different
+// incremental settings versions.
 #define CYCLE_AUTO_START 1    // Cycle auto-start boolean flag for the planner.
 #define BLOCK_DELETE_ENABLE 0 // Block delete enable/disable flag during g-code parsing
 #define REPORT_INCH_MODE 0    // Status reporting unit mode (1 = inch, 0 = mm)
@@ -170,11 +174,8 @@
 #endif
 
 //  Limit step rate for homing
-#define LIMIT_STEP_RATE 1  	// (mm/min)
+#define LIMIT_DEBOUNCE 50  // Limit switch debounce delay (in ms)
-
+// #define LIMIT_INVERT_MASK 0 //
-// Debounce delay is the time delay the controller waits for a "good" signal from the limit switch.
+// #define LIMIT_NORMAL_HIGH 1 // Normal low 0 or normal high 1
-// A delay of 3ms to 5ms is a good starting value.
-#define LIMIT_DEBOUNCE_DELAY 5 // (milliseconds)
-
 
 #endif

gcode.c

@@ -114,6 +114,12 @@ void gc_set_current_position(int32_t x, int32_t y, int32_t z)
   gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS]; 
 }
 
+// Clears and zeros g-code parser position. Called by homing routine.
+void gc_clear_position()
+{
+  clear_vector(gc.position);
+}
+
 static float to_millimeters(double value) 
 {
   return(gc.inches_mode ? (value * MM_PER_INCH) : value);
@@ -336,7 +342,7 @@ uint8_t gc_execute_line(char *line)
         mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false);
       }
       mc_go_home(); 
-      clear_vector(gc.position); // Assumes home is at [0,0,0]
+//      clear_vector(gc.position); // Assumes home is at [0,0,0]
       axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
       break;      
     case NON_MODAL_SET_COORDINATE_OFFSET:

gcode.h

@@ -1,5 +1,5 @@
 /*
-  gcode.c - rs274/ngc parser.
+  gcode.h - rs274/ngc parser.
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
@@ -33,4 +33,7 @@ uint8_t gc_execute_line(char *line);
 // Set g-code parser position. Input in steps.
 void gc_set_current_position(int32_t x, int32_t y, int32_t z); 
 
+// Clear g-code parser position
+void gc_clear_position();
+
 #endif

limits.c

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Copyright (c) 2012 Sungeun K. Jeon
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -20,89 +21,164 @@
   
 #include <util/delay.h>
 #include <avr/io.h>
+#include <avr/interrupt.h>
 #include "stepper.h"
 #include "settings.h"
 #include "nuts_bolts.h"
 #include "config.h"
+#include "spindle_control.h"
 #include "motion_control.h"
 #include "planner.h"
+#include "protocol.h"
 
-// TODO: Deprecated. Need to update for new version. Sys.position now tracks position relative
+#define MICROSECONDS_PER_ACCELERATION_TICK  (1000000/ACCELERATION_TICKS_PER_SECOND)
-// to the home position. Limits should update this vector directly.
 
-void limits_init() {
+void limits_init() 
+{
   LIMIT_DDR &= ~(LIMIT_MASK);
 }
 
-static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool reverse_direction, uint32_t microseconds_per_pulse) {
+// Moves all specified axes in same specified direction (positive=true, negative=false)
-  // First home the Z axis
+// and at the homing rate. Homing is a special motion case, where there is only an 
-  uint32_t step_delay = microseconds_per_pulse - settings.pulse_microseconds;
+// acceleration followed by abrupt asynchronous stops by each axes reaching their limit 
-  uint8_t out_bits = DIRECTION_MASK;
+// switch independently. Instead of showhorning homing cycles into the main stepper 
-  uint8_t limit_bits;
+// algorithm and overcomplicate things, a stripped-down, lite version of the stepper 
+// algorithm is written here. This also lets users hack and tune this code freely for
+// their own particular needs without affecting the rest of Grbl.
+// NOTE: Only the abort runtime command can interrupt this process.
+static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, int8_t pos_dir, double homing_rate) 
+{
+  // Determine governing axes with finest step resolution per distance for the Bresenham
+  // algorithm. This solves the issue when homing multiple axes that have different 
+  // resolutions without exceeding system acceleration setting. It doesn't have to be
+  // perfect since homing locates machine zero, but should create for a more consistent 
+  // and speedy homing routine.
+  // NOTE: For each axes enabled, the following calculations assume they physically move 
+  // an equal distance over each time step until they hit a limit switch, aka dogleg.
+  uint32_t steps[3];
+  clear_vector(steps);
+  if (x_axis) { steps[X_AXIS] = lround(settings.steps_per_mm[X_AXIS]); }
+  if (y_axis) { steps[Y_AXIS] = lround(settings.steps_per_mm[Y_AXIS]); }
+  if (z_axis) { steps[Z_AXIS] = lround(settings.steps_per_mm[Z_AXIS]); }
+  uint32_t step_event_count = max(steps[X_AXIS], max(steps[Y_AXIS], steps[Z_AXIS]));  
   
-  if (x_axis) { out_bits |= (1<<X_STEP_BIT); }
+  // To ensure global acceleration is not exceeded, reduce the governing axes nominal rate
-  if (y_axis) { out_bits |= (1<<Y_STEP_BIT); }
+  // by adjusting the actual axes distance traveled per step. This is the same procedure
-  if (z_axis) { out_bits |= (1<<Z_STEP_BIT); }
+  // used in the main planner to account for distance traveled when moving multiple axes.
+  // NOTE: When axis acceleration independence is installed, this will be updated to move
+  // all axes at their maximum acceleration and rate.
+  double ds = step_event_count/sqrt(x_axis+y_axis+z_axis);
 
-  // Invert direction bits if this is a reverse homing_cycle
+  // Compute the adjusted step rate change with each acceleration tick. (in step/min/acceleration_tick)
-  if (reverse_direction) {
+  uint32_t delta_rate = ceil( ds*settings.acceleration/(60*ACCELERATION_TICKS_PER_SECOND));
-    out_bits ^= DIRECTION_MASK;
-  }
   
-  // Apply the global invert mask
+  // Nominal and initial time increment per step. Nominal should always be greater then 3
-  out_bits ^= settings.invert_mask;
+  // usec, since they are based on the same parameters as the main stepper routine. Initial
+  // is based on the MINIMUM_STEPS_PER_MINUTE config.
+  uint32_t dt_min = lround(1000000*60/(ds*homing_rate)); // Cruising (usec/step)
+  uint32_t dt = 1000000*60/MINIMUM_STEPS_PER_MINUTE; // Initial (usec/step)
       
-  // Set direction pins
+  // Determine default out_bits set. Direction fixed and step pin inverted
-  STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
+  uint8_t out_bits0 = DIRECTION_MASK; 
+  out_bits0 ^= settings.invert_mask;  // Apply the global step and direction invert mask
+  if (!pos_dir) { out_bits0 ^= DIRECTION_MASK; }   // Invert bits, if negative dir.
   
+  // Initialize stepping variables
+  int32_t counter_x = -(step_event_count >> 1); // Bresenham counters
+  int32_t counter_y = counter_x;
+  int32_t counter_z = counter_x;
+  uint32_t step_delay = dt-settings.pulse_microseconds;  // Step delay after pulse
+  uint32_t step_rate = 0;  // Tracks step rate. Initialized from 0 rate. (in step/min)
+  uint32_t trap_counter = MICROSECONDS_PER_ACCELERATION_TICK/2; // Acceleration trapezoid counter
+  uint8_t out_bits;
   for(;;) {
-    limit_bits = LIMIT_PIN;
+  
-    if (reverse_direction) {         
+    // Reset out bits. Both direction and step pins appropriately inverted and set.
-      // Invert limit_bits if this is a reverse homing_cycle
+    out_bits = out_bits0;
-      limit_bits ^= LIMIT_MASK;
+    
+    // Set step pins by Bresenham line algorithm. If limit switch reached, disable and
+    // flag for completion.
+    if (x_axis) {
+      counter_x += steps[X_AXIS];
+      if (counter_x > 0) {
+        if (LIMIT_PIN & (1<<X_LIMIT_BIT)) { out_bits ^= (1<<X_STEP_BIT); }
+        else { x_axis = false; }
+        counter_x -= step_event_count;
       }
-    if (x_axis && !(LIMIT_PIN & (1<<X_LIMIT_BIT))) {
-      x_axis = false;
-      out_bits ^= (1<<X_STEP_BIT);      
     }
-    if (y_axis && !(LIMIT_PIN & (1<<Y_LIMIT_BIT))) {
+    if (y_axis) {
-      y_axis = false;
+      counter_y += steps[Y_AXIS];
-      out_bits ^= (1<<Y_STEP_BIT);
+      if (counter_y > 0) {
+        if (LIMIT_PIN & (1<<Y_LIMIT_BIT)) { out_bits ^= (1<<Y_STEP_BIT); }
+        else { y_axis = false; }
+        counter_y -= step_event_count;
       }
-    if (z_axis && !(LIMIT_PIN & (1<<Z_LIMIT_BIT))) {
-      z_axis = false;
-      out_bits ^= (1<<Z_STEP_BIT);
     }
-    // Check if we are done
+    if (z_axis) {
-    if(!(x_axis || y_axis || z_axis)) { return; }
+      counter_z += steps[Z_AXIS];
-    STEPPING_PORT |= out_bits & STEP_MASK;
+      if (counter_z > 0) {
+        if (LIMIT_PIN & (1<<Z_LIMIT_BIT)) { out_bits ^= (1<<Z_STEP_BIT); }
+        else { z_axis = false; }
+        counter_z -= step_event_count;
+      }
+    }        
+    
+    // Check if we are done or for system abort
+    protocol_execute_runtime();
+    if (!(x_axis || y_axis || z_axis) || sys.abort) { return; }
+        
+    // Perform step.
+    STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (out_bits & STEP_MASK);
     delay_us(settings.pulse_microseconds);
-    STEPPING_PORT ^= out_bits & STEP_MASK;
+    STEPPING_PORT = out_bits0;
     delay_us(step_delay);
+    
+    // Track and set the next step delay, if required. This routine uses another Bresenham
+    // line algorithm to follow the constant acceleration line in the velocity and time 
+    // domain. This is a lite version of the same routine used in the main stepper program.
+    if (dt > dt_min) { // Unless cruising, check for time update.
+      trap_counter += dt; // Track time passed since last update.
+      if (trap_counter > MICROSECONDS_PER_ACCELERATION_TICK) {
+        trap_counter -= MICROSECONDS_PER_ACCELERATION_TICK;
+        step_rate += delta_rate; // Increment velocity
+        dt = (1000000*60)/step_rate; // Compute new time increment
+        if (dt < dt_min) {dt = dt_min;}  // If target rate reached, cruise.
+        step_delay = dt-settings.pulse_microseconds;
+      }
+    }
   }
-  return;
 }
 
-static void approach_limit_switch(bool x, bool y, bool z) {
+static void approach_limit_switch(bool x, bool y, bool z) 
-  homing_cycle(x, y, z, false, 100000);
+{
+  homing_cycle(x, y, z, true, settings.default_seek_rate);
 }
 
+
 static void leave_limit_switch(bool x, bool y, bool z) {
-  homing_cycle(x, y, z, true, 500000);
+  homing_cycle(x, y, z, false, settings.default_feed_rate);
 }
 
-void limits_go_home() {
+void limits_go_home() 
-  plan_synchronize();
+{
-  // Store the current limit switch state
+  plan_synchronize();  // Empty all motions in buffer.
-  uint8_t original_limit_state = LIMIT_PIN;
+  
+  // TODO: Need to come up a better way to manage and set limit switches.
+  uint8_t original_limit_state = LIMIT_PIN;  // Store the current limit switch state
+
+  // Jog all axes toward home to engage their limit switches.
   approach_limit_switch(false, false, true); // First home the z axis
   approach_limit_switch(true, true, false);  // Then home the x and y axis
+  delay_ms(LIMIT_DEBOUNCE); // Delay to debounce signal before leaving limit switches
+  
   // Xor previous and current limit switch state to determine which were high then but have become 
   // low now. These are the actual installed limit switches.
   uint8_t limit_switches_present = (original_limit_state ^ LIMIT_PIN) & LIMIT_MASK;
+  
   // Now carefully leave the limit switches
   leave_limit_switch(
     limit_switches_present & (1<<X_LIMIT_BIT), 
     limit_switches_present & (1<<Y_LIMIT_BIT),
     limit_switches_present & (1<<Z_LIMIT_BIT));
+  delay_ms(LIMIT_DEBOUNCE); // Delay to debounce signal before leaving limit switches    
 }

motion_control.c

@@ -23,6 +23,7 @@
 #include <avr/io.h>
 #include "settings.h"
 #include "config.h"
+#include "gcode.h"
 #include "motion_control.h"
 #include <util/delay.h>
 #include <math.h>
@@ -192,9 +193,12 @@ void mc_dwell(double seconds)
 }
 
 
-// TODO: Update limits and homing cycle subprograms for better integration with new features.
+// Execute homing cycle to locate and set machine zero.
 void mc_go_home()
 {
   limits_go_home();
-  plan_set_current_position(0,0,0);
+  // Upon completion, reset all internal position vectors (g-code parser, planner, system)
+  gc_clear_position();
+  plan_clear_position();
+  clear_vector_double(sys.position);
 }

nuts_bolts.c

@@ -46,8 +46,23 @@ void delay_ms(uint16_t ms)
 }
 
 // Delays variable defined microseconds. Compiler compatibility fix for _delay_us(),
-// which only accepts constants in future compiler releases.
+// which only accepts constants in future compiler releases. Written to perform more 
+// efficiently with larger delays, as the counter adds parasitic time in each iteration.
 void delay_us(uint16_t us) 
 {
-  while ( us-- ) { _delay_us(1); }
+  while (us) {
+    if (us < 10) { 
+      _delay_us(1);
+      us--;
+    } else if (us < 100) {
+      _delay_us(10);
+      us -= 10;
+    } else if (us < 1000) {
+      _delay_us(100);
+      us -= 100;
+    } else {
+      _delay_ms(1);
+      us -= 1000;
+    }
+  }
 }

planner.c

@@ -481,6 +481,12 @@ void plan_set_current_position(int32_t x, int32_t y, int32_t z)
   pl.position[Z_AXIS] = z;
 }
 
+// Clear planner position vector. Called by homing routine.
+void plan_clear_position()
+{
+  clear_vector(pl.position);
+}
+
 // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
 // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
 void plan_cycle_reinitialize(int32_t step_events_remaining) 

planner.h

@@ -69,6 +69,9 @@ block_t *plan_get_current_block();
 // Reset the planner position vector (in steps)
 void plan_set_current_position(int32_t x, int32_t y, int32_t z);
 
+// Clear the planner position vector
+void plan_clear_position();
+
 // Reinitialize plan with a partially completed block
 void plan_cycle_reinitialize(int32_t step_events_remaining);
 

serial.c

@@ -42,7 +42,7 @@ uint8_t tx_buffer[TX_BUFFER_SIZE];
 uint8_t tx_buffer_head = 0;
 volatile uint8_t tx_buffer_tail = 0;
 
-#if ENABLE_XONXOFF
+#ifdef ENABLE_XONXOFF
   #define RX_BUFFER_FULL 96 // XOFF high watermark
   #define RX_BUFFER_LOW 64 // XON low watermark
   #define SEND_XOFF 1
@@ -110,7 +110,7 @@ ISR(USART_UDRE_vect)
   // Temporary tx_buffer_tail (to optimize for volatile)
   uint8_t tail = tx_buffer_tail;
   
-  #if ENABLE_XONXOFF
+  #ifdef ENABLE_XONXOFF
     if (flow_ctrl == SEND_XOFF) { 
       UDR0 = XOFF_CHAR; 
       flow_ctrl = XOFF_SENT; 
@@ -143,7 +143,7 @@ uint8_t serial_read()
     rx_buffer_tail++;
     if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
 
-    #if ENABLE_XONXOFF
+    #ifdef ENABLE_XONXOFF
       if ((get_rx_buffer_count() < RX_BUFFER_LOW) && flow_ctrl == XOFF_SENT) { 
         flow_ctrl = SEND_XON;
         UCSR0B |=  (1 << UDRIE0); // Force TX
@@ -182,7 +182,7 @@ ISR(USART_RX_vect)
         rx_buffer[rx_buffer_head] = data;
         rx_buffer_head = next_head;    
         
-        #if ENABLE_XONXOFF
+        #ifdef ENABLE_XONXOFF
           if ((get_rx_buffer_count() >= RX_BUFFER_FULL) && flow_ctrl == XON_SENT) {
             flow_ctrl = SEND_XOFF;
             UCSR0B |=  (1 << UDRIE0); // Force TX
@@ -197,7 +197,7 @@ void serial_reset_read_buffer()
 {
   rx_buffer_tail = rx_buffer_head;
 
-  #if ENABLE_XONXOFF
+  #ifdef ENABLE_XONXOFF
     flow_ctrl = XON_SENT;
   #endif
 }

stepper.c

@@ -31,7 +31,6 @@
 #include "nuts_bolts.h"
 #include <avr/interrupt.h>
 #include "planner.h"
-#include "limits.h"
 
 // Some useful constants
 #define TICKS_PER_MICROSECOND (F_CPU/1000000)
@@ -299,7 +298,7 @@ ISR(TIMER1_COMPA_vect)
       plan_discard_current_block();
     }
   }
-  out_bits ^= settings.invert_mask;  // Apply stepper invert mask    
+  out_bits ^= settings.invert_mask;  // Apply step and direction invert mask    
   busy = false;
 }
 
@@ -396,7 +395,7 @@ static uint32_t config_step_timer(uint32_t cycles)
   } else {
     // Okay, that was slower than we actually go. Just set the slowest speed
     ceiling = 0xffff;
-    prescaler = 6;
+    prescaler = 5;
     actual_cycles = 0xffff * 1024;
   }
   // Set prescaler