From c07a32258950da2de2bb0c809e14ab9d491b67aa Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 8 Feb 2009 20:40:24 +0100 Subject: [PATCH] fixed buffering of pace changes and general cleaning --- gcode.c | 20 ++++++++++++++------ main.c | 2 +- motion_control.c | 39 ++++++++++++++++----------------------- stepper.c | 46 +++++++++++++++++++++++++++++++++------------- todo.txt | 1 - 5 files changed, 64 insertions(+), 44 deletions(-) diff --git a/gcode.c b/gcode.c index 4b84853..55d3dae 100644 --- a/gcode.c +++ b/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 diff --git a/main.c b/main.c index 3392d9f..879bc45 100644 --- a/main.c +++ b/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 */ diff --git a/motion_control.c b/motion_control.c index b4efe7c..617e44a 100644 --- a/motion_control.c +++ b/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; + } } } } diff --git a/stepper.c b/stepper.c index bba54a3..483091c 100644 --- a/stepper.c +++ b/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<