fixed buffering of pace changes and general cleaning

gcode.c

@@ -126,6 +126,7 @@ void select_plane(uint8_t axis_0, uint8_t axis_1)
 // characters and signed floats (no whitespace).
 uint8_t gc_execute_line(char *line) {
   int counter = 0;  
+  int requires_nudge = false;
   char letter;
   double value;
   double unit_converted_value;
@@ -376,17 +377,24 @@ 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
-      printString("mc_arc(");
-      printInteger(trunc(theta_start/M_PI*180)); printByte(',');
-      printInteger(trunc(angular_travel/M_PI*180)); printByte(',');
-      printInteger(trunc(radius));
-      printByte(')');
+      // 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;      
+      // Rounding errors means the arcing might not land us exactly where we wanted. Thats why this
+      // operation must be finalized with a linear nudge to the exact target spot.
+      requires_nudge = true;
+      break;
     }    
   }
   
   mc_execute();
+  if (requires_nudge) {
+    mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.feed_rate, false);
+    mc_execute();
+  }
   
   // As far as the parser is concerned, the position is now == target. In reality the
   // motion control system might still be processing the action and the real tool position

main.c

@@ -40,7 +40,7 @@ int main(void)
   sp_init(); // initialize the serial protocol
   
   for(;;){
-//    sleep_mode();
+    sleep_mode();
     sp_process(); // process the serial protocol
   }
   return 0;   /* never reached */

motion_control.c

@@ -60,7 +60,6 @@ struct ArcMotionParameters {
   int32_t error, x2, y2;                           // error is always == (x**2 + y**2 - radius**2), 
                                                    // x2 is always 2*x, y2 is always 2*y
   uint8_t axis_x, axis_y;                          // maps the arc axes to stepper axes
-  int32_t target[3];                               // The target position in absolute steps
   int8_t plane_steppers[3];                        // A vector with the steppers of axis_x and axis_y set to 1, the remaining 0
   int incomplete;                                  // True if the arc has not reached its target yet
 };
@@ -102,18 +101,12 @@ void mc_dwell(uint32_t milliseconds)
 // 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(trunc(x*X_STEPS_PER_MM), trunc(y*Y_STEPS_PER_MM), trunc(z*Z_STEPS_PER_MM), feed_rate, invert_feed_rate);
-}
-
-// Same as mc_linear_motion but accepts target in absolute step coordinates
-void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
 {
   memset(&mc.linear, 0, sizeof(mc.arc));
   
-  mc.linear.target[X_AXIS] = x;
-  mc.linear.target[Y_AXIS] = y;
-  mc.linear.target[Z_AXIS] = z;
+  mc.linear.target[X_AXIS] = x*X_STEPS_PER_MM;
+  mc.linear.target[Y_AXIS] = y*Y_STEPS_PER_MM;
+  mc.linear.target[Z_AXIS] = z*Z_STEPS_PER_MM;
   
   mc.mode = MC_MODE_LINEAR;
   uint8_t axis; // loop variable
@@ -125,9 +118,8 @@ void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate,
   // Find the magnitude of the axis with the longest travel
 	mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS], 
 	  max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS]));
-	if(mc.linear.maximum_steps == 0) { return; }
   // Nothing to do?
-	if ((mc.linear.maximum_steps) == 0) 
+	if (mc.linear.maximum_steps == 0) 
 	{ 
     mc.mode = MC_MODE_AT_REST;
     return; 
@@ -306,8 +298,7 @@ void execute_arc()
   // Update the tool position to the new actual position
   mc.position[mc.arc.axis_x] += mc.arc.x-start_x;
   mc.position[mc.arc.axis_y] += mc.arc.y-start_y;
-  // Todo: Because of rounding errors we might still be off by a step or two. 
-  mc.mode = MC_MODE_AT_REST;  
+  mc.mode = MC_MODE_AT_REST;
 }
 
 void mc_go_home()
@@ -324,15 +315,17 @@ void execute_go_home()
 }
 
 void mc_execute() {
-  st_set_pace(mc.pace);
-  sp_send_execution_marker();
-  while(mc.mode) { // Loop because one task might start another task
-    switch(mc.mode) {
-      case MC_MODE_AT_REST: break;
-      case MC_MODE_DWELL: st_synchronize(); _delay_ms(mc.dwell_milliseconds); mc.mode = MC_MODE_AT_REST; break;
-      case MC_MODE_LINEAR: execute_linear_motion(); break;
-      case MC_MODE_ARC: execute_arc(); break;
-      case MC_MODE_HOME: execute_go_home(); break;
+  if (mc.mode != MC_MODE_AT_REST) {
+    st_buffer_pace(mc.pace);
+    sp_send_execution_marker();
+    while(mc.mode) { // Loop because one task might start another task
+      switch(mc.mode) {
+        case MC_MODE_AT_REST: break;
+        case MC_MODE_DWELL: st_synchronize(); _delay_ms(mc.dwell_milliseconds); mc.mode = MC_MODE_AT_REST; break;
+        case MC_MODE_LINEAR: execute_linear_motion(); break;
+        case MC_MODE_ARC: execute_arc(); break;
+        case MC_MODE_HOME: execute_go_home(); break;
+      }
     }
   }
 }

stepper.c

@@ -35,24 +35,34 @@
 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;
+volatile uint32_t current_pace;
+volatile uint32_t next_pace = 0;
 
 uint8_t stepper_mode = STEPPER_MODE_STOPPED;
 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
 // 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 direction pins a nanosecond or two before you step the steppers
-    STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (step_buffer[step_buffer_tail] & DIRECTION_MASK);
-    // Then pulse the stepping pins
-    STEPPING_PORT = (STEPPING_PORT & ~STEP_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 |= (1<<OCIE2A); // enable interrupt
+    if(step_buffer[step_buffer_tail] == 0xff) {
+      // If this is not a step-instruction, but a pace-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);
+      // Then pulse the stepping pins
+      STEPPING_PORT = (STEPPING_PORT & ~STEP_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 |= (1<<OCIE2A); // enable interrupt
+    }
     // move the step buffer tail to the next instruction
     step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
   }
@@ -92,7 +102,7 @@ void st_init()
   sei();
   
 	// start off with a slow pace
-  st_set_pace(20000);
+  config_pace_timer(20000);
   st_start();
 }
 
@@ -148,11 +158,20 @@ inline void st_stop()
   stepper_mode = STEPPER_MODE_STOPPED;
 }
 
-void st_set_pace(uint32_t microseconds)
+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 (next_pace != 0) {
+    st_synchronize();
+  }  
+  next_pace = microseconds;
+  st_buffer_step(0xff);
+}
+
+void config_pace_timer(uint32_t microseconds)
 {
-  printString("pace = ");
-  printInteger(microseconds);
-  printString("\n\r");
   uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;
   uint16_t ceiling;
   uint16_t prescaler;
@@ -180,6 +199,7 @@ void st_set_pace(uint32_t microseconds)
   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
   // Set ceiling
   OCR1A = ceiling;
+  current_pace = microseconds;
 }
 
 int check_limit_switches()

todo.txt

@@ -4,7 +4,6 @@
 * 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 to implement st_set_pace? Consider synchronizing when pace is changed
 * 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