optimized arc code for size and speed

motion_control.c

@@ -35,15 +35,6 @@
 #include "nuts_bolts.h"
 #include "stepper.h"
 
-#define MC_MODE_AT_REST 0
-#define MC_MODE_LINEAR 1
-#define MC_MODE_ARC 2
-#define MC_MODE_DWELL 3
-#define MC_MODE_HOME 4
-
-#define PHASE_HOME_RETURN 0
-#define PHASE_HOME_NUDGE 1
-
 #define ONE_MINUTE_OF_MICROSECONDS 60000000
 
 // Parameters when mode is MC_MODE_ARC
@@ -101,13 +92,13 @@ void mc_dwell(uint32_t milliseconds)
 {
   state.mode = MC_MODE_DWELL;
   state.dwell_milliseconds = milliseconds;
-  state.pace = 1000;
 }
 
+// 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)
 {
   state.mode = MC_MODE_LINEAR;
-  
 	state.linear.target[X_AXIS] = trunc(x*X_STEPS_PER_MM);
 	state.linear.target[Y_AXIS] = trunc(y*Y_STEPS_PER_MM);
 	state.linear.target[Z_AXIS] = trunc(z*Z_STEPS_PER_MM);
@@ -136,17 +127,17 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
   	state.pace = 
   	  (feed_rate*1000000)/state.linear.maximum_steps;	  
   } else {
-  	// Ask old Phytagoras how many millimeters our next move is going to take us:
+  	// Ask old Phytagoras to estimate how many steps our next move is going to take us:
-  	float millimeters_of_travel = 
+  	uint32_t steps_to_travel = 
-      sqrt(pow((X_STEPS_PER_MM*state.linear.step_count[X_AXIS]),2) + 
+      ceil(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));
+                pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2)));
   	state.pace = 
-  	  ((millimeters_of_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;	  
+  	  ((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;	  
   }
 }
 
-void perform_linear_motion()
+void execute_linear_motion()
 {
 	// Flags to keep track of which axes to step
   uint8_t step[3];
@@ -178,7 +169,8 @@ void perform_linear_motion()
 // Prepare 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. 
-void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2)
+// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
+void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate)
 {  
   state.mode = MC_MODE_ARC;
   // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
@@ -207,8 +199,24 @@ 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
   state.arc.axis_x = axis_1;
   state.arc.axis_y = axis_2;
+  // mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
+  state.pace = 
+    ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
 }
 
+// Reset mode if arc has reached target
+inline void check_arc_target()
+{   
+  if ((state.arc.x * state.arc.target_direction_y >= 
+          state.arc.target_x * state.arc.target_direction_y) && 
+         (state.arc.y * state.arc.target_direction_x <= 
+          state.arc.target_y * state.arc.target_direction_x))
+  {
+    state.mode = MC_MODE_AT_REST;
+  }
+}
+
+// Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
 void step_arc_along_x(int8_t dx, int8_t dy) 
 {
   uint32_t diagonal_error;
@@ -224,8 +232,10 @@ void step_arc_along_x(int8_t dx, int8_t dy)
   } else {
     step_axis(state.arc.axis_x); // step straight
   }
+  check_arc_target();
 }
 
+// Internal method used by execute_arc to trace vertically in the general direction provided by dx and dy
 void step_arc_along_y(int8_t dx, int8_t dy) 
 {  
   uint32_t diagonal_error;
@@ -241,13 +251,14 @@ void step_arc_along_y(int8_t dx, int8_t dy)
   } else {
     step_axis(state.arc.axis_y); // step straight
   }
+  check_arc_target();
 }
 
-// Map dx and dy which are local to the arc being generated and map them on to the 
+// Take dx and dy which are local to the arc being generated and map them on to the 
-// selected axes for the current arc.
+// selected tool-space-axes for the current arc.
 void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
 {
-  int direction[3];
+  int8_t direction[3];
   direction[state.arc.axis_x] = dx;
   direction[state.arc.axis_y] = dy;
   set_stepper_directions(direction);
@@ -284,92 +295,77 @@ int quadrant(uint32_t x,uint32_t y)
   }  
 }
 
-
+// Will trace the configured arc until the target is reached. Slightly unrolled for speed.
-int arc_at_target()
+void execute_arc()
-{  
-  return((state.arc.x * state.arc.target_direction_y >= 
-          state.arc.target_x * state.arc.target_direction_y) && 
-         (state.arc.y * state.arc.target_direction_x <= 
-          state.arc.target_y * state.arc.target_direction_x)); 
-}
-
-
-// Will trace the configured arc until the target is reached
-void trace_arc()
 {
   int q = quadrant(state.arc.x, state.arc.y);
-  while(!arc_at_target())
+  // state.mode is set to 0 (MC_MODE_AT_REST) when target is reached
+  while(state.mode == MC_MODE_ARC)
   {
     if (state.arc.angular_direction) {
       switch (q) {
         case 0: 
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while((!arc_at_target()) && state.arc.x>state.arc.y) { step_arc_along_x(1,-1); }
+        while(state.mode && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
         case 1: 
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while((!arc_at_target()) && state.arc.y>0) { step_arc_along_y(1,-1); }
+        while(state.mode && (state.arc.y>0)) { step_arc_along_y(1,-1); }
         case 2: 
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while((!arc_at_target()) && state.arc.y>-state.arc.x) { step_arc_along_y(-1,-1); }
+        while(state.mode && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
         case 3: 
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while((!arc_at_target()) && state.arc.x>0) { step_arc_along_x(-1,-1); }
+        while(state.mode && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
         case 4: 
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while((!arc_at_target()) && state.arc.y<state.arc.x) { step_arc_along_x(-1,1); }
+        while(state.mode && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
         case 5: 
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while((!arc_at_target()) && state.arc.y<0) { step_arc_along_y(-1,1); }
+        while(state.mode && (state.arc.y<0)) { step_arc_along_y(-1,1); }
         case 6: 
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while((!arc_at_target()) && state.arc.y<-state.arc.x) { step_arc_along_y(1,1); }
+        while(state.mode && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
         case 7: 
         map_local_arc_directions_to_stepper_directions(1,1);
-        while((!arc_at_target()) && state.arc.x<0) { step_arc_along_x(1,1); }
+        while(state.mode && (state.arc.x<0)) { step_arc_along_x(1,1); }
       }
     } else {
       switch (q) {
         case 7:
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while((!arc_at_target()) && state.arc.y>-state.arc.x) { step_arc_along_x(-1,-1); }
+        while(state.mode && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
         case 6:
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while((!arc_at_target()) && state.arc.y>0)  { step_arc_along_y(-1,-1); }
+        while(state.mode && (state.arc.y>0))  { step_arc_along_y(-1,-1); }
         case 5:
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while((!arc_at_target()) && state.arc.y>state.arc.x)  { step_arc_along_y(1,-1); }
+        while(state.mode && (state.arc.y>state.arc.x))  { step_arc_along_y(1,-1); }
         case 4:
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while((!arc_at_target()) && state.arc.x<0)  { step_arc_along_x(1,-1); }
+        while(state.mode && (state.arc.x<0))  { step_arc_along_x(1,-1); }
         case 3:
         map_local_arc_directions_to_stepper_directions(1,1);
-        while((!arc_at_target()) && state.arc.y<-state.arc.x) { step_arc_along_x(1,1); }      
+        while(state.mode && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }      
         case 2:
         map_local_arc_directions_to_stepper_directions(1,1);
-        while((!arc_at_target()) && state.arc.y<0)  { step_arc_along_y(1,1); }      
+        while(state.mode && (state.arc.y<0))  { step_arc_along_y(1,1); }      
         case 1:
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while((!arc_at_target()) && state.arc.y<state.arc.x)  { step_arc_along_y(-1,1); }
+        while(state.mode && (state.arc.y<state.arc.x))  { step_arc_along_y(-1,1); }
         case 0:
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while((!arc_at_target()) && state.arc.x>0)  { step_arc_along_x(-1,1); }
+        while(state.mode && (state.arc.x>0))  { step_arc_along_x(-1,1); }
       }    
     }
   }
 }
 
-void perform_arc()
-{
-  trace_arc();
-  state.mode = MC_MODE_AT_REST;
-}
-
 void mc_go_home()
 {
   state.mode = MC_MODE_HOME;
 }
 
-void perform_go_home() 
+void execute_go_home() 
 {
   st_go_home();
   st_synchronize();
@@ -383,9 +379,9 @@ void mc_execute() {
     switch(state.mode) {
       case MC_MODE_AT_REST: break;
       case MC_MODE_DWELL: st_synchronize(); _delay_ms(state.dwell_milliseconds); state.mode = MC_MODE_AT_REST; break;
-      case MC_MODE_LINEAR: perform_linear_motion();
+      case MC_MODE_LINEAR: execute_linear_motion(); break;
-      case MC_MODE_ARC: perform_arc();
+      case MC_MODE_ARC: execute_arc(); break;
-      case MC_MODE_HOME: perform_go_home();
+      case MC_MODE_HOME: execute_go_home(); break;
     }
   }
 }
@@ -395,7 +391,6 @@ int mc_status()
   return(state.mode);
 }
 
-
 // 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).
@@ -430,3 +425,9 @@ inline void step_axis(uint8_t axis)
     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

@@ -23,7 +23,11 @@
 
 #include <avr/io.h>
 
-/* All coordinates are in step-counts. */
+#define MC_MODE_AT_REST 0
+#define MC_MODE_LINEAR 1
+#define MC_MODE_ARC 2
+#define MC_MODE_DWELL 3
+#define MC_MODE_HOME 4
 
 // Initializes the motion_control subsystem resources
 void mc_init();
@@ -37,7 +41,7 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
 // circle in millimeters. axis_1 and axis_2 selects the plane in tool space. 
 // Known issue: This method pretends that all axes uses the same steps/mm as the X axis. Which might
 // not be the case ... (To be continued)
-void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2);
+void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate);
 
 // Prepare linear motion relative to the current position.
 void mc_dwell(uint32_t milliseconds);
@@ -45,9 +49,14 @@ void mc_dwell(uint32_t milliseconds);
 // Prepare to send the tool position home
 void mc_go_home();
 
-// Start the prepared operation.
+// Start the prepared operation. In the current implementation this will block for most of the task at hand.
+// In future implementations it might not block at all. If you want to make sure the system has reached 
+// 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();