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

lots and lots of bugfixes after running on reals hardware for the first time

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-02-03 09:56:45 +01:00
parent 9799955555
commit 50a9f78088
19 changed files with 827 additions and 368 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Makefile
View File

@ -28,7 +28,7 @@
# FUSES ........ Parameters for avrdude to flash the fuses appropriately.
DEVICE = atmega168
CLOCK = 20000000
CLOCK = 16000000
PROGRAMMER = -c avrisp2 -P usb
OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o geometry.o
FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
@ -77,7 +77,7 @@ main.elf: $(OBJECTS)
grbl.hex: main.elf
rm -f grbl.hex
avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex
avr-size grbl.hex *.o
avr-size *.hex *.elf *.o
# If you have an EEPROM section, you must also create a hex file for the
# EEPROM and add it to the "flash" target.

3
arc_algorithm/arc.rb
View File

@ -238,7 +238,8 @@ class CircleTest
end
dx = y.sign*angular_direction unless y == 0
end
break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction &&
y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
end
plot_pixel(tx+20, -ty+20, "o")
return {:tx => tx, :ty => ty, :x => x, :y => y}

8
arc_algorithm/theta.rb
View File

@ -11,6 +11,8 @@ def calc_theta(x,y)
end
end
(-180..180).each do |deg|
pp [deg, calc_theta(sin(1.0*deg/180*PI), cos(1.0*deg/180*PI))/PI*180]
end
pp calc_theta(5,0)/PI*180;
# (-180..180).each do |deg|
# pp [deg, calc_theta(sin(1.0*deg/180*PI), cos(1.0*deg/180*PI))/PI*180]
# end

33
config.h
View File

@ -23,40 +23,36 @@
#define VERSION "0.1"
#define X_STEPS_PER_MM 100.0
#define Y_STEPS_PER_MM 100.0
#define Z_STEPS_PER_MM 100.0
#define X_STEPS_PER_MM 5.0
#define Y_STEPS_PER_MM 5.0
#define Z_STEPS_PER_MM 5.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 1270.0 // in millimeters per minute
#define RAPID_FEEDRATE 100.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 635.0
#define STEPPERS_ENABLE_DDR DDRB
#define STEPPERS_ENABLE_PORT PORTB
#define STEPPERS_ENABLE_BIT 6
#define MOTORS_DDR DDRB
#define MOTORS_PORT PORTB
#define STEPPING_DDR DDRB
#define STEPPING_PORT PORTB
#define X_STEP_BIT 0
#define Y_STEP_BIT 2
#define Z_STEP_BIT 4
#define X_DIRECTION_BIT 1
#define Y_DIRECTION_BIT 3
#define Y_STEP_BIT 1
#define Z_STEP_BIT 2
#define X_DIRECTION_BIT 3
#define Y_DIRECTION_BIT 4
#define Z_DIRECTION_BIT 5
#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 MOTORS_MASK STEP_MASK | DIRECTION_MASK
#define LIMIT_DDR DDRC
#define LIMIT_PORT PORTC
#define X_LIMIT_BIT 0
#define Y_LIMIT_BIT 1
#define Z_LIMIT_BIT 2
#define LIMIT_MASK (1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)
#define SPINDLE_ENABLE_DDR DDRC
#define SPINDLE_ENABLE_PORT PORTC
@ -66,9 +62,14 @@
#define SPINDLE_DIRECTION_PORT PORTC
#define SPINDLE_DIRECTION_BIT 4
#define BAUD_RATE 14400
#define BAUD_RATE 19200
// Unrolling the arc code is faster, but costs about 830 bytes of extra code space.
// Unrolling the arc code is faster, but consumes about 830 extra bytes of code space.
// #define UNROLLED_ARC_LOOP
#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 STEPPING_MASK STEP_MASK | DIRECTION_MASK
#define LIMIT_MASK (1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)
#endif

226
gcode.c
View File

@ -51,6 +51,9 @@
#include "motion_control.h"
#include "spindle_control.h"
#include "geometry.h"
#include "errno.h"
#include "wiring_serial.h"
#define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1
@ -82,6 +85,7 @@ struct ParserState {
uint8_t motion_mode:3; /* {G0, G1, G2, G3, G38.2, G80, G81, G82, G83, G84, G85, G86, G87, G88, G89} */
uint8_t inverse_feed_rate_mode:1; /* G93, G94 */
uint8_t inches_mode:1; /* 0 = millimeter mode, 1 = inches mode {G20, G21} */
uint8_t absolute_mode:1; /* 0 = relative motion, 1 = absolute motion {G90, G91} */
uint8_t program_flow:2;
int spindle_direction:2;
double feed_rate; /* Millimeters/second */
@ -91,9 +95,9 @@ struct ParserState {
uint8_t plane_axis_0, plane_axis_1; // The axes of the selected plane
};
struct ParserState state;
struct ParserState gc;
#define FAIL(status) state.status_code = status;
#define FAIL(status) gc.status_code = status;
int read_double(char *line, //!< string: line of RS274/NGC code being processed
int *counter, //!< pointer to a counter for position on the line
@ -103,44 +107,48 @@ int next_statement(char *letter, double *double_ptr, char *line, int *counter);
void gc_init() {
memset(&state, 0, sizeof(state));
state.feed_rate = DEFAULT_FEEDRATE;
memset(&gc, 0, sizeof(gc));
gc.feed_rate = DEFAULT_FEEDRATE;
gc.plane_axis_0 = X_AXIS; gc.plane_axis_1 = Y_AXIS;
}
inline float to_millimeters(double value) {
return(state.inches_mode ? value * INCHES_PER_MM : value);
return(gc.inches_mode ? value * INCHES_PER_MM : value);
}
void select_plane(uint8_t axis_0, uint8_t axis_1)
{
state.plane_axis_0 = axis_0;
state.plane_axis_1 = axis_1;
gc.plane_axis_0 = axis_0;
gc.plane_axis_1 = axis_1;
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floats (no whitespace).
uint8_t gc_execute_line(char *line) {
int counter;
int counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate;
int radius_mode;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
int radius_mode = false;
uint8_t absolute_mode; /* 0 = relative motion, 1 = absolute motion {G90, G91} */
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* One of the NEXT_ACTION_-constants */
double target[3], offset[3];
double target[3], offset[3];
double p, r;
int int_value, axis;
double p = 0, r = 0;
int int_value;
state.line_number++;
state.status_code = GCSTATUS_OK;
clear_vector(target);
clear_vector(offset);
gc.line_number++;
gc.status_code = GCSTATUS_OK;
/* First: parse all statements */
if (line[0] == '(') { return(state.status_code); }
if (line[0] == '(') { return(gc.status_code); }
if (line[0] == '/') { counter++; } // ignore block delete
// Pass 1: Commands
@ -149,75 +157,81 @@ uint8_t gc_execute_line(char *line) {
switch(letter) {
case 'G':
switch(int_value) {
case 0: state.motion_mode = MOTION_MODE_RAPID_LINEAR; break;
case 1: state.motion_mode = MOTION_MODE_LINEAR; break;
case 2: state.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: state.motion_mode = MOTION_MODE_CCW_ARC; break;
case 0: gc.motion_mode = MOTION_MODE_RAPID_LINEAR; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS); break;
case 20: state.inches_mode = true; break;
case 21: state.inches_mode = false; break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_mode = 1; break;
case 80: state.motion_mode = MOTION_MODE_CANCEL; break;
case 93: state.inverse_feed_rate_mode = true; break;
case 94: state.inverse_feed_rate_mode = false; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: state.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: state.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: state.spindle_direction = 1; break;
case 4: state.spindle_direction = -1; break;
case 5: state.spindle_direction = 0; break;
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': state.tool = trunc(value); break;
case 'T': gc.tool = trunc(value); break;
}
if(state.status_code) { break; }
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (state.status_code) { return(state.status_code); }
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
while(next_statement(&letter, &value, line, &counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (state.inverse_feed_rate_mode) {
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
state.feed_rate = unit_converted_value; // millimeters pr second
gc.feed_rate = unit_converted_value; // millimeters pr second
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': state.spindle_speed = value; break;
case 'S': gc.spindle_speed = value; break;
case 'X': case 'Y': case 'Z':
axis = letter - 'X';
if (absolute_mode) {
target[axis] = unit_converted_value;
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[axis] = state.position[axis]+unit_converted_value;
};
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// 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); }
// Update spindle state
if (state.spindle_direction) {
spindle_run(state.spindle_direction, state.spindle_speed);
if (gc.spindle_direction) {
spindle_run(gc.spindle_direction, gc.spindle_speed);
} else {
spindle_stop();
}
@ -227,7 +241,7 @@ uint8_t gc_execute_line(char *line) {
case NEXT_ACTION_GO_HOME: mc_go_home(); break;
case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break;
case NEXT_ACTION_DEFAULT:
switch (state.motion_mode) {
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR:
if (inverse_feed_rate) {
@ -235,7 +249,7 @@ uint8_t gc_execute_line(char *line) {
inverse_feed_rate, true);
} else {
mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(state.motion_mode == MOTION_MODE_LINEAR) ? state.feed_rate : RAPID_FEEDRATE,
(gc.motion_mode == MOTION_MODE_LINEAR) ? gc.feed_rate : RAPID_FEEDRATE,
false);
}
break;
@ -244,15 +258,14 @@ uint8_t gc_execute_line(char *line) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == distance of
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. This is the distance from the center of the travel vector
to the center of our circle. A perpendicular to the travel vector is scaled to the length of
h and added to the center of the travel vector to form the new point [i,j] which will be
the center of our circle.
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 + (d/2)^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
@ -273,35 +286,60 @@ uint8_t gc_execute_line(char *line) {
h - distance from center of CT to O
Expanding the equations:
h = sqrt(4 r^2 + x^2 + y^2)/2
d = sqrt(x^2 + y^2)
i = x/2 - (h * y)/d
j = y/2 + (h * x)/d
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i = (x - (y * sqrt(4 * r^2 + x^2 + y^2))/sqrt(x^2 + y^2))/2
j = (y + (x * sqrt(4 * r^2 + x^2 + y^2))/sqrt(x^2 + y^2))/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which can be optimized to:
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 + x^2 + y^2)/sqrt(x^2 + y^2)
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[state.plane_axis_0]-state.position[state.plane_axis_0];
double y = target[state.plane_axis_1]-state.position[state.plane_axis_1];
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
// The anti-clockwise circle lies to the right 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.
if (state.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
offset[state.plane_axis_0] = (x-(y*h_x2_div_d))/2;
offset[state.plane_axis_1] = (y+(x*h_x2_div_d))/2;
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:
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,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T
^
|
|
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; }
// 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(')');
}
/*
@ -313,28 +351,34 @@ uint8_t gc_execute_line(char *line) {
* * *
* *
* *
* O ----T <- theta == theta_end (e.g. 90 degrees: theta == PI/2)
* O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* /
C <- theta == theta_start (e.g. -145 degrees: theta == -PI*(3/4))
C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*/
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[state.plane_axis_0], -offset[state.plane_axis_1]);
double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]);
// calculate the theta (angle) of the target point
double theta_end = theta(target[state.plane_axis_0] - offset[state.plane_axis_0] - state.position[state.plane_axis_0],
target[state.plane_axis_1] - offset[state.plane_axis_1] - state.position[state.plane_axis_1]);
double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0],
target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]);
// double theta_end = theta(5,0);
// ensure that the difference is positive so that we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
double angular_travel = theta_end-theta_start;
// Invert angular motion if the g-code wanted a counterclockwise arc
if (state.motion_mode == MOTION_MODE_CCW_ARC) {
if (gc.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel = angular_travel-2*M_PI;
}
// Find the radius
double radius = hypot(offset[state.plane_axis_0], offset[state.plane_axis_1]);
double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
// Prepare the arc
mc_arc(theta_start, angular_travel, radius, state.plane_axis_0, state.plane_axis_1, state.feed_rate);
printString("mc_arc(");
printInteger(trunc(theta_start/M_PI*180)); printByte(',');
printInteger(trunc(angular_travel/M_PI*180)); printByte(',');
printInteger(trunc(radius)); printByte('…');
printByte(')');
mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);
break;
}
}
@ -344,40 +388,39 @@ uint8_t gc_execute_line(char *line) {
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(state.position, target, sizeof(state.position));
return(state.status_code);
memcpy(gc.position, target, sizeof(double)*3);
return(gc.status_code);
}
void gc_get_status(double *position, uint8_t *status_code, int *inches_mode, uint32_t *line_number)
{
int axis;
if (state.inches_mode) {
if (gc.inches_mode) {
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
position[axis] = state.position[axis]*INCHES_PER_MM;
position[axis] = gc.position[axis]*INCHES_PER_MM;
}
} else {
memcpy(position, state.position, sizeof(position));
memcpy(position, gc.position, sizeof(gc.position));
}
*status_code = state.status_code;
*inches_mode = state.inches_mode;
*line_number = state.line_number;
*status_code = gc.status_code;
*inches_mode = gc.inches_mode;
*line_number = gc.line_number;
}
// Parses the next statement and leaves the counter on the first character following
// the statement. Returns 1 if there was a statements, 0 if end of string was reached
// or there was an error (check state.status_code).
int next_statement(char *letter, double *double_ptr, char *line, int *counter) {
if (*line == 0) {
if (line[*counter] == 0) {
return(0); // No more statements
}
*letter = *line;
*letter = line[*counter];
if((*letter < 'A') || (*letter > 'Z')) {
FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER);
return(0);
}
*counter++;
(*counter)++;
if (!read_double(line, counter, double_ptr)) {
return(0);
};
@ -400,4 +443,3 @@ int read_double(char *line, //!< string: line of RS274/NGC code being processed
*counter = end - line;
return(1);
}

1
gcode.h
View File

@ -28,6 +28,7 @@
#define GCSTATUS_EXPECTED_COMMAND_LETTER 2
#define GCSTATUS_UNSUPPORTED_STATEMENT 3
#define GCSTATUS_MOTION_CONTROL_ERROR 4
#define GCSTATUS_FLOATING_POINT_ERROR 5
// Initialize the parser
void gc_init();

6
geometry.c
View File

@ -20,7 +20,8 @@
#include <math.h>
// Find the angle from the positive y axis to the given point with respect to origo.
// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis,
// positive to the right.
double theta(double x, double y)
{
double theta = atan(x/fabs(y));
@ -29,10 +30,9 @@ double theta(double x, double y)
} else {
if (theta>0)
{
return(theta-M_PI);
return(M_PI-theta);
} else {
return(-M_PI-theta);
}
}
}

2
geometry.h
View File

@ -24,4 +24,4 @@
// Find the angle from the positive y axis to the given point with respect to origo.
double theta(double x, double y);
#endif
#endif

220
legacy/unrolled_arc.c Normal file
View File

@ -0,0 +1,220 @@
// Prepare 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.
void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate)
{
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
mc.mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
mc.arc.angular_direction = signof(angular_travel);
// Calculate the initial position and target position in the local coordinate system of the arc
mc.arc.x = round(sin(theta)*radius_steps);
mc.arc.y = round(cos(theta)*radius_steps);
mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps);
mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps);
// Precalculate these values to optimize target detection
mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction;
mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.angular_direction;
// The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error
// <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we
// are exactly on top of the arc.
mc.arc.error = mc.arc.x*mc.arc.x + mc.arc.y*mc.arc.y - radius_steps*radius_steps;
// Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
// by keeping track of the doubles of the arc coordinates at all times.
mc.arc.x2 = 2*mc.arc.x;
mc.arc.y2 = 2*mc.arc.y;
// Set up a vector with the steppers we are going to use tracing the plane of this arc
clear_vector(mc.arc.plane_steppers);
mc.arc.plane_steppers[axis_1] = 1;
mc.arc.plane_steppers[axis_2] = 1;
// And map the local coordinate system of the arc onto the tool axes of the selected plane
mc.arc.axis_x = axis_1;
mc.arc.axis_y = axis_2;
// mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
mc.pace =
ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
mc.arc.incomplete = true;
}
#define check_arc_target \
if ((mc.arc.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.target_y * mc.arc.target_direction_x)) \
{ 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)
{
uint32_t diagonal_error;
mc.arc.x+=dx;
mc.arc.error += 1+mc.arc.x2*dx;
mc.arc.x2 += 2*dx;
diagonal_error = mc.arc.error + 1 + mc.arc.y2*dy;
if(abs(mc.arc.error) >= abs(diagonal_error)) {
mc.arc.y += dy;
mc.arc.y2 += 2*dy;
mc.arc.error = diagonal_error;
step_steppers(mc.arc.plane_steppers); // step diagonal
} else {
step_axis(mc.arc.axis_x); // step straight
}
check_arc_target;
}
// Internal method used by execute_arc to trace vertically in the general direction provided by dx and dy
void step_arc_along_y(int8_t dx, int8_t dy)
{
uint32_t diagonal_error;
mc.arc.y+=dy;
mc.arc.error += 1+mc.arc.y2*dy;
mc.arc.y2 += 2*dy;
diagonal_error = mc.arc.error + 1 + mc.arc.x2*dx;
if(abs(mc.arc.error) >= abs(diagonal_error)) {
mc.arc.x += dx;
mc.arc.x2 += 2*dx;
mc.arc.error = diagonal_error;
step_steppers(mc.arc.plane_steppers); // step diagonal
} else {
step_axis(mc.arc.axis_y); // step straight
}
check_arc_target;
}
// 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.
void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
{
int8_t direction[3];
direction[mc.arc.axis_x] = dx;
direction[mc.arc.axis_y] = dy;
set_stepper_directions(direction);
}
/*
Quandrants of the arc
\ 7|0 /
\ | /
6 \|/ 1 y+
---------|-----------
5 /|\ 2 y-
/ | \
x- / 4|3 \ x+ */
#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
// Determine within which quadrant of the circle the provided coordinate falls
int quadrant(uint32_t x,uint32_t y)
{
// determine if the coordinate is in the quadrants 0,3,4 or 7
register int quad0347 = abs(x)<abs(y);
if (x<0) { // quad 4567
if (y<0) { // quad 45
return(quad0347 ? 4 : 5);
} else { // quad 67
return(quad0347 ? 7 : 6);
}
} else {
if (y<0) { // quad 23
return(quad0347 ? 3 : 2);
} else { // quad 01
return(quad0347 ? 0 : 1);
}
}
}
#endif
// Will trace the configured arc until the target is reached.
void execute_arc()
{
uint32_t start_x = mc.arc.x;
uint32_t start_y = mc.arc.y;
int dx, dy; // Trace directions
int steps = 0;
// mc.mode is set to 0 (MC_MODE_AT_REST) when target is reached
while(mc.arc.incomplete && (steps<400))
{
steps++;
#ifdef UNROLLED_ARC_LOOP
// Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
int q = quadrant(mc.arc.x, mc.arc.y);
if (mc.arc.angular_direction) {
switch (q) {
case 0:
map_local_arc_directions_to_stepper_directions(1,-1);
while(mc.arc.incomplete && (mc.arc.x>mc.arc.y)) { step_arc_along_x(1,-1); }
case 1:
map_local_arc_directions_to_stepper_directions(1,-1);
while(mc.arc.incomplete && (mc.arc.y>0)) { step_arc_along_y(1,-1); }
case 2:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(mc.arc.incomplete && (mc.arc.y>-mc.arc.x)) { step_arc_along_y(-1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(mc.arc.incomplete && (mc.arc.x>0)) { step_arc_along_x(-1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(-1,1);
while(mc.arc.incomplete && (mc.arc.y<mc.arc.x)) { step_arc_along_x(-1,1); }
case 5:
map_local_arc_directions_to_stepper_directions(-1,1);
while(mc.arc.incomplete && (mc.arc.y<0)) { step_arc_along_y(-1,1); }
case 6:
map_local_arc_directions_to_stepper_directions(1,-1);
while(mc.arc.incomplete && (mc.arc.y<-mc.arc.x)) { step_arc_along_y(1,1); }
case 7:
map_local_arc_directions_to_stepper_directions(1,1);
while(mc.arc.incomplete && (mc.arc.x<0)) { step_arc_along_x(1,1); }
}
} else {
switch (q) {
case 7:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(mc.arc.incomplete && (mc.arc.y>-mc.arc.x)) { step_arc_along_x(-1,-1); }
case 6:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(mc.arc.incomplete && (mc.arc.y>0)) { step_arc_along_y(-1,-1); }
case 5:
map_local_arc_directions_to_stepper_directions(1,-1);
while(mc.arc.incomplete && (mc.arc.y>mc.arc.x)) { step_arc_along_y(1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(1,-1);
while(mc.arc.incomplete && (mc.arc.x<0)) { step_arc_along_x(1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(1,1);
while(mc.arc.incomplete && (mc.arc.y<-mc.arc.x)) { step_arc_along_x(1,1); }
case 2:
map_local_arc_directions_to_stepper_directions(1,1);
while(mc.arc.incomplete && (mc.arc.y<0)) { step_arc_along_y(1,1); }
case 1:
map_local_arc_directions_to_stepper_directions(-1,1);
while(mc.arc.incomplete && (mc.arc.y<mc.arc.x)) { step_arc_along_y(-1,1); }
case 0:
map_local_arc_directions_to_stepper_directions(-1,1);
while(mc.arc.incomplete && (mc.arc.x>0)) { step_arc_along_x(-1,1); }
}
}
#else
dx = (mc.arc.y!=0) ? signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x);
dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.y);
map_local_arc_directions_to_stepper_directions(dx,dy);
if (abs(mc.arc.x)<abs(mc.arc.y)) {
step_arc_along_x(dx,dy);
} else {
step_arc_along_y(dx,dy);
}
#endif
}
// Update the tool position to the new actual position
mc.position[mc.arc.axis_x] += mc.arc.x-start_x;
mc.position[mc.arc.axis_y] += mc.arc.y-start_y;
// Because of rounding errors we might be off by a step or two. Adjust for this
// To be implemented
//void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
mc.mode = MC_MODE_AT_REST;
}

7
main.c
View File

@ -20,14 +20,19 @@
#include <avr/io.h>
#include <avr/sleep.h>
#include <util/delay.h>
#include "stepper.h"
#include "spindle_control.h"
#include "motion_control.h"
#include "gcode.h"
#include "serial_protocol.h"
#include "config.h"
#include "wiring_serial.h"
int main(void)
{
beginSerial(BAUD_RATE);
st_init();
mc_init(); // initialize motion control subsystem
spindle_init(); // initialize spindle controller
@ -35,7 +40,7 @@ int main(void)
sp_init(); // initialize the serial protocol
for(;;){
sleep_mode();
// sleep_mode();
sp_process(); // process the serial protocol
}
return 0; /* never reached */

369
motion_control.c
View File

@ -35,6 +35,8 @@
#include "nuts_bolts.h"
#include "stepper.h"
#include "wiring_serial.h"
#define ONE_MINUTE_OF_MICROSECONDS 60000000
// Parameters when mode is MC_MODE_ARC
@ -49,10 +51,10 @@ struct LinearMotionParameters {
struct ArcMotionParameters {
int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
uint32_t x, y, target_x, target_y; // current position and target position in the
// local coordinate system of the arc where [0,0] is the
int32_t x, y, target_x, target_y; // current position and target position in the
// local coordinate system of the arc-generator where [0,0] is the
// center of the arc.
int target_direction_x, target_direction_y; // sign(target_x)*angular_direction precalculated for speed
int target_direction_x, target_direction_y; // signof(target_x)*angular_direction precalculated for speed
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
@ -69,31 +71,41 @@ struct MotionControlState {
int8_t mode; // The current operation mode
int32_t position[3]; // The current position of the tool in absolute steps
int32_t pace; // Microseconds between each update in the current mode
uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
union {
struct LinearMotionParameters linear; // variables used in MC_MODE_LINEAR
struct ArcMotionParameters arc; // variables used in MC_MODE_ARC
uint32_t dwell_milliseconds; // variable used in MC_MODE_DWELL
};
};
struct MotionControlState state;
struct MotionControlState mc;
uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
void set_stepper_directions(int8_t *direction);
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
memset(&state, 0, sizeof(state));
memset(&mc, 0, sizeof(mc));
}
void mc_dwell(uint32_t milliseconds)
{
state.mode = MC_MODE_DWELL;
state.dwell_milliseconds = milliseconds;
mc.mode = MC_MODE_DWELL;
mc.dwell_milliseconds = milliseconds;
}
// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
@ -105,39 +117,45 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
// Same as mc_linear_motion but accepts target in absolute step coordinates
void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
{
state.mode = MC_MODE_LINEAR;
{
mc.linear.target[X_AXIS] = x;
mc.linear.target[Y_AXIS] = y;
mc.linear.target[Z_AXIS] = z;
mc.mode = MC_MODE_LINEAR;
uint8_t axis; // loop variable
// Determine direction and travel magnitude for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
state.linear.step_count[axis] = abs(state.linear.target[axis] - state.position[axis]);
state.linear.direction[axis] = sign(state.linear.step_count[axis]);
mc.linear.step_count[axis] = abs(mc.linear.target[axis] - mc.position[axis]);
mc.linear.direction[axis] = signof(mc.linear.target[axis] - mc.position[axis]);
}
// Find the magnitude of the axis with the longest travel
state.linear.maximum_steps = max(state.linear.step_count[Z_AXIS],
max(state.linear.step_count[X_AXIS], state.linear.step_count[Y_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]));
// Nothing to do?
if ((mc.linear.maximum_steps) == 0)
{
mc.mode = MC_MODE_AT_REST;
return;
}
// Set up a neat counter for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
state.linear.counter[axis] = -state.linear.maximum_steps/2;
mc.linear.counter[axis] = -mc.linear.maximum_steps/2;
}
// Set our direction pins
set_stepper_directions(state.linear.direction);
set_stepper_directions(mc.linear.direction);
// Calculate the microseconds we need to wait between each step to achieve the desired feed rate
if (invert_feed_rate) {
state.pace =
(feed_rate*1000000)/state.linear.maximum_steps;
mc.pace =
(feed_rate*1000000)/mc.linear.maximum_steps;
} else {
// Ask old Phytagoras to estimate how many steps our next move is going to take us:
uint32_t steps_to_travel =
ceil(sqrt(pow((X_STEPS_PER_MM*state.linear.step_count[X_AXIS]),2) +
pow((Y_STEPS_PER_MM*state.linear.step_count[Y_AXIS]),2) +
pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2)));
state.pace =
((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;
ceil(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)));
mc.pace =
((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / mc.linear.maximum_steps;
}
}
@ -147,27 +165,27 @@ void execute_linear_motion()
uint8_t step[3];
uint8_t axis; // loop variable
// Trace the line
clear_vector(step);
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
if (state.linear.target[axis] != state.position[axis])
{
state.linear.counter[axis] += state.linear.step_count[axis];
if (state.linear.counter[axis] > 0)
{
step[axis] = true;
state.linear.counter[axis] -= state.linear.maximum_steps;
state.position[axis] += state.linear.direction[axis];
}
}
}
if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
step_steppers(step);
} else {
state.mode = MC_MODE_AT_REST;
}
while(mc.mode) {
// Trace the line
clear_vector(step);
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
if (mc.linear.target[axis] != mc.position[axis])
{
mc.linear.counter[axis] += mc.linear.step_count[axis];
if (mc.linear.counter[axis] > 0)
{
step[axis] = true;
mc.linear.counter[axis] -= mc.linear.maximum_steps;
mc.position[axis] += mc.linear.direction[axis];
}
}
}
if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
step_steppers(step);
} else {
mc.mode = MC_MODE_AT_REST;
}
}
}
// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc,
@ -176,61 +194,62 @@ void execute_linear_motion()
// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate)
{
state.mode = MC_MODE_ARC;
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
mc.mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
state.arc.angular_direction = sign(angular_travel);
mc.arc.angular_direction = signof(angular_travel);
// Calculate the initial position and target position in the local coordinate system of the arc
state.arc.x = round(sin(theta)*radius*X_STEPS_PER_MM);
state.arc.y = round(cos(theta)*radius*X_STEPS_PER_MM);
state.arc.target_x = trunc(sin(theta+angular_travel)*(radius*X_STEPS_PER_MM-0.5));
state.arc.target_y = trunc(cos(theta+angular_travel)*(radius*X_STEPS_PER_MM-0.5));
mc.arc.x = round(sin(theta)*radius_steps);
mc.arc.y = round(cos(theta)*radius_steps);
mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps);
mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps);
// Precalculate these values to optimize target detection
state.arc.target_direction_x = sign(state.arc.target_x)*state.arc.angular_direction;
state.arc.target_direction_y = sign(state.arc.target_y)*state.arc.angular_direction;
mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction;
mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.angular_direction;
// The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error
// <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we
// are exactly on top of the arc.
state.arc.error = round(pow(state.arc.x,2) + pow(state.arc.y,2) - pow(radius,2));
mc.arc.error = mc.arc.x*mc.arc.x + mc.arc.y*mc.arc.y - radius_steps*radius_steps;
// Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
// by keeping track of the doubles of the arc coordinates at all times.
state.arc.x2 = 2*state.arc.x;
state.arc.y2 = 2*state.arc.y;
mc.arc.x2 = 2*mc.arc.x;
mc.arc.y2 = 2*mc.arc.y;
// Set up a vector with the steppers we are going to use tracing the plane of this arc
clear_vector(state.arc.plane_steppers);
state.arc.plane_steppers[axis_1] = 1;
state.arc.plane_steppers[axis_2] = 1;
clear_vector(mc.arc.plane_steppers);
mc.arc.plane_steppers[axis_1] = 1;
mc.arc.plane_steppers[axis_2] = 1;
// And map the local coordinate system of the arc onto the tool axes of the selected plane
state.arc.axis_x = axis_1;
state.arc.axis_y = axis_2;
mc.arc.axis_x = axis_1;
mc.arc.axis_y = axis_2;
// mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
state.pace =
mc.pace =
ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
state.arc.incomplete = true;
mc.arc.incomplete = true;
}
#define check_arc_target \
if ((state.arc.x * state.arc.target_direction_y >= \
state.arc.target_x * state.arc.target_direction_y) && \
(state.arc.y * state.arc.target_direction_x <= \
state.arc.target_y * state.arc.target_direction_x)) \
{ state.arc.incomplete = false; }
if ((mc.arc.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.target_y * mc.arc.target_direction_x)) \
{ 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)
{
uint32_t diagonal_error;
state.arc.x+=dx;
state.arc.error += 1+state.arc.x2*dx;
state.arc.x2 += 2*dx;
diagonal_error = state.arc.error + 1 + state.arc.y2*dy;
if(abs(state.arc.error) < abs(diagonal_error)) {
state.arc.y += dy;
state.arc.y2 += 2*dy;
state.arc.error = diagonal_error;
step_steppers(state.arc.plane_steppers); // step diagonal
mc.arc.x+=dx;
mc.arc.error += 1+mc.arc.x2*dx;
mc.arc.x2 += 2*dx;
diagonal_error = mc.arc.error + 1 + mc.arc.y2*dy;
if(abs(mc.arc.error) >= abs(diagonal_error)) {
mc.arc.y += dy;
mc.arc.y2 += 2*dy;
mc.arc.error = diagonal_error;
step_steppers(mc.arc.plane_steppers); // step diagonal
} else {
step_axis(state.arc.axis_x); // step straight
step_axis(mc.arc.axis_x); // step straight
}
check_arc_target;
}
@ -239,172 +258,77 @@ void step_arc_along_x(int8_t dx, int8_t dy)
void step_arc_along_y(int8_t dx, int8_t dy)
{
uint32_t diagonal_error;
state.arc.y+=dy;
state.arc.error += 1+state.arc.y2*dy;
state.arc.y2 += 2*dy;
diagonal_error = state.arc.error + 1 + state.arc.x2*dx;
if(abs(state.arc.error) < abs(diagonal_error)) {
state.arc.x += dx;
state.arc.x2 += 2*dx;
state.arc.error = diagonal_error;
step_steppers(state.arc.plane_steppers); // step diagonal
mc.arc.y+=dy;
mc.arc.error += 1+mc.arc.y2*dy;
mc.arc.y2 += 2*dy;
diagonal_error = mc.arc.error + 1 + mc.arc.x2*dx;
if(abs(mc.arc.error) >= abs(diagonal_error)) {
mc.arc.x += dx;
mc.arc.x2 += 2*dx;
mc.arc.error = diagonal_error;
step_steppers(mc.arc.plane_steppers); // step diagonal
} else {
step_axis(state.arc.axis_y); // step straight
step_axis(mc.arc.axis_y); // step straight
}
check_arc_target;
}
// 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.
void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
{
int8_t direction[3];
direction[state.arc.axis_x] = dx;
direction[state.arc.axis_y] = dy;
set_stepper_directions(direction);
}
/*
Quandrants of the arc
\ 7|0 /
\ | /
6 \|/ 1 y+
---------|-----------
5 /|\ 2 y-
/ | \
x- / 4|3 \ x+ */
#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
// Determine within which quadrant of the circle the provided coordinate falls
int quadrant(uint32_t x,uint32_t y)
{
// determine if the coordinate is in the quadrants 0,3,4 or 7
register int quad0347 = abs(x)<abs(y);
if (x<0) { // quad 4567
if (y<0) { // quad 45
return(quad0347 ? 4 : 5);
} else { // quad 67
return(quad0347 ? 7 : 6);
}
} else {
if (y<0) { // quad 23
return(quad0347 ? 3 : 2);
} else { // quad 01
return(quad0347 ? 0 : 1);
}
}
}
#endif
// Will trace the configured arc until the target is reached.
void execute_arc()
{
uint32_t start_x = state.arc.x;
uint32_t start_y = state.arc.y;
uint32_t start_x = mc.arc.x;
uint32_t start_y = mc.arc.y;
int dx, dy; // Trace directions
int8_t direction[3];
// state.mode is set to 0 (MC_MODE_AT_REST) when target is reached
while(state.arc.incomplete)
// mc.mode is set to 0 (MC_MODE_AT_REST) when target is reached
while(mc.arc.incomplete)
{
#ifdef UNROLLED_ARC_LOOP
// Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
int q = quadrant(state.arc.x, state.arc.y);
if (state.arc.angular_direction) {
switch (q) {
case 0:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
case 1:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(1,-1); }
case 2:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
case 5:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(-1,1); }
case 6:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
case 7:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,1); }
}
} else {
switch (q) {
case 7:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
case 6:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(-1,-1); }
case 5:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.y>state.arc.x)) { step_arc_along_y(1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }
case 2:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(1,1); }
case 1:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_y(-1,1); }
case 0:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,1); }
}
}
#else
dx = (state.arc.y!=0) ? sign(state.arc.y) * state.arc.angular_direction : -sign(state.arc.x);
dy = (state.arc.x!=0) ? -sign(state.arc.x) * state.arc.angular_direction : -sign(state.arc.y);
if (fabs(state.arc.x)<fabs(state.arc.y)) {
dx = (mc.arc.y!=0) ? signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x);
dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.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[mc.arc.axis_x] = dx;
direction[mc.arc.axis_y] = dy;
set_stepper_directions(direction);
if (abs(mc.arc.x)<abs(mc.arc.y)) {
step_arc_along_x(dx,dy);
} else {
} else {
step_arc_along_y(dx,dy);
}
#endif
}
// Update the tool position to the new actual position
state.position[state.arc.axis_x] += state.arc.x-start_x;
state.position[state.arc.axis_y] += state.arc.y-start_y;
mc.position[mc.arc.axis_x] += mc.arc.x-start_x;
mc.position[mc.arc.axis_y] += mc.arc.y-start_y;
// Because of rounding errors we might be off by a step or two. Adjust for this
// To be implemented
//void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
mc.mode = MC_MODE_AT_REST;
}
void mc_go_home()
{
state.mode = MC_MODE_HOME;
mc.mode = MC_MODE_HOME;
}
void execute_go_home()
{
st_go_home();
st_synchronize();
clear_vector(state.position); // By definition this is location [0, 0, 0]
state.mode = MC_MODE_AT_REST;
clear_vector(mc.position); // By definition this is location [0, 0, 0]
mc.mode = MC_MODE_AT_REST;
}
void mc_execute() {
st_set_pace(state.pace);
while(state.mode) {
switch(state.mode) {
st_set_pace(mc.pace);
printByte('~');
while(mc.mode) { // Loop because one task might start another task
switch(mc.mode) {
case MC_MODE_AT_REST: break;
case MC_MODE_DWELL: st_synchronize(); _delay_ms(state.dwell_milliseconds); state.mode = MC_MODE_AT_REST; break;
case MC_MODE_DWELL: st_synchronize(); _delay_ms(mc.dwell_milliseconds); mc.mode = MC_MODE_AT_REST; break;
case MC_MODE_LINEAR: execute_linear_motion(); break;
case MC_MODE_ARC: execute_arc(); break;
case MC_MODE_HOME: execute_go_home(); break;
@ -414,7 +338,7 @@ void mc_execute() {
int mc_status()
{
return(state.mode);
return(mc.mode);
}
// Set the direction pins for the stepper motors according to the provided vector.
@ -427,11 +351,10 @@ void set_stepper_directions(int8_t *direction)
way we can generate the whole direction bit-mask without doing any comparisions
or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry.
*/
direction_bits = ~(
mc.direction_bits = (
((direction[X_AXIS]&0x80)>>(7-X_DIRECTION_BIT)) |
((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) |
((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT))
);
((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT)));
}
// Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one
@ -439,16 +362,17 @@ void set_stepper_directions(int8_t *direction)
// 1, and the rest to 0.
inline void step_steppers(uint8_t *enabled)
{
st_buffer_step(direction_bits | enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT);
st_buffer_step(mc.direction_bits | (enabled[X_AXIS]<<X_STEP_BIT) |
(enabled[Y_AXIS]<<Y_STEP_BIT) | (enabled[Z_AXIS]<<Z_STEP_BIT));
}
// 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;
case X_AXIS: st_buffer_step(mc.direction_bits | (1<<X_STEP_BIT)); break;
case Y_AXIS: st_buffer_step(mc.direction_bits | (1<<Y_STEP_BIT)); break;
case Z_AXIS: st_buffer_step(mc.direction_bits | (1<<Z_STEP_BIT)); break;
}
}
@ -457,3 +381,4 @@ void mc_wait()
{
st_synchronize();
}

2
motion_control.h
View File

@ -61,4 +61,6 @@ void mc_wait();
// result < 0: the system is in an error state.
int mc_status();
void printCurrentPosition();
#endif

2
nuts_bolts.h
View File

@ -30,7 +30,7 @@
#define true 1
// Decide the sign of a value
#define sign(a) (a>0 ? 1 : ((a<0) ? -1 : 0))
#define signof(a) ((a>0) ? 1 : ((a<0) ? -1 : 0))
#define clear_vector(a) memset(a, 0, sizeof(a))

28
serial_protocol.c
View File

@ -33,6 +33,7 @@ uint8_t line_counter;
void prompt() {
printString(PROMPT);
line_counter = 0;
}
void print_result() {
@ -42,7 +43,7 @@ void print_result() {
uint32_t line_number;
int i; // loop variable
gc_get_status(position, &status_code, &inches_mode, &line_number);
printString("[ ");
printString("\r\n[ ");
for(i=X_AXIS; i<=Z_AXIS; i++) {
printInteger(trunc(position[i]*100));
printByte(' ');
@ -52,11 +53,12 @@ void print_result() {
printInteger(line_number);
printByte(':');
switch(status_code) {
case GCSTATUS_OK: printString("0 OK\n"); break;
case GCSTATUS_BAD_NUMBER_FORMAT: printString("1 Bad number format\n");
case GCSTATUS_EXPECTED_COMMAND_LETTER: printString("2 Expected command letter\n"); break;
case GCSTATUS_UNSUPPORTED_STATEMENT: printString("3 Unsupported statement\n"); break;
case GCSTATUS_MOTION_CONTROL_ERROR: printString("4 Motion control error\n"); break;
case GCSTATUS_OK: printString("0 OK\r\n"); break;
case GCSTATUS_BAD_NUMBER_FORMAT: printString("1 Bad number format\r\n"); break;
case GCSTATUS_EXPECTED_COMMAND_LETTER: printString("2 Expected command letter\r\n"); break;
case GCSTATUS_UNSUPPORTED_STATEMENT: printString("3 Unsupported statement\r\n"); break;
case GCSTATUS_MOTION_CONTROL_ERROR: printString("4 Motion control error\r\n"); break;
case GCSTATUS_FLOATING_POINT_ERROR: printString("5 Floating point error\r\n"); break;
}
}
@ -64,9 +66,9 @@ void sp_init()
{
beginSerial(BAUD_RATE);
printString("Grbl ");
printString("\r\nGrbl ");
printString(VERSION);
printByte('\n');
printByte('\r');
prompt();
}
@ -75,14 +77,20 @@ void sp_process()
char c;
while((c = serialRead()) != -1)
{
if(c == '\n') {
//printByte(c); // Echo
if(c == '\r') {
line[line_counter] = 0;
//printByte(EXECUTION_MARKER);
gc_execute_line(line);
line_counter = 0;
print_result();
prompt();
} else if (c == ' ' || c == '\t') {
// Throw away whitepace
} else if (c >= 'a' && c <= 'z') {
line[line_counter++] = c-'a'+'A';
} else {
line[line_counter] = c;
line[line_counter++] = c;
}
}
}

5
serial_protocol.h
View File

@ -20,7 +20,10 @@
#ifndef serial_h
#define serial_h
#define PROMPT ">>>"
// A string to let the client know we are ready for a new command
#define PROMPT "\r\n>>>"
// A character to acknowledge that the execution has started
#define EXECUTION_MARKER '~'
void sp_init();
void sp_process();

51
stepper.c
View File

@ -19,13 +19,16 @@
*/
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. The circle buffer implementation by the wiring_serial library by David A. Mellis */
and Philipp Tiefenbacher. The circle buffer implementation gleaned from the wiring_serial library
by David A. Mellis */
#include "stepper.h"
#include "config.h"
#include "nuts_bolts.h"
#include <avr/interrupt.h>
#include "wiring_serial.h"
#define TICKS_PER_MICROSECOND F_CPU/1000000
#define STEP_BUFFER_SIZE 100
@ -34,37 +37,38 @@ volatile int step_buffer_head = 0;
volatile int step_buffer_tail = 0;
uint8_t stepper_mode = STEPPER_MODE_STOPPED;
uint8_t echo_steps = true;
// This timer interrupt is executed at the pace set with set_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) {
// Set the stepper port according to the instructions
MOTORS_PORT = (MOTORS_PORT & ~MOTORS_MASK) | step_buffer[step_buffer_tail];
// 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 |= OCIE2A; // enable interrupt
// move the step buffer tail to the next instruction
step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
}
if (step_buffer_head != step_buffer_tail) {
// Set the stepper port according to the instructions
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | step_buffer[step_buffer_tail];
// 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
// move the step buffer tail to the next instruction
step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
}
}
// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
// the motor port after a short period (5us) completing one step cycle.
SIGNAL(SIG_OUTPUT_COMPARE2A)
{
MOTORS_PORT = MOTORS_PORT & ~MOTORS_MASK; // reset stepper pins
TIMSK2 &= ~OCIE2A; // disable this timer interrupt until next time
STEPPING_PORT = STEPPING_PORT & ~STEPPING_MASK; // reset stepper pins
TIMSK2 &= ~(1<<OCIE2A); // disable this timer interrupt until next time
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
MOTORS_DDR |= MOTORS_MASK;
STEPPING_DDR |= STEPPING_MASK;
LIMIT_DDR &= ~(LIMIT_MASK);
STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
@ -92,13 +96,18 @@ void st_init()
void st_buffer_step(uint8_t motor_port_bits)
{
if (echo_steps) {
printByte('!'+motor_port_bits);
// printIntegerInBase(motor_port_bits,2);printByte('\r');printByte('\n');
}
int i = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
// If the buffer is full: good! That means we are well ahead of the robot.
// Nap until there is room for more steps.
while(step_buffer_tail == i) { sleep_mode(); }
step_buffer[step_buffer_head] = motor_port_bits;
// step_buffer[step_buffer_head] = motor_port_bits;
step_buffer_head = i;
}
@ -140,6 +149,12 @@ inline void st_stop()
void st_set_pace(uint32_t microseconds)
{
printString("pace = ");
printInteger(microseconds);
printString("\n\r");
if (microseconds <= 500) {
microseconds = 500;
}
uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;
uint16_t ceiling;
uint16_t prescaler;
@ -163,6 +178,7 @@ void st_set_pace(uint32_t microseconds)
ceiling = 0xffff;
prescaler = 4;
}
// Set prescaler
TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
// Set ceiling
@ -226,3 +242,8 @@ void st_go_home()
{
// Todo: Perform the homing cycle
}
void st_set_echo(int value)
{
echo_steps = value;
}

3
stepper.h
View File

@ -53,4 +53,7 @@ inline void st_stop();
// Execute the homing cycle
void st_go_home();
// Echo steps to serial port? (true/false)
void st_set_echo(int value);
#endif

225
testbench/echotest.rb Normal file
View File

@ -0,0 +1,225 @@
require 'pp'
require 'rubygems'
require 'serialport'
require 'png'
# Requires the gems serialport and png
#
# $ gem install ruby-serialport
# $ gem install png
@sp = SerialPort.new '/dev/tty.usbserial-A4001o6L', 19200
@canvas = PNG::Canvas.new(500,500)
@x = 0
@y = 0
@ready_for_command = true
@last_byte = Time.now
@input = ''
@step_count = 0
Thread.new do
step_mode = false
@sp.each_byte do |byte|
#puts byte.chr
@last_byte = Time.now
@input << byte.chr
if byte == 126 # ('~')
step_mode = true
@input << '[step mode on]'
else
if step_mode
@step_count += 1
if byte == 10
@input << '[step mode off]'
step_mode = false
else
step = byte-33
puts @step_count
if (step & 1) != 0
if (step & 8) != 0
puts "x-1"
@x+=1
else
puts "x+1"
@x-=1
end
end
if (step & 2) != 0
if (step & 16) != 0
puts "y-1"
@y+=1
else
puts "y+1"
@y-=1
end
end
end
else
if byte == 62 # '>'
@ready_for_command = true
end
end
end
@canvas[250-@x,250-@y] = PNG::Color::Black
end
end
def send_command(command)
while !@ready_for_command do
sleep(1)
puts "Processing ..."
if (Time.now-@last_byte > 5)
return
end
end
sleep(0.5)
puts "Sent: #{command}"
@input << "[sent #{command}]"
@sp.write("#{command}\r")
@ready_for_command = false
end
blocks = <<-END.split("\n").map{|s|s.strip}
N10 G00 Z100.000 G53
N15 G00 X38.105 Y71.468 G53
N20 Z2.0 F200 G53
N25 G01 Z-0.2 G53
N30 X37.177 Y74.406 G53
N35 X37.779 Y74.698 G53
N40 X41.077 Y76.347 G53
N45 X42.177 Y73.719 G53
N50 X40.304 Y72.173 G53
N55 X39.548 Y73.873 G53
N60 X38.638 Y73.152 G53
N65 X39.153 Y71.915 G53
N70 X38.105 Y71.468 G53
N75 G00 Z2.0 G53
N80 X37.266 Y75.042 G53
N85 G01 Z-0.2 G53
N90 X37.862 Y79.198 G53
N95 G00 Z2.0 G53
N100 X37.684 Y77.960 G53
N105 G01 Z-0.2 G53
N110 X42.718 Y77.239 G53
N115 G00 Z2.0 G53
N120 X42.895 Y78.476 G53
N125 G01 Z-0.2 G53
N130 X42.300 Y74.320 G53
N135 G00 Z2.0 G53
N140 X43.975 Y77.690 G53
N145 G01 Z-0.2 G53
N150 X43.425 Y77.769 G53
N155 G00 Z2.0 G53
N160 X44.604 Y78.161 G53
N165 G01 Z-0.2 G53
N170 G02 X43.975 Y77.690 I-0.550 J0.079 G53
N175 G00 Z2.0 G53
N180 X44.643 Y78.436 G53
N185 G01 Z-0.2 G53
N190 X44.604 Y78.161 G53
N195 G00 Z2.0 G53
N200 X44.172 Y79.065 G53
N205 G01 Z-0.2 G53
N210 G02 X44.643 Y78.436 I-0.079 J-0.550 G53
N215 G00 Z2.0 G53
N220 X43.622 Y79.144 G53
N225 G01 Z-0.2 G53
N230 X44.172 Y79.065 G53
N235 G00 Z2.0 G53
N240 X43.780 Y80.243 G53
N245 G01 Z-0.2 G53
N250 X43.622 Y79.144 G53
N255 G00 Z2.0 G53
N260 X44.880 Y80.086 G53
N265 G01 Z-0.2 G53
N270 X43.780 Y80.243 G53
N275 G00 Z2.0 G53
N280 X46.488 Y79.154 G53
N285 G01 Z-0.2 G53
N290 G02 X46.808 Y79.669 I1.265 J-0.427 G53
N295 G02 X47.312 Y79.597 I0.229 J-0.198 G53
N300 G02 X47.474 Y79.012 I-1.172 J-0.639 G53
N305 G02 X47.452 Y78.454 I-2.606 J-0.179 G53
N310 G02 X47.316 Y77.912 I-2.587 J0.362 G53
N315 G02 X46.997 Y77.397 I-1.265 J0.427 G53
N320 G02 X46.493 Y77.469 I-0.229 J0.198 G53
N325 G02 X46.331 Y78.054 I1.172 J0.639 G53
N330 G02 X46.352 Y78.612 I2.606 J0.179 G53
N335 G02 X46.489 Y79.154 I2.587 J-0.362 G53
N340 G00 Z2.0 G53
N345 X45.370 Y77.630 G53
N350 G01 Z-0.2 G53
N355 X45.350 Y77.493 G53
N360 G00 Z2.0 G53
N365 X48.179 Y77.649 G53
N370 G01 Z-0.2 G53
N375 X48.218 Y77.924 G53
N380 G02 X49.318 Y77.766 I0.550 J-0.079 G53
N385 G01 X49.278 Y77.491 G53
N390 G02 X48.179 Y77.649 I-0.550 J0.079 G53
N395 G00 Z2.0 G53
N400 X48.435 Y78.945 G53
N405 G01 Z-0.2 G53
N410 G03 X49.397 Y78.807 I0.481 J-0.069 G53
N415 G01 X49.417 Y78.944 G53
N420 G03 X48.454 Y79.082 I-0.481 J0.069 G53
N425 G01 X48.435 Y78.945 G53
N430 G00 Z2.0 G53
N435 X50.379 Y79.297 G53
N440 G01 Z-0.2 G53
N445 X51.479 Y79.140 G53
N450 G00 Z2.0 G53
N455 X50.222 Y78.198 G53
N460 G01 Z-0.2 G53
N465 X50.379 Y79.297 G53
N470 G00 Z2.0 G53
N475 X50.772 Y78.119 G53
N480 G01 Z-0.2 G53
N485 X50.222 Y78.198 G53
N490 G00 Z2.0 G53
N495 X51.243 Y77.490 G53
N500 G01 Z-0.2 G53
N505 G03 X50.772 Y78.119 I-0.550 J0.079 G53
N510 G00 Z2.0 G53
N515 X51.203 Y77.215 G53
N520 G01 Z-0.2 G53
N525 X51.243 Y77.490 G53
N530 G00 Z2.0 G53
N535 X50.574 Y76.744 G53
N540 G01 Z-0.2 G53
N545 G03 X51.203 Y77.215 I0.079 J0.550 G53
N550 G00 Z2.0 G53
N555 X50.025 Y76.823 G53
N560 G01 Z-0.2 G53
N565 X50.574 Y76.744 G53
N570 G00 Z2.0 G53
N575 X53.049 Y76.389 G53
N580 G01 Z-0.2 G53
N585 X51.949 Y76.547 G53
N590 X53.274 Y78.222 G53
N595 G03 X53.327 Y78.454 I-0.218 J0.172 G53
N600 G03 X52.262 Y78.607 I-0.552 J-0.061 G53
N605 G00 Z2.0 G53
N610 M30 G53
END
blocks.each do |block|
send_command(block)
if (Time.now-@last_byte > 5)
puts "Bailing, cause somethin' went wrong"
break
end
end
sleep(3);
PNG.new(@canvas).save("gcodetest.png")
puts @input
`open gcodetest.png`

BIN
testbench/gcodetest.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.0 KiB