From df243d2490f176254f5d3fad79f6bd71926aa883 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Wed, 3 Mar 2010 00:26:48 +0100 Subject: [PATCH] successfully maintained 30khz, lots of optimization of code and buffering allocation --- gcode.c | 4 +- motion_control.c | 97 ++++++++++-------------------------------------- script/stream.rb | 2 +- stepper.c | 53 +++++++++++++++----------- wiring_serial.c | 2 +- 5 files changed, 56 insertions(+), 102 deletions(-) diff --git a/gcode.c b/gcode.c index 8b9ca67..ae11358 100644 --- a/gcode.c +++ b/gcode.c @@ -112,7 +112,7 @@ void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) void gc_init() { memset(&gc, 0, sizeof(gc)); - gc.feed_rate = DEFAULT_FEEDRATE; + gc.feed_rate = DEFAULT_FEEDRATE/60; select_plane(X_AXIS, Y_AXIS, Z_AXIS); gc.absolute_mode = true; } @@ -209,7 +209,7 @@ uint8_t gc_execute_line(char *line) { if (gc.inverse_feed_rate_mode) { inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only } else { - gc.feed_rate = unit_converted_value; // millimeters pr second + gc.feed_rate = unit_converted_value/60; // millimeters pr second } break; case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break; diff --git a/motion_control.c b/motion_control.c index 3914e39..c702b8c 100644 --- a/motion_control.c +++ b/motion_control.c @@ -63,90 +63,38 @@ void mc_dwell(uint32_t milliseconds) mode = MC_MODE_AT_REST; } -// Calculate the microseconds between steps that we should wait in order to travel the -// designated amount of millimeters in the amount of steps we are going to generate -void compute_and_set_step_pace(double feed_rate, double millimeters_of_travel, uint32_t steps, int invert) { - int32_t pace; - if (invert) { - pace = round(ONE_MINUTE_OF_MICROSECONDS/feed_rate/steps); - } else { - pace = round((ONE_MINUTE_OF_MICROSECONDS/X_STEPS_PER_MM)/feed_rate); - } - st_buffer_pace(pace); -} - // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in // 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 --------------------------------------------------------------------------------------------------- - PORTD |= (1<<4); - PORTD |= (1<<5); - target[X_AXIS] = round(x*X_STEPS_PER_MM); - target[Y_AXIS] = round(y*Y_STEPS_PER_MM); - target[Z_AXIS] = round(z*Z_STEPS_PER_MM); - PORTD ^= (1<<5); - // Determine direction and travel magnitude for each axis - for(axis = X_AXIS; axis <= Z_AXIS; axis++) { - step_count[axis] = labs(target[axis] - position[axis]); - direction[axis] = signof(target[axis] - position[axis]); - } - PORTD ^= (1<<5); - // Find the magnitude of the axis with the longest travel - maximum_steps = max(step_count[Z_AXIS], - max(step_count[X_AXIS], step_count[Y_AXIS])); - PORTD ^= (1<<5); - // Nothing to do? - if (maximum_steps == 0) { PORTD &= ~(1<<4); PORTD |= (1<<5); return; } - PORTD ^= (1<<5); - // Set up a neat counter for each axis - for(axis = X_AXIS; axis <= Z_AXIS; axis++) { - counter[axis] = -maximum_steps/2; - } - PORTD ^= (1<<5); - // Set our direction pins - set_stepper_directions(direction); - PORTD ^= (1<<5); - // Ask old Phytagoras to estimate how many mm our next move is going to take us - double millimeters_of_travel = - sqrt(square(X_STEPS_PER_MM*step_count[X_AXIS]) + - square(Y_STEPS_PER_MM*step_count[Y_AXIS]) + - square(Z_STEPS_PER_MM*step_count[Z_AXIS])); - PORTD ^= (1<<5); - // And set the step pace - compute_and_set_step_pace(feed_rate, millimeters_of_travel, maximum_steps, invert_feed_rate); - PORTD &= ~(1<<5); - PORTD &= ~(1<<4); - - // Execution ----------------------------------------------------------------------------------------------- + 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); - mode = MC_MODE_LINEAR; + for(axis = X_AXIS; axis <= Z_AXIS; axis++) { + steps[axis] = target[axis]-position[axis]; + } - do { - // Trace the line - step_bits = 0; - for(axis = X_AXIS; axis <= Z_AXIS; axis++) { - if (target[axis] != position[axis]) - { - counter[axis] += step_count[axis]; - if (counter[axis] > 0) - { - step_bits |= st_bit_for_stepper(axis); - counter[axis] -= maximum_steps; - position[axis] += direction[axis]; - } - } - } - if(step_bits) { - step_steppers(step_bits); - } - } while (step_bits); - mode = MC_MODE_AT_REST; + if (invert_feed_rate) { + st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate)); + } else { + // Ask old Phytagoras to estimate how many mm our next move is going to take us + double millimeters_of_travel = sqrt( + square(steps[X_AXIS]/X_STEPS_PER_MM) + + square(steps[Y_AXIS]/Y_STEPS_PER_MM) + + square(steps[Z_AXIS]/Z_STEPS_PER_MM)); + st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], + lround((millimeters_of_travel/feed_rate)*1000000)); + } + memcpy(position, target, sizeof(target)); // position[] = target[] } @@ -189,8 +137,3 @@ void set_stepper_directions(int8_t *direction) ((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) | ((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT))); } - -inline void step_steppers(uint8_t bits) -{ - st_buffer_step(direction_bits | bits); -} diff --git a/script/stream.rb b/script/stream.rb index 2b11d24..0698d67 100644 --- a/script/stream.rb +++ b/script/stream.rb @@ -29,7 +29,7 @@ SerialPort.open('/dev/tty.usbserial-A9007QcR', 9600) do |sp| sleep 5 ARGV.each do |file| puts "Processing file #{file}" - prebuffer = $prebuffer ? 7 : 0 + prebuffer = $prebuffer ? 12 : 0 File.readlines(file).each do |line| next if line.strip == '' puts line.strip diff --git a/stepper.c b/stepper.c index 0f5c6fa..a860aa3 100644 --- a/stepper.c +++ b/stepper.c @@ -25,6 +25,7 @@ #include "stepper.h" #include "config.h" #include +#include #include #include "nuts_bolts.h" #include @@ -32,30 +33,30 @@ #include "wiring_serial.h" #define TICKS_PER_MICROSECOND (F_CPU/1000000) -#define LINE_BUFFER_SIZE 5 +#define LINE_BUFFER_SIZE 8 struct Line { uint32_t steps_x, steps_y, steps_z; - uint32_t maximum_steps; - uint32_t iterations; + int32_t maximum_steps; uint8_t direction_bits; uint32_t rate; -} +}; -volatile uint8_t line_buffer[LINE_BUFFER_SIZE]; // A buffer for step instructions +struct Line line_buffer[LINE_BUFFER_SIZE]; // A buffer for step instructions volatile int line_buffer_head = 0; volatile int line_buffer_tail = 0; // Variables used by SIG_OUTPUT_COMPARE1A uint8_t out_bits; struct Line *current_line; -uint32_t counter_x, counter_y, counter_z; +volatile int32_t counter_x, counter_y, counter_z; +uint32_t iterations; uint8_t stepper_mode = STEPPER_MODE_STOPPED; void config_pace_timer(uint32_t microseconds); -void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate) { +void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds) { // Buffer nothing unless stepping subsystem is running if (stepper_mode != STEPPER_MODE_RUNNING) { return; } // Calculate the buffer head after we push this byte @@ -68,18 +69,20 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t struct Line *line = &line_buffer[line_buffer_head]; line->steps_x = labs(steps_x); line->steps_y = labs(steps_y); - line->steps_z = labs(steps_y); + line->steps_z = labs(steps_z); line->maximum_steps = max(line->steps_x, max(line->steps_y, line->steps_z)); - line->iterations = line->maximum_steps; - line->rate = rate; + // Bail if this is a zero-length line + if (line->maximum_steps == 0) { return; }; + line->rate = microseconds/line->maximum_steps; uint8_t direction_bits = 0; if (steps_x < 0) { direction_bits |= (1<direction_bits = direction_bits; - // Move buffer head line_buffer_head = next_buffer_head; + // enable stepper interrupt + TIMSK1 |= (1<rate); - counter_x = -current_line->maximum_steps/2; + counter_x = -(current_line->maximum_steps/2); counter_y = counter_x; counter_z = counter_x; - // move the line buffer tail to the next instruction - line_buffer_tail = (line_buffer_tail + 1) % LINE_BUFFER_SIZE; - } - } + iterations = current_line->maximum_steps; + } else { + // disable this interrupt until there is something to handle + TIMSK1 &= ~(1<direction_bits; @@ -116,25 +125,28 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) out_bits |= (1<maximum_steps; } - counter_y += current_line-> steps_y; + counter_y += current_line->steps_y; if (counter_y > 0) { out_bits |= (1<maximum_steps; } - counter_z += current_line-> steps_z; + counter_z += current_line->steps_z; if (counter_z > 0) { out_bits |= (1<maximum_steps; } // If current line is finished, reset pointer - current_line->iterations -= 1; - if (current_line->iterations <= 0) { + iterations -= 1; + if (iterations <= 0) { current_line = NULL; + // move the line buffer tail to the next instruction + line_buffer_tail = (line_buffer_tail + 1) % LINE_BUFFER_SIZE; } } else { out_bits = 0; } out_bits ^= STEPPING_INVERT_MASK; + PORTD &= ~(1<<3); } // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets @@ -258,7 +270,6 @@ void config_pace_timer(uint32_t microseconds) TCCR1B = (TCCR1B & ~(0x07<