ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

optimized for size and did some housekeeping

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gcode.c
View File

@ -54,8 +54,6 @@
#include "errno.h"
#include "serial_protocol.h"
#include "wiring_serial.h"
#define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1
#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 (gc.status_code) { return(gc.status_code); }
// Pass 2: Parameters
counter = 0;
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &counter)) {
int_value = trunc(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
// 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); }
// 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; }
@ -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!),
// 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.
if (r < 0) { h_x2_div_d = -h_x2_div_d; }
// 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_1] = (y+(x*h_x2_div_d))/2;
@ -375,12 +371,7 @@ 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(')');
// Trace the arc
mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);
break;
}

77
motion_control.c
View File

@ -34,13 +34,10 @@
#include <stdlib.h>
#include "nuts_bolts.h"
#include "stepper.h"
#include "serial_protocol.h"
#include "wiring_serial.h"
#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
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()
{
mode = 0;
mode = MC_MODE_AT_REST;
clear_vector(position);
}
@ -63,7 +60,7 @@ void mc_dwell(uint32_t milliseconds)
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.
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
maximum_steps; // The larges absolute step-count of any axis
// Setup
target[X_AXIS] = x*X_STEPS_PER_MM;
target[Y_AXIS] = y*Y_STEPS_PER_MM;
target[Z_AXIS] = z*Z_STEPS_PER_MM;
mode = MC_MODE_LINEAR;
// Determine direction and travel magnitude for each axis
for(axis = X_AXIS; axis <= Z_AXIS; 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],
max(step_count[X_AXIS], step_count[Y_AXIS]));
// Nothing to do?
if (maximum_steps == 0)
{
mode = MC_MODE_AT_REST;
return;
}
if (maximum_steps == 0) { return; }
// Set up a neat counter for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
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
mode = MC_MODE_LINEAR;
while(mode) {
// Trace the line
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
// 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.
@ -160,14 +155,15 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
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
int8_t diagonal_bits; // A bitmask with the stepper bits for both selected axes set
int8_t diagonal_bits; // A bitmask with the stepper bits for both selected axes set
int incomplete; // True if the arc has not reached its target yet
int dx, dy; // Trace directions
// Setup
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; }
mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
angular_direction = signof(angular_travel);
// 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
axis_x = axis_1;
axis_y = axis_2;
// The amount of steppings performed while tracing a full 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:
// 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 half circle:
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;
// 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);
incomplete = true;
// Execution
mode = MC_MODE_ARC;
incomplete = true;
while(incomplete)
{
dx = (y!=0) ? signof(y) * angular_direction : -signof(x);
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
// selected tool-space-axes for the current arc.
direction[axis_x] = dx;
direction[axis_y] = dy;
set_stepper_directions(direction);
// Check which axis will be "major" for this stepping
if (abs(x)<abs(y)) {
// Step arc horizontally
x+=dx;
error += 1+x2*dx;
x2 += 2*dx;
x+=dx; x2 += 2*dx;
diagonal_error = error + 1 + y2*dy;
if(abs(error) >= abs(diagonal_error)) {
y += dy;
y2 += 2*dy;
y += dy; y2 += 2*dy;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else {
@ -232,21 +226,18 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
}
} else {
// Step arc vertically
y+=dy;
error += 1+y2*dy;
y2 += 2*dy;
y+=dy; y2 += 2*dy;
diagonal_error = error + 1 + x2*dx;
if(abs(error) >= abs(diagonal_error)) {
x += dx;
x2 += 2*dx;
x += dx; x2 += 2*dx;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else {
step_axis(axis_y); // step straight
}
}
// Check if target has been reached
// Check if target has been reached. Todo: Simplify/optimize/clarify
if ((x * target_direction_y >=
target_x * target_direction_y) &&
(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)))
{ incomplete = false; } }
}
// Update the tool position to the new actual position
position[axis_x] += x-start_x;
position[axis_y] += y-start_y;
@ -275,7 +265,7 @@ int mc_status()
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
// each motor. The values should be -1 (reverse), 0 or 1 (forward).
void set_stepper_directions(int8_t *direction)
@ -302,16 +292,5 @@ inline void step_steppers(uint8_t bits)
// Step only one motor
inline void step_axis(uint8_t axis)
{
switch (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;
}
st_buffer_step(direction_bits | st_bit_for_stepper(axis));
}
// Wait until all operations are completed
void mc_wait()
{
st_synchronize();
}

5
motion_control.h
View File

@ -54,13 +54,8 @@ void mc_go_home();
// quiescence call mc_wait()
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,
// result < 0: the system is in an error state.
int mc_status();
void printCurrentPosition();
#endif

30
stepper.c
View File

@ -32,6 +32,9 @@
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
#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 int step_buffer_head = 0;
volatile int step_buffer_tail = 0;
@ -43,25 +46,26 @@ 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
// 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
// five microseconds.
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
if (step_buffer_head != step_buffer_tail) {
if(step_buffer[step_buffer_tail] == 0xff) {
// If this is not a step-instruction, but a pace-marker: change pace
uint8_t popped = step_buffer[step_buffer_tail];
if(popped == PACE_CHANGE_MARKER) {
// This is not a step-instruction, but a pace-change-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);
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (popped & DIRECTION_MASK);
// 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
TCNT2 = 0; // reset counter
OCR2A = 5*TICKS_PER_MICROSECOND; // set the time
TIMSK2 |= (1<<OCIE2A); // enable interrupt
TCNT2 = 0; // reset counter
OCR2A = 5*TICKS_PER_MICROSECOND; // set the trigger 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;
@ -101,7 +105,7 @@ void st_init()
sei();
// start off with a slow pace
// start off with a mellow pace
config_pace_timer(20000);
st_start();
}
@ -163,12 +167,12 @@ 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();
// If the single-element pace "buffer" is full, sleep until it is popped
while (next_pace != 0) {
sleep_mode();
}
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) {

9
todo.txt
View File

@ -1,13 +1,12 @@
* Support helical interpolation
* 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
* Generalize feed rate code and support inverse feed rate for arcs
* 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
* Implement homing cycle in stepper.c
* 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
support elipses?! Oh no.
* Support helical interpolation
* Eliminate need for circle_x and circle_y in step_arc_along_
* Eliminate need for x and y in step_arc_along_
* Tool table
* Tool length offsets
* Tool change M6