added buffered stepping support and the rudiments of the arc-interpolator

Makefile

@@ -30,7 +30,7 @@
 DEVICE     = atmega168
 CLOCK      = 20000000
 PROGRAMMER = -c avrisp2 -P usb
-OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o
+OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o
 FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
 
 # Tune the lines below only if you know what you are doing:

arc_algorithm/arc.rb

@@ -15,11 +15,11 @@ class CircleTest
   def init
     @pixels = []
     @tool_position = [14,14]
-    30.times { @pixels << '.'*30 }
+    40.times { @pixels << '.'*40 }
   end
 
   def plot_pixel(x,y, c) 
-    return if x<0 || y<0 || x>29 || y > 29
+    return if x<0 || y<0 || x>39 || y > 39
     @pixels[y] = @pixels[y][0..x][0..-2]+c+@pixels[y][(x+1)..-1]
   end
   
@@ -34,10 +34,10 @@ class CircleTest
   # dP[x, y+1]: 1 + 2 y
   # dP[x, y-1]: 1 - 2 y
 
-  # dP[x+1, y+1]: 2 (1 + x + y)
+  # dP[x+1, y+1]: 2 (1 + x + y) 1+2x+1+2y
-  # dP[x+1, y-1]: 2 (1 + x - y)
+  # dP[x+1, y-1]: 2 (1 + x - y) 1+2x+1-2y
-  # dP[x-1, y-1]: 2 (1 - x - y)
+  # dP[x-1, y-1]: 2 (1 - x - y) 2-2x-2y 
-  # dP[x-1, y+1]: 2 (1 - x + y)
+  # dP[x-1, y+1]: 2 (1 - x + y) 2-2x+2x
    
   # dP[x+a, y+b]: |dx| - 2*dx*x + |dy| + 2*dy*y
   
@@ -111,6 +111,7 @@ class CircleTest
 
   # A DDA-direct search circle interpolator. Optimal and impure
   def arc_clean(theta, angular_travel, radius) 
+    radius = radius
     x = (sin(theta)*radius).round
     y = (cos(theta)*radius).round
     angular_direction = angular_travel.sign
@@ -130,8 +131,10 @@ class CircleTest
         plot_pixel(x+14, -y+14, "X")
       end
         
-      dx = (y==0) ? angular_direction : y.sign*angular_direction
+      dx = (y==0) ? -x.sign : y.sign*angular_direction
-      dy = (x==0) ? angular_direction : -x.sign*angular_direction
+      dy = (x==0) ? -y.sign : -x.sign*angular_direction
+      
+      pp [[x,y],[dx,dy]]
 
       if x.abs<y.abs
         f_straight = f + 1+2*x*dx
@@ -173,12 +176,250 @@ class CircleTest
     puts "Diameter: #{max_x-min_x}"
   end
 
+  # A DDA-direct search circle interpolator. Optimal and impure
+  def arc_supaoptimal(theta, angular_travel, radius) 
+    radius = radius
+    x = (sin(theta)*radius).round
+    y = (cos(theta)*radius).round
+    angular_direction = angular_travel.sign
+    tx = (sin(theta+angular_travel)*(radius-0.5)).floor
+    ty = (cos(theta+angular_travel)*(radius-0.5)).floor
+    f = (x**2 + y**2 - radius**2).round
+    x2 = 2*x
+    y2 = 2*y
+    dx = (y==0) ? -x.sign : y.sign*angular_direction
+    dy = (x==0) ? -y.sign : -x.sign*angular_direction
+    
+    max_steps = (angular_travel.abs*radius*2).floor
+    
+    # dP[x+1,y]: 1 + 2 x
+    # dP[x, y+1]: 1 + 2 y
+    
+    max_steps.times do |i|
+      if i > 0
+        plot_pixel(x+20, -y+20, "012"[i%3].chr)
+      else
+        plot_pixel(x+20, -y+20, "X")
+      end
+        
+      
+      raise "x2 out of range" unless x2 == 2*x
+      raise "y2 out of range" unless y2 == 2*y
+      f_should_be = (x**2+y**2-radius**2)
+      if f.round != f_should_be.round
+        show
+        raise "f out of range. Is #{f}, should be #{f_should_be}"
+        
+      end
+
+      if x.abs<y.abs
+        x += dx
+        
+        f += 1+x2*dx
+        x2 += 2*dx
+        f_diagonal = f + 1 + y2*dy
+        if  (f.abs >= f_diagonal.abs)
+          y += dy
+          dx = y.sign*angular_direction unless y == 0
+          y2 += 2*dy
+          f = f_diagonal
+        end
+        dy = -x.sign*angular_direction unless x == 0
+      else
+        y += dy
+        f += 1+y2*dy
+        y2 += 2*dy
+        f_diagonal = f + 1 + x2*dx
+        if  (f.abs >= f_diagonal.abs)
+          x += dx
+          dy = -x.sign*angular_direction unless x == 0
+          x2 += 2*dx
+          f = f_diagonal
+        end
+        dx = y.sign*angular_direction unless y == 0
+      end
+      break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
+    end
+    plot_pixel(tx+20, -ty+20, "o")
+    return {:tx => tx, :ty => ty, :x => x, :y => y}
+  end
+
+  # A DDA-direct search circle interpolator unrolled for each octant. Optimal and impure
+  def arc_unrolled(theta, angular_travel, radius) 
+    radius = radius
+    x = (sin(theta)*radius).round
+    y = (cos(theta)*radius).round
+    angular_direction = angular_travel.sign
+    tx = (sin(theta+angular_travel)*(radius-0.5)).floor
+    ty = (cos(theta+angular_travel)*(radius-0.5)).floor
+    f = (x**2 + y**2 - radius**2).round
+    x2 = 2*x
+    y2 = 2*y
+    dx = (y==0) ? -x.sign : y.sign*angular_direction
+    dy = (x==0) ? -y.sign : -x.sign*angular_direction
+    
+    max_steps = (angular_travel.abs*radius*2).floor
+    
+    # Quandrants of the circls
+    #       \ 1|2 /
+    #      8\ | / 3
+    #         \|/
+    # ---------|-----------
+    #     7   /|\  4
+    #       /  | \  
+    #     / 6 | 5 \ 
+    #            
+    #
+    #
+    
+    if angular_direction>0 # clockwise
+      if x.abs<y.abs # quad 1,2,6,5
+        if y>0 # quad 1,2
+          while x<0 # quad 1  x+,y+
+            x += 1        
+            f += 1+x2
+            x2 += 2
+            f_diagonal = f + 1 + y2
+            if  (f.abs >= f_diagonal.abs)
+              y += 1
+              y2 += 2
+              f = f_diagonal
+            end
+          end
+          while x>=0 # quad 2, x+, y-
+            x += 1        
+            f += 1+x2
+            x2 += 2
+            f_diagonal = f + 1 - y2
+            if  (f.abs >= f_diagonal.abs)
+              y -= 1
+              y2 -= 2
+              f = f_diagonal
+            end
+          end
+        end
+        if y<=0 # quad 6, 5
+          while x<0 # quad 6 x-, y+
+            x -= 1        
+            f += 1-x2
+            x2 -= 2
+            f_diagonal = f + 1 + y2
+            if  (f.abs >= f_diagonal.abs)
+              y += 1
+              y2 += 2
+              f = f_diagonal
+            end
+          end
+          while x>=0 # quad 5 x-, y-
+            x -= 1        
+            f += 1-x2
+            x2 -= 2
+            f_diagonal = f + 1 - y2
+            if  (f.abs >= f_diagonal.abs)
+              y -= 1
+              y2 -= 2
+              f = f_diagonal
+            end
+          end
+        end
+        # Quandrants of the circls
+        #       \ 1|2 /
+        #      8\ | / 3
+        #         \|/
+        # ---------|-----------
+        #     7   /|\  4
+        #       /  | \  
+        #     / 6 | 5 \ 
+      else 3 # quad 3,4,7,8
+        if x>0 # quad 3,4
+          while y>0 # quad 3  x+,y+
+            x += 1        
+            f += 1+x2
+            x2 += 2
+            f_diagonal = f + 1 + y2
+            if  (f.abs >= f_diagonal.abs)
+              y += 1
+              y2 += 2
+              f = f_diagonal
+            end
+          end
+          while x>=0 # quad 2, x+, y-
+            x += 1        
+            f += 1+x2
+            x2 += 2
+            f_diagonal = f + 1 - y2
+            if  (f.abs >= f_diagonal.abs)
+              y -= 1
+              y2 -= 2
+              f = f_diagonal
+            end
+          end
+        end
+        if y<=0 # quad 6, 5
+          while x<0 # quad 6 x-, y+
+            x -= 1        
+            f += 1-x2
+            x2 -= 2
+            f_diagonal = f + 1 + y2
+            if  (f.abs >= f_diagonal.abs)
+              y += 1
+              y2 += 2
+              f = f_diagonal
+            end
+          end
+          while x>=0 # quad 5 x-, y-
+            x -= 1        
+            f += 1-x2
+            x2 -= 2
+            f_diagonal = f + 1 - y2
+            if  (f.abs >= f_diagonal.abs)
+              y -= 1
+              y2 -= 2
+              f = f_diagonal
+            end
+          end
+      end  
+        
+      else
+        y += dy
+        f += 1+y2*dy
+        y2 += 2*dy
+        f_diagonal = f + 1 + x2*dx
+        if  (f.abs >= f_diagonal.abs)
+          x += dx
+          dy = -x.sign*angular_direction unless x == 0
+          x2 += 2*dx
+          f = f_diagonal
+        end
+        dx = y.sign*angular_direction unless y == 0
+      end
+      break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
+    end
+    plot_pixel(tx+20, -ty+20, "o")
+    return {:tx => tx, :ty => ty, :x => x, :y => y}
+  end
+
   
 end
 
 test = CircleTest.new
 test.init
 
-test.arc_clean(0, -Math::PI, 5)
+#test.arc_clean(0, Math::PI*2, 5)
+(1..10000).each do |r|
+  test.init
+  data = test.arc_supaoptimal(2.9, Math::PI*1, r)
+  if (data[:tx]-data[:x]).abs > 1 || (data[:ty]-data[:y]).abs > 1
+    puts "r=#{r} fails target control" 
+    pp data
+    puts
+  end
+end
+
+# test.init
+# data = test.arc_supaoptimal(1.1, -Math::PI, 19)
+# pp data
+
+#test.pure_arc(0,1,1,4)
 
 test.show

config.h

@@ -39,19 +39,17 @@
 #define STEPPERS_ENABLE_PORT    PORTB
 #define STEPPERS_ENABLE_BIT         6
 
-#define STEP_DDR       DDRB
+#define MOTORS_DDR       DDRB
-#define STEP_PORT      PORTB 
+#define MOTORS_PORT      PORTB 
 #define X_STEP_BIT           0
 #define Y_STEP_BIT           2
 #define Z_STEP_BIT           4
-#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
-
-#define DIRECTION_DDR        DDRB
-#define DIRECTION_PORT       PORTB 
 #define X_DIRECTION_BIT            1
 #define Y_DIRECTION_BIT            3
 #define Z_DIRECTION_BIT            5
+#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
 #define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)
+#define MOTORS_MASK STEP_MASK | DIRECTION_MASK
 
 #define LIMIT_DDR      DDRC
 #define LIMIT_PORT     PORTC 

main.c

@@ -20,19 +20,20 @@
 
 #include <avr/io.h>
 #include <avr/sleep.h>
+#include "stepper.h"
+#include "spindle_control.h"
 #include "motion_control.h"
 #include "gcode.h"
-#include "spindle_control.h"
 #include "serial_protocol.h"
 
 int main(void)
 {
+  st_init();
   mc_init(); // initialize motion control subsystem
-  gc_init(); // initialize gcode-parser
   spindle_init(); // initialize spindle controller
+  gc_init(); // initialize gcode-parser
   sp_init(); // initialize the serial protocol
   
-  gc_execute_line("123.1");  
   for(;;){
     sleep_mode();
     sp_process(); // process the serial protocol

motion_control.c

@@ -27,6 +27,7 @@
 #include <math.h>
 #include <stdlib.h>
 #include "nuts_bolts.h"
+#include "stepper.h"
 
 // position represents the current position of the head measured in steps
 // target is the target for the current linear motion
@@ -38,7 +39,6 @@
 #define MODE_ARC 2
 #define MODE_DWELL 3
 #define MODE_HOME 4
-#define MODE_LIMIT_OVERRUN -1
 
 #define PHASE_HOME_RETURN 0
 #define PHASE_HOME_NUDGE 1
@@ -55,17 +55,15 @@ struct LinearMotionParameters {
     maximum_steps;   // The larges absolute step-count of any axis
 };
 
-// Parameters when mode is MODE_LINEAR
 struct ArcMotionParameters {
-  uint32_t radius;
+  int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
-  int16_t degrees;
+  uint32_t circle_x, circle_y, target_x, target_y; // current position and target position in the 
-  int ccw;
+                                                   // local coordinate system of the circle where [0,0] is the 
-};
+                                                   // center of the circle.
-
+  int32_t error, x2, y2;                           // error is always == (circle_x**2 + circle_y**2 - radius**2), 
-struct HomeCycleParameters {
+                                                   // x2 is always 2*x, y2 is always 2*y
-  int8_t direction[3];  // The direction of travel along each axis (-1, 0 or 1)
+  uint8_t axis_x, axis_y;                           // maps the circle axes to stepper axes
-  int8_t phase;       // current phase of the home cycle. 
+  int32_t target[3];                                // The target position in absolute steps
-  int8_t away[3];     // a vector of booleans. True for each axis that is still away.
 };
 
 /* The whole state of the motion-control-system in one struct. Makes the code a little bit hard to 
@@ -75,57 +73,39 @@ struct HomeCycleParameters {
 struct MotionControlState {
   int8_t mode;            // The current operation mode
   int32_t position[3];    // The current position of the tool in absolute steps
-  int32_t update_delay_us;  // Microseconds between each update in the current mode
+  int32_t pace;  // Microseconds between each update in the current mode
   union {
     struct LinearMotionParameters linear; // variables used in MODE_LINEAR
     struct ArcMotionParameters arc;       // variables used in MODE_ARC
-    struct HomeCycleParameters home;      // variables used in MODE_HOME
     uint32_t dwell_milliseconds;          // variable used in MODE_DWELL
-    int8_t limit_overrun_direction[3];    // variable used in MODE_LIMIT_OVERRUN
   };
 };
 struct MotionControlState state;
 
-int check_limit_switches();
+uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
+
 void enable_steppers();
 void disable_steppers();
-void set_direction_pins(int8_t *direction);
+void set_direction_bits(int8_t *direction);
 inline void step_steppers(uint8_t *enabled);
-void limit_overrun(uint8_t *direction);
-int check_limit_switch(int axis);
 inline void step_axis(uint8_t axis);
 
 void mc_init()
 {
 	// Initialize state variables
   memset(&state, 0, sizeof(state));
-
-	// Configure directions of interface pins
-  STEP_DDR   |= STEP_MASK;
-  DIRECTION_DDR |= DIRECTION_MASK;
-  LIMIT_DDR &= ~(LIMIT_MASK);
-  STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
-	
-	disable_steppers();
-}
-
-void limit_overrun(uint8_t *direction) 
-{
-  state.mode = MODE_LIMIT_OVERRUN;
-  memcpy(state.limit_overrun_direction, direction, sizeof(state.limit_overrun_direction));
 }
 
 void mc_dwell(uint32_t milliseconds) 
 {
-  mc_wait();
+  st_synchronize();
   state.mode = MODE_DWELL;
   state.dwell_milliseconds = milliseconds;
-  state.update_delay_us = 1000;
+  state.pace = 1000;
 }
 
 void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate)
 {
-  mc_wait();
   state.mode = MODE_LINEAR;
   
 	state.linear.target[X_AXIS] = trunc(x*X_STEPS_PER_MM);
@@ -149,19 +129,19 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
   }
 
 	// Set our direction pins 
-  set_direction_pins(state.linear.direction);
+  set_direction_bits(state.linear.direction);
 
   // Calculate the microseconds we need to wait between each step to achieve the desired feed rate
   if (invert_feed_rate) {
-  	state.update_delay_us = 
+  	state.pace = 
-  	  (feed_rate*1000000.0)/state.linear.maximum_steps;	  
+  	  (feed_rate*1000000)/state.linear.maximum_steps;	  
   } else {
   	// Ask old Phytagoras how many millimeters our next move is going to take us:
   	float millimeters_of_travel = 
       sqrt(pow((X_STEPS_PER_MM*state.linear.step_count[X_AXIS]),2) + 
            pow((Y_STEPS_PER_MM*state.linear.step_count[Y_AXIS]),2) + 
            pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2));
-  	state.update_delay_us = 
+  	state.pace = 
   	  ((millimeters_of_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;	  
   }
 }
@@ -190,63 +170,132 @@ void perform_linear_motion()
 	if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
     step_steppers(step);
 
-		// If we trip any limit switch while moving: Abort, abort!
-		if (check_limit_switches()) { 
-      limit_overrun(state.linear.direction);
-		}
-
-		_delay_us(state.update_delay_us);
 	} else {
     state.mode = MODE_AT_REST;
 	}
 }
 
+void mc_arc(double theta, double angular_travel, double radius, uint32_t *target)
+{
+  state.mode = MODE_ARC;
+  // Calculate the initial position and target position in the local coordinate system of the circle
+  state.arc.circle_x = round(sin(theta)*radius); 
+  state.arc.circle_y = round(cos(theta)*radius);
+  state.arc.target_x = trunc(sin(theta+angular_travel)*(radius-0.5));
+  state.arc.target_y = trunc(cos(theta+angular_travel)*(radius-0.5));
+  // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
+  state.arc.angular_direction = sign(angular_travel);
+  // The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error 
+  // <0 we are inside the circle, when it is >0 we are outside of the circle, and when it is 0 we 
+  // are exactly on top of the circle.
+  state.arc.error = round(pow(state.arc.circle_x,2) + pow(state.arc.circle_y,2) - pow(radius,2));
+  // Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
+  // by keeping track of the doubles of the circle coordinates at all times.
+  state.arc.x2 = 2*state.arc.circle_x;
+  state.arc.y2 = 2*state.arc.circle_y; 
+}
+
+void step_arc_along_x(dx,dy) 
+{
+  uint32_t diagonal_error;
+  state.arc.circle_x+=dx;
+  state.arc.error += 1+state.arc.x2*dx;
+  state.arc.x2 += 2*dx;
+  diagonal_error = state.arc.error + 1 + state.arc.y2*dy;
+  if(abs(state.arc.error) < abs(diagonal_error)) {
+    state.arc.circle_y += dy;
+    state.arc.y2 += 2*dy;
+    state.arc.error = diagonal_error;
+  };
+}
+
+void step_arc_along_y(dx,dy) 
+{  
+  uint32_t diagonal_error;
+  state.arc.circle_y+=dy; 
+  state.arc.error += 1+state.arc.y2*dy; 
+  state.arc.y2 += 2*dy; 
+  diagonal_error = state.arc.error + 1 + state.arc.x2*dx; 
+  if(abs(state.arc.error) < abs(diagonal_error)) { 
+    state.arc.circle_x += dx; 
+    state.arc.x2 += 2*dx; 
+    state.arc.error = diagonal_error; 
+  }
+}
+
+/*
+ Quandrants of the circle
+       \ 7|0 /
+        \ | / 
+      6  \|/  1    y+
+ ---------|-----------
+      5  /|\  2    y-
+        / | \  
+   x-  / 4|3 \ x+           */
+
+int quadrant(uint32_t x,uint32_t y)
+{
+  // determine if the coordinate is in the quadrants 0,3,4 or 7
+  register int quad0347 = abs(x)<abs(y);
+  
+  if (x<0) { // quad 4567
+    if (y<0) { // quad 45
+      return(quad0347 ? 4 : 5);
+    } else { // quad 67
+      return(quad0347 ? 7 : 6);
+    }
+  } else {
+    if (y<0) { // quad 23
+      return(quad0347 ? 3 : 2);
+    } else { // quad 01
+      return(quad0347 ? 0 : 1);
+    }
+  }  
+}
+
+void perform_arc()
+{
+  int q = quadrant(state.arc.circle_x, state.arc.circle_y);
+  
+  if (state.arc.angular_direction) {
+    switch (q) {
+      case 0: while(state.arc.circle_x>state.arc.circle_y) { step_arc_along_x(1,-1); }
+      case 1: while(state.arc.circle_y>0) { step_arc_along_y(1,-1); }
+      case 2: while(state.arc.circle_y>-state.arc.circle_x) { step_arc_along_y(-1,-1); }
+      case 3: while(state.arc.circle_x>0) { step_arc_along_x(-1,-1); }
+      case 4: while(state.arc.circle_y<state.arc.circle_x) { step_arc_along_x(-1,1); }
+      case 5: while(state.arc.circle_y<0) { step_arc_along_y(-1,1); }
+      case 6: while(state.arc.circle_y<-state.arc.circle_x) { step_arc_along_y(1,1); }
+      case 7: while(state.arc.circle_x<0) { step_arc_along_x(1,1); }
+    }
+  } else {
+    switch (q) {
+      case 7: while(state.arc.circle_y>-state.arc.circle_x) { step_arc_along_x(-1,-1); }
+      case 6: while(state.arc.circle_y>0)  { step_arc_along_y(-1,-1); }
+      case 5: while(state.arc.circle_y>state.arc.circle_x)  { step_arc_along_y(1,-1); }
+      case 4: while(state.arc.circle_x<0)  { step_arc_along_x(1,-1); }
+      case 3: while(state.arc.circle_y<-state.arc.circle_x) { step_arc_along_x(1,1); }      
+      case 2: while(state.arc.circle_y<0)  { step_arc_along_y(1,1); }      
+      case 1: while(state.arc.circle_y<state.arc.circle_x)  { step_arc_along_y(-1,1); }
+      case 0: while(state.arc.circle_x>0)  { step_arc_along_x(-1,1); }
+    }    
+  }
+}
+
 void mc_go_home()
 {
   state.mode = MODE_HOME;
-  memset(state.home.direction, -1, sizeof(state.home.direction)); // direction = [-1,-1,-1]
-  set_direction_pins(state.home.direction);
-  clear_vector(state.home.away);
 }
 
 void perform_go_home() 
 {
-  int axis;
+  st_go_home();
-  if(state.home.phase == PHASE_HOME_RETURN) {
-    // We are running all axes in reverse until all limit switches are tripped
-    // Check all limit switches:
-    for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
-      state.home.away[axis] |= check_limit_switch(axis);
-    }
-    // Step steppers. First retract along Z-axis. Then X and Y.
-    if(state.home.away[Z_AXIS]) {
-      step_axis(Z_AXIS);
-    } else {
-      // Check if all axes are home
-      if(!(state.home.away[X_AXIS] || state.home.away[Y_AXIS])) {
-        // All axes are home, prepare next phase: to nudge the tool carefully out of the limit switches
-        memset(state.home.direction, 1, sizeof(state.home.direction)); // direction = [1,1,1]
-        set_direction_pins(state.home.direction);
-        state.home.phase == PHASE_HOME_NUDGE;
-        return;
-      }
-      step_steppers(state.home.away);
-    }
-  } else {    
-    for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
-      if(check_limit_switch(axis)) {
-        step_axis(axis);
-        return;
-      }
-    }
-    // When this code is reached it means all axes are free of their limit-switches. Complete the cycle and rest:
   clear_vector(state.position); // By definition this is location [0, 0, 0]
   state.mode = MODE_AT_REST;
 }
-}
 
 void mc_execute() {
-  enable_steppers();
+  st_set_pace(state.pace);
   while(state.mode) {
     switch(state.mode) {
       case MODE_AT_REST: break;
@@ -254,13 +303,7 @@ void mc_execute() {
       case MODE_LINEAR: perform_linear_motion();
       case MODE_HOME: perform_go_home();
     }
-    _delay_us(state.update_delay_us);
   }
-  disable_steppers();
-}
-
-void mc_wait() {  
-  return; // No concurrency support yet. So waiting for all to pass is moot.
 }
 
 int mc_status() 
@@ -268,49 +311,22 @@ int mc_status()
   return(state.mode);
 }
 
-int check_limit_switches()
-{
-  // Dual read as crude debounce
-  return((LIMIT_PORT & LIMIT_MASK) | (LIMIT_PORT & LIMIT_MASK));
-}
-
-int check_limit_switch(int axis)
-{
-  uint8_t mask = 0;
-  switch (axis) {
-    case X_AXIS: mask = 1<<X_LIMIT_BIT; break;
-    case Y_AXIS: mask = 1<<Y_LIMIT_BIT; break;
-    case Z_AXIS: mask = 1<<Z_LIMIT_BIT; break;
-  }
-  return((LIMIT_PORT&mask) || (LIMIT_PORT&mask));    
-}
-
-void enable_steppers()
-{
-  STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
-}
-
-void disable_steppers()
-{
-  STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
-}
 
 // Set the direction pins for the stepper motors according to the provided vector. 
 // direction is an array of three 8 bit integers representing the direction of 
 // each motor. The values should be -1 (reverse), 0 or 1 (forward).
-void set_direction_pins(int8_t *direction) 
+void set_direction_bits(int8_t *direction) 
 {
   /* Sorry about this convoluted code! It uses the fact that bit 7 of each direction
      int is set when the direction == -1, but is 0 when direction is forward. This
      way we can generate the whole direction bit-mask without doing any comparisions
      or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry.
   */
-  uint8_t forward_bits = ~(
+  direction_bits = ~(
     ((direction[X_AXIS]&128)>>(7-X_DIRECTION_BIT)) |
     ((direction[Y_AXIS]&128)>>(7-Y_DIRECTION_BIT)) |
     ((direction[Z_AXIS]&128)>>(7-Z_DIRECTION_BIT))
   );
-  DIRECTION_PORT = DIRECTION_PORT & ~(DIRECTION_MASK) | forward_bits;
 }
 
 // Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one
@@ -318,21 +334,15 @@ void set_direction_pins(int8_t *direction)
 // 1, and the rest to 0. 
 inline void step_steppers(uint8_t *enabled) 
 {
-  STEP_PORT |= enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT;
+  st_buffer_step(direction_bits | enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT);
-  _delay_us(5);
-  STEP_PORT &= ~STEP_MASK;
 }
 
 // Step only one motor
 inline void step_axis(uint8_t axis) 
 {
-  uint8_t mask = 0;
   switch (axis) {
-    case X_AXIS: mask = 1<<X_STEP_BIT; break;
+    case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
-    case Y_AXIS: mask = 1<<Y_STEP_BIT; break;
+    case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
-    case Z_AXIS: mask = 1<<Z_STEP_BIT; break;
+    case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
   }
-  STEP_PORT &= mask;
-  _delay_us(5);
-  STEP_PORT &= ~STEP_MASK;  
 }

motion_control.h

@@ -31,17 +31,16 @@ void mc_init();
 // Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement.
 void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate);
+
 // Prepare linear motion relative to the current position.
 void mc_dwell(uint32_t milliseconds);
+
 // Prepare to send the tool position home
 void mc_go_home();
+
 // Start the prepared operation.
 void mc_execute();
 
-// Block until the motion control system is idle
-void mc_wait();
-
-
 // Check motion control status. result == 0: the system is idle. result > 0: the system is busy,
 // result < 0: the system is in an error state.
 int mc_status();

stepper.c

@@ -0,0 +1,220 @@
+/*
+  stepper.c - stepper motor interface
+  Part of Grbl
+
+  Copyright (c) 2009 Simen Svale Skogsrud
+
+  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/>.
+*/
+
+#include "stepper.h"
+#include "config.h"
+#include "nuts_bolts.h"
+#include <avr/interrupt.h>
+
+#define TICKS_PER_MICROSECOND F_CPU/1000000
+#define STEP_BUFFER_SIZE 100
+
+volatile uint8_t step_buffer[STEP_BUFFER_SIZE]; // A buffer for step instructions
+volatile int step_buffer_head = 0;
+volatile int step_buffer_tail = 0;
+
+uint8_t state = STEPPER_STATE_STOPPED;
+
+// This timer interrupt is executed at the pace set with set_pace. It pops one instruction from
+// the step_buffer, executes it. Then it starts timer2 in order to reset the motor port after
+// five microseconds.
+SIGNAL(SIG_OUTPUT_COMPARE1A)
+{
+  if (step_buffer_head != step_buffer_tail) {
+    // Set the stepper port according to the instructions
+    MOTORS_PORT = (MOTORS_PORT & ~MOTORS_MASK) | step_buffer[step_buffer_tail];
+    // Reset and start timer 2 which will reset the motor port after 5 microsecond
+    TCNT2 = 0; // reset counter
+    OCR2A = 5*TICKS_PER_MICROSECOND; // set the time
+    TIMSK2 |= OCIE2A; // enable interrupt
+    // move the step buffer tail to the next instruction
+    step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
+	}
+}
+
+// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
+// the motor port after a short period (5us) completing one step cycle.
+SIGNAL(SIG_OUTPUT_COMPARE2A)
+{
+  MOTORS_PORT = MOTORS_PORT & ~MOTORS_MASK; // reset stepper pins
+  TIMSK2 &= ~OCIE2A; // disable this timer interrupt until next time
+}
+
+// Initialize and start the stepper motor subsystem
+void st_init()
+{
+	// Configure directions of interface pins
+  MOTORS_DDR   |= MOTORS_MASK;
+  LIMIT_DDR &= ~(LIMIT_MASK);
+  STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
+
+	// waveform generation = 0100 = CTC
+	TCCR1B &= ~(1<<WGM13);
+	TCCR1B |=  (1<<WGM12);
+	TCCR1A &= ~(1<<WGM11); 
+	TCCR1A &= ~(1<<WGM10);
+
+	// output mode = 00 (disconnected)
+	TCCR1A &= ~(3<<COM1A0); 
+	TCCR1A &= ~(3<<COM1B0); 
+	
+	// Configure Timer 2
+  TCCR2A = 0; // Normal operation
+  TCCT2B = 1<<CS20; // Full speed, no prescaler
+  TIMSK2 = 0; // All interrupts disabled
+  
+	// start off with a slow pace
+  st_set_pace(1000000);
+  st_start();
+}
+
+void st_buffer_step(uint8_t motor_port_bits)
+{
+	int i = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
+	
+	// If the buffer is full: good! That means we are well ahead of the robot. 
+	// Nap until there is room for more steps.
+	while(step_buffer_tail == i) { sleep_mode(); }
+	
+  step_buffer[step_buffer_head] = motor_port_bits;
+  step_buffer_head = i;
+}
+
+// Block until all buffered steps are executed
+void st_synchronize()
+{
+  if (state == STEPPER_MODE_RUNNING) {
+    while(step_buffer_tail != step_buffer_head) { sleep_mode(); }    
+  } else {
+    st_flush();
+  }
+}
+
+// Cancel all pending steps
+void st_flush()
+{
+  step_buffer_tail = step_buffer_head;
+}
+
+// Start the stepper subsystem
+void st_start()
+{
+  // Enable timer interrupt
+	TIMSK1 |= (1<<OCIE1A);
+  STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
+  state = STEPPER_STATE_RUNNING;
+}
+
+// Execute all buffered steps, then stop the stepper subsystem
+inline void st_stop()
+{
+  st_synchronize();
+	TIMSK1 &= ~(1<<OCIE1A);
+  STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
+  state = STEPPER_STATE_STOPPED;
+}
+
+void st_set_pace(uint32_t microseconds)
+{
+  uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;
+  uint16_t ceiling;
+  uint16_t prescaler;
+	if (ticks <= 65535L) {
+		ceiling = ticks;
+    prescaler = 0; // prescaler: 0
+	} else if (ticks <= 0x7ffffL) {
+    ceiling = ticks >> 3;
+    prescaler = 1; // prescaler: 8
+	} else if (ticks <= 0x3fffffL) {
+		ceiling =  ticks >> 6;
+    prescaler = 2; // prescaler: 64
+	} else if (ticks <= 0xffffffL) {
+		ceiling =  (ticks >> 8);
+    prescaler = 3; // prescaler: 256
+	} else if (ticks <= 0x3ffffffL) {
+		ceiling = (ticks >> 10);
+    prescaler = 4; // prescaler: 1024
+	} else {
+		ceiling = 0xffff;
+    prescaler = 4;
+	}
+	// Set prescaler
+  TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
+  // Set ceiling
+  OCR1A = ceiling;
+}
+
+int check_limit_switches()
+{
+  // Dual read as crude debounce
+  return((LIMIT_PORT & LIMIT_MASK) | (LIMIT_PORT & LIMIT_MASK));
+}
+
+int check_limit_switch(int axis)
+{
+  uint8_t mask = 0;
+  switch (axis) {
+    case X_AXIS: mask = 1<<X_LIMIT_BIT; break;
+    case Y_AXIS: mask = 1<<Y_LIMIT_BIT; break;
+    case Z_AXIS: mask = 1<<Z_LIMIT_BIT; break;
+  }
+  return((LIMIT_PORT&mask) || (LIMIT_PORT&mask));    
+}
+
+// void perform_go_home() 
+// {
+//   int axis;
+//   if(state.home.phase == PHASE_HOME_RETURN) {
+//     // We are running all axes in reverse until all limit switches are tripped
+//     // Check all limit switches:
+//     for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
+//       state.home.away[axis] |= check_limit_switch(axis);
+//     }
+//     // Step steppers. First retract along Z-axis. Then X and Y.
+//     if(state.home.away[Z_AXIS]) {
+//       step_axis(Z_AXIS);
+//     } else {
+//       // Check if all axes are home
+//       if(!(state.home.away[X_AXIS] || state.home.away[Y_AXIS])) {
+//         // All axes are home, prepare next phase: to nudge the tool carefully out of the limit switches
+//         memset(state.home.direction, 1, sizeof(state.home.direction)); // direction = [1,1,1]
+//         set_direction_bits(state.home.direction);
+//         state.home.phase == PHASE_HOME_NUDGE;
+//         return;
+//       }
+//       step_steppers(state.home.away);
+//     }
+//   } else {    
+//     for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
+//       if(check_limit_switch(axis)) {
+//         step_axis(axis);
+//         return;
+//       }
+//     }
+//     // When this code is reached it means all axes are free of their limit-switches. Complete the cycle and rest:
+//     clear_vector(state.position); // By definition this is location [0, 0, 0]
+//     state.mode = MODE_AT_REST;
+//   }
+// }
+
+void st_go_home()
+{
+  // Todo: Perform the homing cycle
+}

stepper.h

@@ -0,0 +1,56 @@
+/*
+  stepper.h - stepper motor interface
+  Part of Grbl
+
+  Copyright (c) 2009 Simen Svale Skogsrud
+
+  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 stepper_h
+#define stepper_h 
+
+#include <avr/io.h>
+#include <avr/sleep.h>
+
+#define STEPPER_STATE_STOPPED 0
+#define STEPPER_STATE_RUNNING 1
+#define STEPPER_STATE_LIMIT_OVERRUN 2
+#define STEPPER_STATE_HOMING 3
+
+// Initialize and start the stepper motor subsystem
+void st_init();
+
+// Set the rate steps are taken from the buffer and executed
+void st_set_pace(uint32_t microseconds);
+
+// Buffer a new instruction for the steppers
+void st_buffer_step(uint8_t motor_port_bits);
+
+// Block until all buffered steps are executed
+void st_synchronize();
+
+// Cancel all pending steps
+void st_flush();
+
+// Start the stepper subsystem
+void st_start();
+
+// Execute all buffered steps, then stop the stepper subsystem
+inline void st_stop();
+
+// Execute the homing cycle
+void st_go_home();
+
+#endif

todo.txt

@@ -1,3 +1,4 @@
+* Eliminate need for circle_x and circle_y in step_arc_along…
 * Use timer interrupts to drive steppers
 * Tool table
 * Tool length offsets