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 X_STEPS_PER_MM 5.0
#define Y_STEPS_PER_MM 5.0
#define Z_STEPS_PER_MM 5.0
#define X_STEPS_PER_MM 10.0
#define Y_STEPS_PER_MM 10.0
#define Z_STEPS_PER_MM 10.0
#define INCHES_PER_MM 25.4
#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 Z_STEPS_PER_INCH Z_STEPS_PER_MM*INCHES_PER_MM
#define RAPID_FEEDRATE 100.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 635.0
#define RAPID_FEEDRATE 200000.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 200000.0
#define STEPPERS_ENABLE_DDR DDRB
#define STEPPERS_ENABLE_PORT PORTB
@ -62,7 +62,7 @@
#define SPINDLE_DIRECTION_PORT PORTC
#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 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) {
case MOTION_MODE_CANCEL: break;
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],
inverse_feed_rate, true);
} 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 y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
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
// 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); }
/* 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.
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
|
|
C */
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
C <-- Current position */
// 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
offset[gc.plane_axis_0] = (x-(y*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(");
printInteger(trunc(theta_start/M_PI*180)); printByte(',');
printInteger(trunc(angular_travel/M_PI*180)); printByte(',');
printInteger(trunc(radius)); printByte('…');
printInteger(trunc(radius));
printByte(')');
mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);
break;

31
motion_control.c
View File

@ -38,7 +38,7 @@
#include "wiring_serial.h"
#define ONE_MINUTE_OF_MICROSECONDS 60000000
#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
// Parameters when mode is MC_MODE_ARC
struct LinearMotionParameters {
@ -87,16 +87,6 @@ inline void step_steppers(uint8_t *enabled);
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 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()
{
// 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
mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS],
max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS]));
if(mc.linear.maximum_steps == 0) { return; }
// Nothing to do?
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 {
// Ask old Phytagoras to estimate how many mm our next move is going to take us:
double millimeters_to_travel =
ceil(sqrt(pow((mc.linear.step_count[X_AXIS]),2) +
pow((mc.linear.step_count[Y_AXIS]),2) +
pow((mc.linear.step_count[Z_AXIS]),2)));
sqrt(pow(X_STEPS_PER_MM*mc.linear.step_count[X_AXIS],2) +
pow(Y_STEPS_PER_MM*mc.linear.step_count[Y_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
// designated amount of millimeters in the amount of steps we are going to generate
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));
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; }
mc.mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
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;
// 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:
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
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.
// It doesn't matter what fraction of the circle we are actuallyt going to trace. The pace is the same.
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;
}
@ -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.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
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;
while((c = serialRead()) != -1)
{
if(c == '\r') { // Line is complete. Then execute!
if((c < 32)) { // Line is complete. Then execute!
line[line_counter] = 0;
gc_execute_line(line);
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
* 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