optimized for size and did some housekeeping

gcode.c

@@ -54,8 +54,6 @@
 #include "errno.h"
 #include "serial_protocol.h"
 
-#include "wiring_serial.h"
-
 #define NEXT_ACTION_DEFAULT 0
 #define NEXT_ACTION_DWELL 1
 #define NEXT_ACTION_GO_HOME 2
@@ -195,11 +193,11 @@ uint8_t gc_execute_line(char *line) {
   // If there were any errors parsing this line, we will return right away with the bad news
   if (gc.status_code) { return(gc.status_code); }
 
-  // Pass 2: Parameters
   counter = 0;
   clear_vector(offset);
   memcpy(target, gc.position, sizeof(target)); // target = gc.position
 
+  // Pass 2: Parameters
   while(next_statement(&letter, &value, line, &counter)) {
     int_value = trunc(value);
     unit_converted_value = to_millimeters(value);
@@ -313,7 +311,6 @@ uint8_t gc_execute_line(char *line) {
         // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
         // real CNC, and thus - for practical reasons - we will terminate promptly:
         if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
-
         // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
         if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
         
@@ -336,10 +333,9 @@ uint8_t gc_execute_line(char *line) {
 
         // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), 
         // even though it is advised against ever generating such circles in a single line of g-code. By 
-        // inverting the sign of h_x2_div_d the center of the circles is placed on the opposide side of the line of
+        // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
         // travel and thus we get the unadvisably long circles as prescribed.
         if (r < 0) { h_x2_div_d = -h_x2_div_d; }        
-        
         // Complete the operation by calculating the actual center of the arc
         offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
         offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
@@ -375,12 +371,7 @@ uint8_t gc_execute_line(char *line) {
       }
       // Find the radius
       double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
-      // Prepare the arc
+      // Trace the arc
-      // printString("mc_arc(");
-      // printInteger(trunc(theta_start/M_PI*180)); printByte(',');
-      // printInteger(trunc(angular_travel/M_PI*180)); printByte(',');
-      // printInteger(trunc(radius));
-      // printByte(')');
       mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);        
       break;
     }    

motion_control.c

@@ -34,13 +34,10 @@
 #include <stdlib.h>
 #include "nuts_bolts.h"
 #include "stepper.h"
-#include "serial_protocol.h"
-
-#include "wiring_serial.h"
 
 #define ONE_MINUTE_OF_MICROSECONDS 60000000.0
 
-int8_t mode;            // The current operation mode
+volatile int8_t mode;   // The current operation mode
 int32_t position[3];    // The current position of the tool in absolute steps
 uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
 
@@ -51,7 +48,7 @@ void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate,
 
 void mc_init()
 {
-  mode = 0;
+  mode = MC_MODE_AT_REST;
   clear_vector(position);
 }
 
@@ -63,7 +60,7 @@ void mc_dwell(uint32_t milliseconds)
   mode = MC_MODE_AT_REST; 
 }
 
-// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
+// Execute 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_line(double x, double y, double z, float feed_rate, int invert_feed_rate)
 {
@@ -76,12 +73,12 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
     counter[3],      // A counter used in the bresenham algorithm for line plotting
     maximum_steps;   // The larges absolute step-count of any axis
   
+  
+  // Setup
+
   target[X_AXIS] = x*X_STEPS_PER_MM;
   target[Y_AXIS] = y*Y_STEPS_PER_MM;
   target[Z_AXIS] = z*Z_STEPS_PER_MM;
-  
-  mode = MC_MODE_LINEAR;
-
   // Determine direction and travel magnitude for each axis
   for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
   	step_count[axis] = abs(target[axis] - position[axis]);
@@ -91,11 +88,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
 	maximum_steps = max(step_count[Z_AXIS], 
 	  max(step_count[X_AXIS], step_count[Y_AXIS]));
   // Nothing to do?
-	if (maximum_steps == 0) 
+	if (maximum_steps == 0) { return; }
-	{ 
-    mode = MC_MODE_AT_REST;
-    return; 
-	}
 	// Set up a neat counter for each axis
   for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
     counter[axis] = -maximum_steps/2;
@@ -118,6 +111,8 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
   
   // Execution
 
+  mode = MC_MODE_LINEAR;
+  
   while(mode) {
   	// Trace the line
     step_bits = 0;
@@ -142,7 +137,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
 }
 
 
-// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc,
+// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
 // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
 // circle in millimeters. axis_1 and axis_2 selects the plane in tool space. 
 // ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
@@ -165,9 +160,10 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
   
   int dx, dy; // Trace directions
 
+  // Setup
+  
   uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
 	if(radius_steps == 0) { return; }
-  mode = MC_MODE_ARC;
   // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
   angular_direction = signof(angular_travel);
   // Calculate the initial position and target position in the local coordinate system of the arc
@@ -192,39 +188,37 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
   // And map the local coordinate system of the arc onto the tool axes of the selected plane
   axis_x = axis_1;
   axis_y = axis_2;
-  // The amount of steppings performed while tracing a full circle is equal to the sum of sides in a 
+  // The amount of steppings performed while tracing a half circle is equal to the sum of sides in a 
-  // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a full circle:
+  // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a half circle:
   uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2)));
-  // We then calculate the millimeters of travel along the circumference of that same full circle
+  // We then calculate the millimeters of travel along the circumference of that same half circle
   double millimeters_half_circumference = radius*M_PI;
   // Then we calculate the microseconds between each step as if we will trace the full circle.
-  // It doesn't matter what fraction of the circle we are actuallyt going to trace. The pace is the same.
+  // It doesn't matter what fraction of the circle we are actually going to trace. The pace is the same.
   st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle);    
 
-  incomplete = true;
-
   // Execution
 
+  mode = MC_MODE_ARC;
+  
+  incomplete = true;
   while(incomplete)
   {
     dx = (y!=0) ?  signof(y) * angular_direction : -signof(x);
     dy = (x!=0) ? -signof(x) * angular_direction : -signof(y);
-
     // Take dx and dy which are local to the arc being generated and map them on to the 
     // selected tool-space-axes for the current arc.
     direction[axis_x] = dx;
     direction[axis_y] = dy;
     set_stepper_directions(direction);
-
+    // Check which axis will be "major" for this stepping
     if (abs(x)<abs(y)) {
       // Step arc horizontally      
-      x+=dx;
       error += 1+x2*dx;
-      x2 += 2*dx;
+      x+=dx; x2 += 2*dx;
       diagonal_error = error + 1 + y2*dy;
       if(abs(error) >= abs(diagonal_error)) {
-        y += dy;
+        y += dy; y2 += 2*dy;
-        y2 += 2*dy;
         error = diagonal_error;
         step_steppers(diagonal_bits); // step diagonal
       } else {
@@ -232,21 +226,18 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
       }
     } else {      
       // Step arc vertically      
-      y+=dy; 
       error += 1+y2*dy; 
-      y2 += 2*dy; 
+      y+=dy; y2 += 2*dy; 
       diagonal_error = error + 1 + x2*dx; 
       if(abs(error) >= abs(diagonal_error)) { 
-        x += dx; 
+        x += dx; x2 += 2*dx; 
-        x2 += 2*dx; 
         error = diagonal_error; 
         step_steppers(diagonal_bits); // step diagonal
       } else {
         step_axis(axis_y); // step straight
       }
     }    
-    
+    // Check if target has been reached. Todo: Simplify/optimize/clarify
-    // Check if target has been reached
     if ((x * target_direction_y >= 
             target_x * target_direction_y) && 
            (y * target_direction_x <= 
@@ -254,7 +245,6 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
     { if ((signof(x) == signof(target_x)) && (signof(y) == signof(target_y))) 
       { incomplete = false; } }    
   }  
-  
   // Update the tool position to the new actual position
   position[axis_x] += x-start_x;
   position[axis_y] += y-start_y;
@@ -275,7 +265,7 @@ int mc_status()
   return(mode);
 }
 
-// Set the direction pins for the stepper motors according to the provided vector. 
+// Set the direction bits 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_stepper_directions(int8_t *direction) 
@@ -302,16 +292,5 @@ inline void step_steppers(uint8_t bits)
 // Step only one motor
 inline void step_axis(uint8_t axis) 
 {
-  switch (axis) {
+  st_buffer_step(direction_bits | st_bit_for_stepper(axis));
-    case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
-    case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
-    case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
-  }
 }
-
-// Wait until all operations are completed
-void mc_wait()
-{
-  st_synchronize();
-}
-

motion_control.h

@@ -54,13 +54,8 @@ void mc_go_home();
 // quiescence call mc_wait()
 void mc_execute();
 
-// Wait until all operations complete
-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();
 
-void printCurrentPosition();
-
 #endif

stepper.c

@@ -32,6 +32,9 @@
 #define TICKS_PER_MICROSECOND (F_CPU/1000000)
 #define STEP_BUFFER_SIZE 100
 
+// A marker used to notify the stepper handler of a pace change
+#define PACE_CHANGE_MARKER 0xff
+
 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;
@@ -43,24 +46,25 @@ uint8_t echo_steps = true;
 
 void config_pace_timer(uint32_t microseconds);
 
-// This timer interrupt is executed at the pace set with set_pace. It pops one instruction from
+// This timer interrupt is executed at the pace set with st_buffer_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) {
-    if(step_buffer[step_buffer_tail] == 0xff) {
+    uint8_t popped = step_buffer[step_buffer_tail]; 
-      // If this is not a step-instruction, but a pace-marker: change pace
+    if(popped == PACE_CHANGE_MARKER) {
+      // This is not a step-instruction, but a pace-change-marker: change pace
       config_pace_timer(next_pace);
       next_pace = 0;
     } else {
       // Set the direction pins a nanosecond or two before you step the steppers
-      STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (step_buffer[step_buffer_tail] & DIRECTION_MASK);
+      STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (popped & DIRECTION_MASK);
       // Then pulse the stepping pins
-      STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | step_buffer[step_buffer_tail];
+      STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | popped;
       // 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
+      OCR2A = 5*TICKS_PER_MICROSECOND; // set the trigger time
       TIMSK2 |= (1<<OCIE2A);           // enable interrupt
     }
     // move the step buffer tail to the next instruction
@@ -101,7 +105,7 @@ void st_init()
   
   sei();
   
-	// start off with a slow pace
+	// start off with a mellow pace
   config_pace_timer(20000);
   st_start();
 }
@@ -163,12 +167,12 @@ void st_buffer_pace(uint32_t microseconds)
 {
   // Do nothing if the pace in unchanged
   if (current_pace == microseconds) { return; }
-  // If the one-element pace buffer is full, flush step buffer
+  // If the single-element pace "buffer" is full, sleep until it is popped
-  if (next_pace != 0) {
+  while (next_pace != 0) {
-    st_synchronize();
+    sleep_mode();
   }  
   next_pace = microseconds;
-  st_buffer_step(0xff);
+  st_buffer_step(PACE_CHANGE_MARKER); // Place a pace-change marker in the step-buffer
 }
 
 uint8_t st_bit_for_stepper(int axis) {

todo.txt

@@ -1,13 +1,12 @@
+* Support helical interpolation
 * Optimize arc target detection code utilizing the primary axis of travel
-* Use bitmasks, not vectors to build steps in motion_control
+* Arcs might be a step or two off because of FP gotchas. Must add a little nudge in the end there?
-* Arcs might be a step or two off because of FP gotchas. Must add a little nudge in the end there
+* Support inverse feed rate for arcs
-* Generalize feed rate code and support inverse feed rate for arcs
 * Implement homing cycle in stepper.c
 * Implement limit switch support in stepper.c (use port-triggered interrupts?)
 * How on earth am I going to deal with arcs in setups that have different steps/mm on each axis? Must 
   support elipses?! Oh no.
-* Support helical interpolation
+* Eliminate need for x and y in step_arc_along_
-* Eliminate need for circle_x and circle_y in step_arc_along_
 * Tool table
 * Tool length offsets
 * Tool change M6