further refactoring debris extraction

motion_control.c

@@ -34,18 +34,11 @@
 #include <stdlib.h>
 #include "nuts_bolts.h"
 #include "stepper.h"
-#include "geometry.h"
 
 #include "wiring_serial.h"
 
-#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
-
 int32_t position[3];    // The current position of the tool in absolute steps
 
-inline void step_steppers(uint8_t bits);
-inline void step_axis(uint8_t axis);
-void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
-
 void mc_init()
 {
   clear_vector(position);
@@ -53,7 +46,8 @@ void mc_init()
 
 void mc_dwell(uint32_t milliseconds) 
 {
-  st_buffer_line(0, 0, 0, milliseconds*1000);
+  st_synchronize();
+  _delay_ms(milliseconds);
 }
 
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
@@ -61,13 +55,10 @@ void mc_dwell(uint32_t milliseconds)
 // 1/feed_rate minutes.
 void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate)
 {
-	// Flags to keep track of which axes to step
   uint8_t axis; // loop variable
   int32_t target[3]; // The target position in absolute steps
   int32_t steps[3]; // The target line in relative steps
   
-  // Setup ---------------------------------------------------------------------------------------------------
-
   target[X_AXIS] = lround(x*X_STEPS_PER_MM);
   target[Y_AXIS] = lround(y*Y_STEPS_PER_MM);
   target[Z_AXIS] = lround(z*Z_STEPS_PER_MM); 
@@ -95,6 +86,10 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
 // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
 // circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
 // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
+
+// The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
+// segment is configured in config.h by setting MM_PER_ARC_SEGMENT.  
+
 // 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, double linear_travel, int axis_1, int axis_2, 
   int axis_linear, double feed_rate, int invert_feed_rate)
@@ -102,14 +97,22 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr
   double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
   if (millimeters_of_travel == 0.0) { return; }
   uint16_t segments = ceil(millimeters_of_travel/MM_PER_ARC_SEGMENT);
+  // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
+  // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
+  // all segments.
   if (invert_feed_rate) { feed_rate *= segments; }
+  // The angular motion for each segment
   double theta_per_segment = angular_travel/segments;
+  // The linear motion for each segment
   double linear_per_segment = linear_travel/segments;
+  // Compute the center of this circle
   double center_x = (position[axis_1]/X_STEPS_PER_MM)-sin(theta)*radius;
   double center_y = (position[axis_2]/Y_STEPS_PER_MM)-cos(theta)*radius;
+  // a vector to track the end point of each segment
   double target[3];
   int i;
-  target[axis_linear] = position[axis_linear];
+  // Initialize the linear axis
+  target[axis_linear] = position[axis_linear]/Z_STEPS_PER_MM;
   for (i=0; i<=segments; i++) {
     target[axis_linear] += linear_per_segment;
     theta += theta_per_segment;