optimized for size and did some housekeeping

This commit is contained in:
Simen Svale Skogsrud 2009-02-08 22:08:27 +01:00
parent 3e5e866115
commit 2992683c8d
5 changed files with 59 additions and 91 deletions

15
gcode.c
View File

@ -54,8 +54,6 @@
#include "errno.h" #include "errno.h"
#include "serial_protocol.h" #include "serial_protocol.h"
#include "wiring_serial.h"
#define NEXT_ACTION_DEFAULT 0 #define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1 #define NEXT_ACTION_DWELL 1
#define NEXT_ACTION_GO_HOME 2 #define NEXT_ACTION_GO_HOME 2
@ -195,11 +193,11 @@ uint8_t gc_execute_line(char *line) {
// If there were any errors parsing this line, we will return right away with the bad news // If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); } if (gc.status_code) { return(gc.status_code); }
// Pass 2: Parameters
counter = 0; counter = 0;
clear_vector(offset); clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // target = gc.position memcpy(target, gc.position, sizeof(target)); // target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &counter)) { while(next_statement(&letter, &value, line, &counter)) {
int_value = trunc(value); int_value = trunc(value);
unit_converted_value = to_millimeters(value); unit_converted_value = to_millimeters(value);
@ -313,7 +311,6 @@ uint8_t gc_execute_line(char *line) {
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly: // real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); } if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
@ -336,10 +333,9 @@ uint8_t gc_execute_line(char *line) {
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By // even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposide side of the line of // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long circles as prescribed. // travel and thus we get the unadvisably long circles as prescribed.
if (r < 0) { h_x2_div_d = -h_x2_div_d; } if (r < 0) { h_x2_div_d = -h_x2_div_d; }
// Complete the operation by calculating the actual center of the arc // Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2; offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2; offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
@ -375,12 +371,7 @@ uint8_t gc_execute_line(char *line) {
} }
// Find the radius // Find the radius
double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
// Prepare the arc // Trace 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(')');
mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate); mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);
break; break;
} }

View File

@ -34,13 +34,10 @@
#include <stdlib.h> #include <stdlib.h>
#include "nuts_bolts.h" #include "nuts_bolts.h"
#include "stepper.h" #include "stepper.h"
#include "serial_protocol.h"
#include "wiring_serial.h"
#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 #define ONE_MINUTE_OF_MICROSECONDS 60000000.0
int8_t mode; // The current operation mode volatile int8_t mode; // The current operation mode
int32_t position[3]; // The current position of the tool in absolute steps int32_t position[3]; // The current position of the tool in absolute steps
uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
@ -51,7 +48,7 @@ void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate,
void mc_init() void mc_init()
{ {
mode = 0; mode = MC_MODE_AT_REST;
clear_vector(position); clear_vector(position);
} }
@ -63,7 +60,7 @@ void mc_dwell(uint32_t milliseconds)
mode = MC_MODE_AT_REST; mode = MC_MODE_AT_REST;
} }
// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // Execute 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. // unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement.
void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate) void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate)
{ {
@ -76,12 +73,12 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
counter[3], // A counter used in the bresenham algorithm for line plotting counter[3], // A counter used in the bresenham algorithm for line plotting
maximum_steps; // The larges absolute step-count of any axis maximum_steps; // The larges absolute step-count of any axis
// Setup
target[X_AXIS] = x*X_STEPS_PER_MM; target[X_AXIS] = x*X_STEPS_PER_MM;
target[Y_AXIS] = y*Y_STEPS_PER_MM; target[Y_AXIS] = y*Y_STEPS_PER_MM;
target[Z_AXIS] = z*Z_STEPS_PER_MM; target[Z_AXIS] = z*Z_STEPS_PER_MM;
mode = MC_MODE_LINEAR;
// Determine direction and travel magnitude for each axis // Determine direction and travel magnitude for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) { for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
step_count[axis] = abs(target[axis] - position[axis]); step_count[axis] = abs(target[axis] - position[axis]);
@ -91,11 +88,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
maximum_steps = max(step_count[Z_AXIS], maximum_steps = max(step_count[Z_AXIS],
max(step_count[X_AXIS], step_count[Y_AXIS])); max(step_count[X_AXIS], step_count[Y_AXIS]));
// Nothing to do? // Nothing to do?
if (maximum_steps == 0) if (maximum_steps == 0) { return; }
{
mode = MC_MODE_AT_REST;
return;
}
// Set up a neat counter for each axis // Set up a neat counter for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) { for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
counter[axis] = -maximum_steps/2; counter[axis] = -maximum_steps/2;
@ -118,6 +111,8 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
// Execution // Execution
mode = MC_MODE_LINEAR;
while(mode) { while(mode) {
// Trace the line // Trace the line
step_bits = 0; step_bits = 0;
@ -142,7 +137,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
} }
// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc, // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
// circle in millimeters. axis_1 and axis_2 selects the plane in tool space. // circle in millimeters. axis_1 and axis_2 selects the plane in tool space.
// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis. // ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
@ -165,9 +160,10 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
int dx, dy; // Trace directions int dx, dy; // Trace directions
// Setup
uint32_t radius_steps = round(radius*X_STEPS_PER_MM); uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; } if(radius_steps == 0) { return; }
mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise) // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
angular_direction = signof(angular_travel); angular_direction = signof(angular_travel);
// Calculate the initial position and target position in the local coordinate system of the arc // Calculate the initial position and target position in the local coordinate system of the arc
@ -192,39 +188,37 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// And map the local coordinate system of the arc onto the tool axes of the selected plane // And map the local coordinate system of the arc onto the tool axes of the selected plane
axis_x = axis_1; axis_x = axis_1;
axis_y = axis_2; axis_y = axis_2;
// The amount of steppings performed while tracing a full circle is equal to the sum of sides in a // The amount of steppings performed while tracing a half circle is equal to the sum of sides in a
// square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a full circle: // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a half circle:
uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2))); uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2)));
// We then calculate the millimeters of travel along the circumference of that same full circle // We then calculate the millimeters of travel along the circumference of that same half circle
double millimeters_half_circumference = radius*M_PI; double millimeters_half_circumference = radius*M_PI;
// Then we calculate the microseconds between each step as if we will trace the full circle. // Then we calculate the microseconds between each step as if we will trace the full circle.
// It doesn't matter what fraction of the circle we are actuallyt going to trace. The pace is the same. // It doesn't matter what fraction of the circle we are actually going to trace. The pace is the same.
st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle); st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle);
incomplete = true;
// Execution // Execution
mode = MC_MODE_ARC;
incomplete = true;
while(incomplete) while(incomplete)
{ {
dx = (y!=0) ? signof(y) * angular_direction : -signof(x); dx = (y!=0) ? signof(y) * angular_direction : -signof(x);
dy = (x!=0) ? -signof(x) * angular_direction : -signof(y); dy = (x!=0) ? -signof(x) * angular_direction : -signof(y);
// Take dx and dy which are local to the arc being generated and map them on to the // Take dx and dy which are local to the arc being generated and map them on to the
// selected tool-space-axes for the current arc. // selected tool-space-axes for the current arc.
direction[axis_x] = dx; direction[axis_x] = dx;
direction[axis_y] = dy; direction[axis_y] = dy;
set_stepper_directions(direction); set_stepper_directions(direction);
// Check which axis will be "major" for this stepping
if (abs(x)<abs(y)) { if (abs(x)<abs(y)) {
// Step arc horizontally // Step arc horizontally
x+=dx;
error += 1+x2*dx; error += 1+x2*dx;
x2 += 2*dx; x+=dx; x2 += 2*dx;
diagonal_error = error + 1 + y2*dy; diagonal_error = error + 1 + y2*dy;
if(abs(error) >= abs(diagonal_error)) { if(abs(error) >= abs(diagonal_error)) {
y += dy; y += dy; y2 += 2*dy;
y2 += 2*dy;
error = diagonal_error; error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal step_steppers(diagonal_bits); // step diagonal
} else { } else {
@ -232,21 +226,18 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
} }
} else { } else {
// Step arc vertically // Step arc vertically
y+=dy;
error += 1+y2*dy; error += 1+y2*dy;
y2 += 2*dy; y+=dy; y2 += 2*dy;
diagonal_error = error + 1 + x2*dx; diagonal_error = error + 1 + x2*dx;
if(abs(error) >= abs(diagonal_error)) { if(abs(error) >= abs(diagonal_error)) {
x += dx; x += dx; x2 += 2*dx;
x2 += 2*dx;
error = diagonal_error; error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal step_steppers(diagonal_bits); // step diagonal
} else { } else {
step_axis(axis_y); // step straight step_axis(axis_y); // step straight
} }
} }
// Check if target has been reached. Todo: Simplify/optimize/clarify
// Check if target has been reached
if ((x * target_direction_y >= if ((x * target_direction_y >=
target_x * target_direction_y) && target_x * target_direction_y) &&
(y * target_direction_x <= (y * target_direction_x <=
@ -254,7 +245,6 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
{ if ((signof(x) == signof(target_x)) && (signof(y) == signof(target_y))) { if ((signof(x) == signof(target_x)) && (signof(y) == signof(target_y)))
{ incomplete = false; } } { incomplete = false; } }
} }
// Update the tool position to the new actual position // Update the tool position to the new actual position
position[axis_x] += x-start_x; position[axis_x] += x-start_x;
position[axis_y] += y-start_y; position[axis_y] += y-start_y;
@ -275,7 +265,7 @@ int mc_status()
return(mode); return(mode);
} }
// Set the direction pins for the stepper motors according to the provided vector. // Set the direction bits for the stepper motors according to the provided vector.
// direction is an array of three 8 bit integers representing the direction of // direction is an array of three 8 bit integers representing the direction of
// each motor. The values should be -1 (reverse), 0 or 1 (forward). // each motor. The values should be -1 (reverse), 0 or 1 (forward).
void set_stepper_directions(int8_t *direction) void set_stepper_directions(int8_t *direction)
@ -302,16 +292,5 @@ inline void step_steppers(uint8_t bits)
// Step only one motor // Step only one motor
inline void step_axis(uint8_t axis) inline void step_axis(uint8_t axis)
{ {
switch (axis) { st_buffer_step(direction_bits | st_bit_for_stepper(axis));
case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
} }
}
// Wait until all operations are completed
void mc_wait()
{
st_synchronize();
}

View File

@ -54,13 +54,8 @@ void mc_go_home();
// quiescence call mc_wait() // quiescence call mc_wait()
void mc_execute(); void mc_execute();
// Wait until all operations complete
void mc_wait();
// Check motion control status. result == 0: the system is idle. result > 0: the system is busy, // Check motion control status. result == 0: the system is idle. result > 0: the system is busy,
// result < 0: the system is in an error state. // result < 0: the system is in an error state.
int mc_status(); int mc_status();
void printCurrentPosition();
#endif #endif

View File

@ -32,6 +32,9 @@
#define TICKS_PER_MICROSECOND (F_CPU/1000000) #define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define STEP_BUFFER_SIZE 100 #define STEP_BUFFER_SIZE 100
// A marker used to notify the stepper handler of a pace change
#define PACE_CHANGE_MARKER 0xff
volatile uint8_t step_buffer[STEP_BUFFER_SIZE]; // A buffer for step instructions volatile uint8_t step_buffer[STEP_BUFFER_SIZE]; // A buffer for step instructions
volatile int step_buffer_head = 0; volatile int step_buffer_head = 0;
volatile int step_buffer_tail = 0; volatile int step_buffer_tail = 0;
@ -43,24 +46,25 @@ uint8_t echo_steps = true;
void config_pace_timer(uint32_t microseconds); void config_pace_timer(uint32_t microseconds);
// This timer interrupt is executed at the pace set with set_pace. It pops one instruction from // This timer interrupt is executed at the pace set with st_buffer_pace. It pops one instruction from
// the step_buffer, executes it. Then it starts timer2 in order to reset the motor port after // the step_buffer, executes it. Then it starts timer2 in order to reset the motor port after
// five microseconds. // five microseconds.
SIGNAL(SIG_OUTPUT_COMPARE1A) SIGNAL(SIG_OUTPUT_COMPARE1A)
{ {
if (step_buffer_head != step_buffer_tail) { if (step_buffer_head != step_buffer_tail) {
if(step_buffer[step_buffer_tail] == 0xff) { uint8_t popped = step_buffer[step_buffer_tail];
// If this is not a step-instruction, but a pace-marker: change pace if(popped == PACE_CHANGE_MARKER) {
// This is not a step-instruction, but a pace-change-marker: change pace
config_pace_timer(next_pace); config_pace_timer(next_pace);
next_pace = 0; next_pace = 0;
} else { } else {
// Set the direction pins a nanosecond or two before you step the steppers // 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); STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (popped & DIRECTION_MASK);
// Then pulse the stepping pins // Then pulse the stepping pins
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | step_buffer[step_buffer_tail]; STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | popped;
// Reset and start timer 2 which will reset the motor port after 5 microsecond // Reset and start timer 2 which will reset the motor port after 5 microsecond
TCNT2 = 0; // reset counter TCNT2 = 0; // reset counter
OCR2A = 5*TICKS_PER_MICROSECOND; // set the time OCR2A = 5*TICKS_PER_MICROSECOND; // set the trigger time
TIMSK2 |= (1<<OCIE2A); // enable interrupt TIMSK2 |= (1<<OCIE2A); // enable interrupt
} }
// move the step buffer tail to the next instruction // move the step buffer tail to the next instruction
@ -101,7 +105,7 @@ void st_init()
sei(); sei();
// start off with a slow pace // start off with a mellow pace
config_pace_timer(20000); config_pace_timer(20000);
st_start(); st_start();
} }
@ -163,12 +167,12 @@ void st_buffer_pace(uint32_t microseconds)
{ {
// Do nothing if the pace in unchanged // Do nothing if the pace in unchanged
if (current_pace == microseconds) { return; } if (current_pace == microseconds) { return; }
// If the one-element pace buffer is full, flush step buffer // If the single-element pace "buffer" is full, sleep until it is popped
if (next_pace != 0) { while (next_pace != 0) {
st_synchronize(); sleep_mode();
} }
next_pace = microseconds; next_pace = microseconds;
st_buffer_step(0xff); st_buffer_step(PACE_CHANGE_MARKER); // Place a pace-change marker in the step-buffer
} }
uint8_t st_bit_for_stepper(int axis) { uint8_t st_bit_for_stepper(int axis) {

View File

@ -1,13 +1,12 @@
* Support helical interpolation
* Optimize arc target detection code utilizing the primary axis of travel * Optimize arc target detection code utilizing the primary axis of travel
* Use bitmasks, not vectors to build steps in motion_control * Arcs might be a step or two off because of FP gotchas. Must add a little nudge in the end there?
* Arcs might be a step or two off because of FP gotchas. Must add a little nudge in the end there * Support inverse feed rate for arcs
* Generalize feed rate code and support inverse feed rate for arcs
* Implement homing cycle in stepper.c * Implement homing cycle in stepper.c
* Implement limit switch support in stepper.c (use port-triggered interrupts?) * Implement limit switch support in stepper.c (use port-triggered interrupts?)
* How on earth am I going to deal with arcs in setups that have different steps/mm on each axis? Must * 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 elipses?! Oh no.
* Support helical interpolation * Eliminate need for x and y in step_arc_along_
* Eliminate need for circle_x and circle_y in step_arc_along_
* Tool table * Tool table
* Tool length offsets * Tool length offsets
* Tool change M6 * Tool change M6