successfully maintained 30khz, lots of optimization of code and buffering allocation
This commit is contained in:
Simen Svale Skogsrud
parent
2be1f473cd
commit
df243d2490
4
gcode.c
4
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;
|
||||
|
||||
@ -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);
|
||||
|
||||
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);
|
||||
|
||||
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
|
||||
steps[axis] = target[axis]-position[axis];
|
||||
}
|
||||
|
||||
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(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 -----------------------------------------------------------------------------------------------
|
||||
|
||||
mode = MC_MODE_LINEAR;
|
||||
|
||||
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];
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
if(step_bits) {
|
||||
step_steppers(step_bits);
|
||||
}
|
||||
} while (step_bits);
|
||||
mode = MC_MODE_AT_REST;
|
||||
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);
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
45
stepper.c
45
stepper.c
@ -25,6 +25,7 @@
|
||||
#include "stepper.h"
|
||||
#include "config.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <util/delay.h>
|
||||
#include "nuts_bolts.h"
|
||||
#include <avr/interrupt.h>
|
||||
@ -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<<X_DIRECTION_BIT); }
|
||||
if (steps_y < 0) { direction_bits |= (1<<Y_DIRECTION_BIT); }
|
||||
if (steps_z < 0) { direction_bits |= (1<<Z_DIRECTION_BIT); }
|
||||
line->direction_bits = direction_bits;
|
||||
|
||||
// Move buffer head
|
||||
line_buffer_head = next_buffer_head;
|
||||
// enable stepper interrupt
|
||||
TIMSK1 |= (1<<OCIE1A);
|
||||
}
|
||||
|
||||
// This timer interrupt is executed at the pace set with st_buffer_pace. It pops one instruction from
|
||||
@ -87,6 +90,7 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
|
||||
// five microseconds.
|
||||
SIGNAL(SIG_OUTPUT_COMPARE1A)
|
||||
{
|
||||
PORTD |= (1<<3);
|
||||
// Set the direction pins a cuple of nanoseconds before we step the steppers
|
||||
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
|
||||
// Then pulse the stepping pins
|
||||
@ -96,16 +100,21 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
|
||||
|
||||
// If there is no current line, attempt to pop one from the buffer
|
||||
if (current_line == NULL) {
|
||||
PORTD &= ~(1<<4);
|
||||
// Anything in the buffer?
|
||||
if (line_buffer_head != line_buffer_tail) {
|
||||
PORTD ^= (1<<5);
|
||||
// Retrieve a new line and get ready to step it
|
||||
current_line = &line_buffer[line_buffer_tail];
|
||||
config_pace_timer(current_line->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<<OCIE1A);
|
||||
PORTD |= (1<<4);
|
||||
}
|
||||
}
|
||||
|
||||
@ -127,14 +136,17 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
|
||||
counter_z -= current_line->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<<CS10)) | ((prescaler+1)<<CS10);
|
||||
// Set ceiling
|
||||
OCR1A = ceiling;
|
||||
current_pace = microseconds;
|
||||
}
|
||||
|
||||
int check_limit_switches()
|
||||
|
||||
@ -28,7 +28,7 @@
|
||||
// using a ring buffer (I think), in which rx_buffer_head is the index of the
|
||||
// location to which to write the next incoming character and rx_buffer_tail
|
||||
// is the index of the location from which to read.
|
||||
#define RX_BUFFER_SIZE 256
|
||||
#define RX_BUFFER_SIZE 200
|
||||
|
||||
unsigned char rx_buffer[RX_BUFFER_SIZE];
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user