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

pace calculation correct, arc algorithm correct, support for negative R

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-02-08 12:24:52 +01:00
parent d012440518
commit 6c3a6a25d5
9 changed files with 45 additions and 7211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
config.h
View File

@ -23,17 +23,17 @@
#define VERSION "0.0" #define VERSION "0.0"
#define X_STEPS_PER_MM 5.0 #define X_STEPS_PER_MM 10.0
#define Y_STEPS_PER_MM 5.0 #define Y_STEPS_PER_MM 10.0
#define Z_STEPS_PER_MM 5.0 #define Z_STEPS_PER_MM 10.0
#define INCHES_PER_MM 25.4 #define INCHES_PER_MM 25.4
#define X_STEPS_PER_INCH X_STEPS_PER_MM*INCHES_PER_MM #define X_STEPS_PER_INCH X_STEPS_PER_MM*INCHES_PER_MM
#define Y_STEPS_PER_INCH Y_STEPS_PER_MM*INCHES_PER_MM #define Y_STEPS_PER_INCH Y_STEPS_PER_MM*INCHES_PER_MM
#define Z_STEPS_PER_INCH Z_STEPS_PER_MM*INCHES_PER_MM #define Z_STEPS_PER_INCH Z_STEPS_PER_MM*INCHES_PER_MM
#define RAPID_FEEDRATE 100.0 // in millimeters per minute #define RAPID_FEEDRATE 200000.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 635.0 #define DEFAULT_FEEDRATE 200000.0
#define STEPPERS_ENABLE_DDR DDRB #define STEPPERS_ENABLE_DDR DDRB
#define STEPPERS_ENABLE_PORT PORTB #define STEPPERS_ENABLE_PORT PORTB
@ -62,7 +62,7 @@
#define SPINDLE_DIRECTION_PORT PORTC #define SPINDLE_DIRECTION_PORT PORTC
#define SPINDLE_DIRECTION_BIT 4 #define SPINDLE_DIRECTION_BIT 4
#define BAUD_RATE 19200 #define BAUD_RATE 9600
#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT) #define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
#define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT) #define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)

39
gcode.c
View File

@ -244,7 +244,7 @@ uint8_t gc_execute_line(char *line) {
switch (gc.motion_mode) { switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break; case MOTION_MODE_CANCEL: break;
case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR: case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR:
if (inverse_feed_rate) { if (gc.inverse_feed_rate_mode) {
mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
inverse_feed_rate, true); inverse_feed_rate, true);
} else { } else {
@ -309,37 +309,40 @@ uint8_t gc_execute_line(char *line) {
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(&offset); clear_vector(&offset);
double h_x2_div_d = sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == h * 2 / d double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// 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
// earthly 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); }
/* The anti-clockwise circle lies to the right of the target direction. When offset is positive, // 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; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated. the left hand circle will be generated - when it is negative the right hand circle is generated.
T T <-- Target position
^ ^
| Clockwise circles with this center | Clockwise circles with this center will have
| will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
| |
| |
C */ C <-- Current position */
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
// 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
// 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 // 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;
// printByte('(');
// printInteger(trunc(offset[gc.plane_axis_0])); printByte(',');
// printInteger(trunc(offset[gc.plane_axis_1]));
// printByte(')');
} }
/* /*
@ -376,7 +379,7 @@ uint8_t gc_execute_line(char *line) {
printString("mc_arc("); printString("mc_arc(");
printInteger(trunc(theta_start/M_PI*180)); printByte(','); printInteger(trunc(theta_start/M_PI*180)); printByte(',');
printInteger(trunc(angular_travel/M_PI*180)); printByte(','); printInteger(trunc(angular_travel/M_PI*180)); printByte(',');
printInteger(trunc(radius)); printByte('…'); printInteger(trunc(radius));
printByte(')'); 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;

31
motion_control.c
View File

@ -38,7 +38,7 @@
#include "wiring_serial.h" #include "wiring_serial.h"
#define ONE_MINUTE_OF_MICROSECONDS 60000000 #define ONE_MINUTE_OF_MICROSECONDS 60000000.0
// Parameters when mode is MC_MODE_ARC // Parameters when mode is MC_MODE_ARC
struct LinearMotionParameters { struct LinearMotionParameters {
@ -87,16 +87,6 @@ inline void step_steppers(uint8_t *enabled);
inline void step_axis(uint8_t axis); inline void step_axis(uint8_t axis);
void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate); void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
// void printCurrentPosition() {
// int axis;
// printString("[ ");
// for(axis=X_AXIS; axis<=Z_AXIS; axis++) {
// printInteger(trunc(mc.position[axis]*100));
// printByte(' ');
// }
// printString("]\n\r");
// }
//
void mc_init() void mc_init()
{ {
// Initialize state variables // Initialize state variables
@ -135,6 +125,7 @@ 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 // Find the magnitude of the axis with the longest travel
mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS], mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS],
max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS])); max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS]));
if(mc.linear.maximum_steps == 0) { return; }
// Nothing to do? // Nothing to do?
if ((mc.linear.maximum_steps) == 0) if ((mc.linear.maximum_steps) == 0)
{ {
@ -154,13 +145,13 @@ void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate,
} else { } else {
// Ask old Phytagoras to estimate how many mm our next move is going to take us: // Ask old Phytagoras to estimate how many mm our next move is going to take us:
double millimeters_to_travel = double millimeters_to_travel =
ceil(sqrt(pow((mc.linear.step_count[X_AXIS]),2) + sqrt(pow(X_STEPS_PER_MM*mc.linear.step_count[X_AXIS],2) +
pow((mc.linear.step_count[Y_AXIS]),2) + pow(Y_STEPS_PER_MM*mc.linear.step_count[Y_AXIS],2) +
pow((mc.linear.step_count[Z_AXIS]),2))); pow(Z_STEPS_PER_MM*mc.linear.step_count[Z_AXIS],2));
// Calculate the microseconds between steps that we should wait in order to travel the // 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 // designated amount of millimeters in the amount of steps we are going to generate
mc.pace = mc.pace =
round(((millimeters_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / mc.linear.maximum_steps); ((millimeters_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / mc.linear.maximum_steps;
} }
} }
@ -201,6 +192,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
{ {
memset(&mc.arc, 0, sizeof(mc.arc)); memset(&mc.arc, 0, sizeof(mc.arc));
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; }
mc.mode = MC_MODE_ARC; mc.mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise) // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
mc.arc.angular_direction = signof(angular_travel); mc.arc.angular_direction = signof(angular_travel);
@ -229,13 +221,13 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
mc.arc.axis_y = axis_2; mc.arc.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 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: // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a full circle:
uint32_t steps_in_full_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 full circle
double millimeters_circumference = 2*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 actuallyt going to trace. The pace is the same.
mc.pace = mc.pace =
round(((millimeters_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_full_circle); ((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle;
mc.arc.incomplete = true; mc.arc.incomplete = true;
} }
@ -244,7 +236,8 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
mc.arc.target_x * mc.arc.target_direction_y) && \ mc.arc.target_x * mc.arc.target_direction_y) && \
(mc.arc.y * mc.arc.target_direction_x <= \ (mc.arc.y * mc.arc.target_direction_x <= \
mc.arc.target_y * mc.arc.target_direction_x)) \ mc.arc.target_y * mc.arc.target_direction_x)) \
{ mc.arc.incomplete = false; } { if ((signof(mc.arc.x) == signof(mc.arc.target_x)) && (signof(mc.arc.y) == signof(mc.arc.target_y))) \
{ mc.arc.incomplete = false; } }
// Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy // Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
void step_arc_along_x(int8_t dx, int8_t dy) void step_arc_along_x(int8_t dx, int8_t dy)

2
scripts/console Executable file
View File

@ -0,0 +1,2 @@
socat -d -d READLINE /dev/tty.usbserial-A4001o6L,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1

2
scripts/proxy Executable file
View File

@ -0,0 +1,2 @@
socat -d -d tcp4-listen:5001,fork /dev/tty.usbserial-A4001o6L,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1

2
serial_protocol.c
View File

@ -83,7 +83,7 @@ void sp_process()
char c; char c;
while((c = serialRead()) != -1) while((c = serialRead()) != -1)
{ {
if(c == '\r') { // Line is complete. Then execute! if((c < 32)) { // Line is complete. Then execute!
line[line_counter] = 0; line[line_counter] = 0;
gc_execute_line(line); gc_execute_line(line);
line_counter = 0; line_counter = 0;

7167
testbench/echotest.rb
View File

File diff suppressed because it is too large Load Diff

BIN
testbench/gcodetest.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.6 KiB

1
todo.txt
View File

@ -1,3 +1,4 @@
* Optimize arc target detection code utilizing the primary axis of travel
* Use bitmasks, not vectors to build steps in motion_control * 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
* Generalize feed rate code and support inverse feed rate for arcs * Generalize feed rate code and support inverse feed rate for arcs