Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Merge chamnit/v0_7 with grbl/master

  • Loading branch information...
commit 9713f9067dbc47c7622357736d0a55ad3cd8771d 1 parent 74576a8
@chamnit chamnit authored
View
4 Makefile
@@ -30,8 +30,8 @@
DEVICE = atmega328p
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 \
- eeprom.o settings.o planner.o
+OBJECTS = main.o motion_control.o gcode.o spindle_control.o serial.o protocol.o stepper.o \
+ eeprom.o settings.o planner.o nuts_bolts.o limits.o print.o
# FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m
# update that line with this when programmer is back up:
View
58 config.h
@@ -3,6 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
+ Copyright (c) 2011 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@@ -21,14 +22,16 @@
#ifndef config_h
#define config_h
+// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
+
#define BAUD_RATE 9600
// Updated default pin-assignments from 0.6 onwards
// (see bottom of file for a copy of the old config)
-#define STEPPERS_ENABLE_DDR DDRB
-#define STEPPERS_ENABLE_PORT PORTB
-#define STEPPERS_ENABLE_BIT 0
+#define STEPPERS_DISABLE_DDR DDRB
+#define STEPPERS_DISABLE_PORT PORTB
+#define STEPPERS_DISABLE_BIT 0
#define STEPPING_DDR DDRD
#define STEPPING_PORT PORTD
@@ -40,7 +43,7 @@
#define Z_DIRECTION_BIT 7
#define LIMIT_DDR DDRB
-#define LIMIT_PORT PORTB
+#define LIMIT_PIN PINB
#define X_LIMIT_BIT 1
#define Y_LIMIT_BIT 2
#define Z_LIMIT_BIT 3
@@ -53,17 +56,51 @@
#define SPINDLE_DIRECTION_PORT PORTB
#define SPINDLE_DIRECTION_BIT 5
-// The temporal resolution of the acceleration management subsystem. Higher number
-// give smoother acceleration but may impact performance
-#define ACCELERATION_TICKS_PER_SECOND 40L
+// This parameter sets the delay time before disabling the steppers after the final block of movement.
+// A short delay ensures the steppers come to a complete stop and the residual inertial force in the
+// CNC axes don't cause the axes to drift off position. This is particularly important when manually
+// entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift,
+// grbl has no way to know this has happened, since stepper motors are open-loop control. Depending
+// on the machine, this parameter may need to be larger or smaller than the default time.
+// NOTE: If defined 0, the delay will not be compiled.
+#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer >= 0
+
+// The temporal resolution of the acceleration management subsystem. Higher number give smoother
+// acceleration but may impact performance.
+// NOTE: Increasing this parameter will help any resolution related issues, especially with machines
+// requiring very high accelerations and/or very fast feedrates. In general, this will reduce the
+// error between how the planner plans the motions and how the stepper program actually performs them.
+// However, at some point, the resolution can be high enough, where the errors related to numerical
+// round-off can be great enough to cause problems and/or it's too fast for the Arduino. The correct
+// value for this parameter is machine dependent, so it's advised to set this only as high as needed.
+// Approximate successful values can range from 30L to 100L or more.
+#define ACCELERATION_TICKS_PER_SECOND 50L
+
+// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
+// of the buffer and all stops. This should not be much greater than zero and should only be changed
+// if unwanted behavior is observed on a user's machine when running at very slow speeds.
+#define MINIMUM_PLANNER_SPEED 0.0 // (mm/min)
+
+// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never runs
+// slower than this value, except when sleeping. This parameter overrides the minimum planner speed.
+// This is primarily used to guarantee that the end of a movement is always reached and not stop to
+// never reach its target. This parameter should always be greater than zero.
+#define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only
+
+// Number of arc generation iterations by small angle approximation before exact arc trajectory
+// correction. This parameter maybe decreased if there are issues with the accuracy of the arc
+// generations. In general, the default value is more than enough for the intended CNC applications
+// of grbl, and should be on the order or greater than the size of the buffer to help with the
+// computational efficiency of generating arcs.
+#define N_ARC_CORRECTION 25 // Integer (1-255)
#endif
// Pin-assignments from Grbl 0.5
-// #define STEPPERS_ENABLE_DDR DDRD
-// #define STEPPERS_ENABLE_PORT PORTD
-// #define STEPPERS_ENABLE_BIT 2
+// #define STEPPERS_DISABLE_DDR DDRD
+// #define STEPPERS_DISABLE_PORT PORTD
+// #define STEPPERS_DISABLE_BIT 2
//
// #define STEPPING_DDR DDRC
// #define STEPPING_PORT PORTC
@@ -87,4 +124,3 @@
// #define SPINDLE_DIRECTION_DDR DDRD
// #define SPINDLE_DIRECTION_PORT PORTD
// #define SPINDLE_DIRECTION_BIT 7
-
View
30 doc/planner-maths.txt
@@ -0,0 +1,30 @@
+Reasoning behind the mathematics in 'planner' module (in the key of 'Mathematica')
+==================================================================================
+
+
+s == speed, a == acceleration, t == time, d == distance
+
+Basic definitions:
+
+ Speed[s_, a_, t_] := s + (a*t)
+ Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
+
+Distance to reach a specific speed with a constant acceleration:
+
+ Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
+ d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
+
+Speed after a given distance of travel with constant acceleration:
+
+ Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
+ m -> Sqrt[2 a d + s^2]
+
+ DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
+
+When to start braking (di) to reach a specified destionation speed (s2) after accelerating
+from initial speed s1 without ever stopping at a plateau:
+
+ Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
+ di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
+
+ IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
View
2  doc/resources.txt
@@ -3,7 +3,7 @@ Allocation of AVR peripherals in Grbl
See config.h for pin allocation.
-The UART is handled by 'wiring_serial' and used primarily for streaming gcode
+The UART is handled by 'serial' and used primarily for streaming gcode
16 bit Timer 1 and the TIMER1_COMPA interrupt is used by the 'stepper' module to handle step events
View
8 doc/structure.txt
@@ -3,10 +3,10 @@ The general structure of Grbl
The main processing stack:
-'serial_protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution.
+'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution.
Provides status responses for each command.
-'gcode' : Recieves gcode from 'serial_protocol', parses it according to the current state
+'gcode' : Recieves gcode from 'protocol', parses it according to the current state
of the parser and issues commands via '..._control' modules
'spindle_control' : Commands for controlling the spindle.
@@ -36,4 +36,6 @@ Supporting files:
'nuts_bolts.h' : A tiny collection of useful constants and macros used everywhere
-'wiring_serial' : Low level serial library initially from an old version of the Arduino software
+'serial' : Low level serial communications
+
+'print' : Functions to print strings of different formats (using serial)
View
204 gcode.c
@@ -3,7 +3,8 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
-
+ Copyright (c) 2011 Sungeun K. Jeon
+
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
@@ -22,7 +23,6 @@
by Kramer, Proctor and Messina. */
#include "gcode.h"
-#include <stdlib.h>
#include <string.h>
#include "nuts_bolts.h"
#include <math.h>
@@ -30,13 +30,14 @@
#include "motion_control.h"
#include "spindle_control.h"
#include "errno.h"
-#include "serial_protocol.h"
+#include "protocol.h"
#define MM_PER_INCH (25.4)
#define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1
#define NEXT_ACTION_GO_HOME 2
+#define NEXT_ACTION_SET_COORDINATE_OFFSET 3
#define MOTION_MODE_SEEK 0 // G0
#define MOTION_MODE_LINEAR 1 // G1
@@ -63,7 +64,7 @@ typedef struct {
uint8_t inches_mode; /* 0 = millimeter mode, 1 = inches mode {G20, G21} */
uint8_t absolute_mode; /* 0 = relative motion, 1 = absolute motion {G90, G91} */
uint8_t program_flow;
- int spindle_direction;
+ int8_t spindle_direction;
double feed_rate, seek_rate; /* Millimeters/second */
double position[3]; /* Where the interpreter considers the tool to be at this point in the code */
uint8_t tool;
@@ -76,14 +77,9 @@ static parser_state_t gc;
#define FAIL(status) gc.status_code = status;
-int read_double(char *line, // <- string: line of RS274/NGC code being processed
- int *char_counter, // <- pointer to a counter for position on the line
- double *double_ptr); // <- pointer to double to be read
-
-int next_statement(char *letter, double *double_ptr, char *line, int *char_counter);
+static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter);
-
-void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
+static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
{
gc.plane_axis_0 = axis_0;
gc.plane_axis_1 = axis_1;
@@ -92,75 +88,37 @@ void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
void gc_init() {
memset(&gc, 0, sizeof(gc));
- gc.feed_rate = settings.default_feed_rate/60;
- gc.seek_rate = settings.default_seek_rate/60;
+ gc.feed_rate = settings.default_feed_rate;
+ gc.seek_rate = settings.default_seek_rate;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
- gc.absolute_mode = TRUE;
+ gc.absolute_mode = true;
}
-inline float to_millimeters(double value) {
+static float to_millimeters(double value) {
return(gc.inches_mode ? (value * MM_PER_INCH) : value);
}
-// 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));
- if (y>0) {
- return(theta);
- } else {
- if (theta>0)
- {
- return(M_PI-theta);
- } else {
- return(-M_PI-theta);
- }
- }
-}
-
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
-// characters and signed floating point values (no whitespace).
+// characters and signed floating point values (no whitespace). Comments and block delete
+// characters have been removed.
uint8_t gc_execute_line(char *line) {
- int char_counter = 0;
+ uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
- int radius_mode = FALSE;
+ uint8_t radius_mode = false;
- uint8_t absolute_override = FALSE; /* 1 = absolute motion for this block only {G53} */
+ uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
-
- clear_vector(target);
- clear_vector(offset);
- gc.status_code = GCSTATUS_OK;
+ gc.status_code = STATUS_OK;
- // Disregard comments and block delete
- if (line[0] == '(') { return(gc.status_code); }
- if (line[0] == '/') { char_counter++; } // ignore block delete
-
- // If the line starts with an '$' it is a configuration-command
- if (line[0] == '$') {
- // Parameter lines are on the form '$4=374.3' or '$' to dump current settings
- char_counter = 1;
- if(line[char_counter] == 0) { settings_dump(); return(GCSTATUS_OK); }
- read_double(line, &char_counter, &p);
- if(line[char_counter++] != '=') { return(GCSTATUS_UNSUPPORTED_STATEMENT); }
- read_double(line, &char_counter, &value);
- if(line[char_counter] != 0) { return(GCSTATUS_UNSUPPORTED_STATEMENT); }
- settings_store_setting(p, value);
- return(gc.status_code);
- }
-
- /* We'll handle this as g-code. First: parse all statements */
-
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
@@ -177,16 +135,17 @@ uint8_t gc_execute_line(char *line) {
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
- case 20: gc.inches_mode = TRUE; break;
- case 21: gc.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_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);
+ 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 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
+ case 93: gc.inverse_feed_rate_mode = true; break;
+ case 94: gc.inverse_feed_rate_mode = false; break;
+ default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
@@ -197,7 +156,7 @@ uint8_t gc_execute_line(char *line) {
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);
+ default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
@@ -209,6 +168,7 @@ uint8_t gc_execute_line(char *line) {
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
+ clear_vector(target);
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
@@ -218,19 +178,20 @@ uint8_t gc_execute_line(char *line) {
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
+ if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
- gc.seek_rate = unit_converted_value/60;
+ gc.seek_rate = unit_converted_value;
} else {
- gc.feed_rate = unit_converted_value/60; // millimeters pr second
+ gc.feed_rate = unit_converted_value; // millimeters per minute
}
}
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 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.spindle_speed = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
@@ -246,21 +207,20 @@ uint8_t gc_execute_line(char *line) {
if (gc.status_code) { return(gc.status_code); }
// Update spindle state
- if (gc.spindle_direction) {
- spindle_run(gc.spindle_direction, gc.spindle_speed);
- } else {
- spindle_stop();
- }
+ spindle_run(gc.spindle_direction, gc.spindle_speed);
// Perform any physical actions
switch (next_action) {
- case NEXT_ACTION_GO_HOME: mc_go_home(); break;
- case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break;
+ case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(target); break;
+ case NEXT_ACTION_DWELL: mc_dwell(p); break;
+ case NEXT_ACTION_SET_COORDINATE_OFFSET:
+ mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
+ break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
- mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, FALSE);
+ mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
@@ -327,7 +287,7 @@ uint8_t gc_execute_line(char *line) {
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
// 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(STATUS_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; }
@@ -352,50 +312,29 @@ uint8_t gc_execute_line(char *line) {
// 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 opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
- if (r < 0) { h_x2_div_d = -h_x2_div_d; }
+ if (r < 0) {
+ h_x2_div_d = -h_x2_div_d;
+ r = -r; // Finished with r. Set to positive for mc_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_1] = (y+(x*h_x2_div_d))/2;
- }
-
- /*
- This segment sets up an clockwise or counterclockwise arc from the current position to the target position around
- the center designated by the offset vector. All theta-values measured in radians of deviance from the positive
- y-axis.
-
- | <- theta == 0
- * * *
- * *
- * *
- * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
- * /
- 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[gc.plane_axis_0], -offset[gc.plane_axis_1]);
- // calculate the theta (angle) of the target point
- 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]);
- // 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 (gc.motion_mode == MOTION_MODE_CCW_ARC) {
- angular_travel = angular_travel-2*M_PI;
+ offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
+ offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
+
+ } else { // Offset mode specific computations
+
+ r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
+
}
- // Find the radius
- double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
- // Calculate the motion along the depth axis of the helix
- double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
+
+ // Set clockwise/counter-clockwise sign for mc_arc computations
+ uint8_t isclockwise = false;
+ if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
+
// Trace the arc
- mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
+ mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
- gc.position);
- // Finish off with a line to make sure we arrive exactly where we think we are
- mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
- (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
+ r, isclockwise);
+
break;
#endif
}
@@ -411,40 +350,24 @@ uint8_t gc_execute_line(char *line) {
// 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 *char_counter) {
+static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter) {
if (line[*char_counter] == 0) {
return(0); // No more statements
}
*letter = line[*char_counter];
if((*letter < 'A') || (*letter > 'Z')) {
- FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER);
+ FAIL(STATUS_EXPECTED_COMMAND_LETTER);
return(0);
}
(*char_counter)++;
if (!read_double(line, char_counter, double_ptr)) {
+ FAIL(STATUS_BAD_NUMBER_FORMAT);
return(0);
};
return(1);
}
-int read_double(char *line, //!< string: line of RS274/NGC code being processed
- int *char_counter, //!< pointer to a counter for position on the line
- double *double_ptr) //!< pointer to double to be read
-{
- char *start = line + *char_counter;
- char *end;
-
- *double_ptr = strtod(start, &end);
- if(end == start) {
- FAIL(GCSTATUS_BAD_NUMBER_FORMAT);
- return(0);
- };
-
- *char_counter = end - line;
- return(1);
-}
-
/*
Intentionally not supported:
@@ -464,4 +387,3 @@ int read_double(char *line, //!< string: line of RS274/NGC code be
group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection
group 13 = {G61, G61.1, G64} path control mode
*/
-
View
6 gcode.h
@@ -23,12 +23,6 @@
#define gcode_h
#include <avr/io.h>
-#define GCSTATUS_OK 0
-#define GCSTATUS_BAD_NUMBER_FORMAT 1
-#define GCSTATUS_EXPECTED_COMMAND_LETTER 2
-#define GCSTATUS_UNSUPPORTED_STATEMENT 3
-#define GCSTATUS_FLOATING_POINT_ERROR 4
-
// Initialize the parser
void gc_init();
View
103 limits.c
@@ -0,0 +1,103 @@
+/*
+ limits.h - code pertaining to limit-switches and performing the homing cycle
+ Part of Grbl
+
+ Copyright (c) 2009-2011 Simen Svale Skogsrud
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <util/delay.h>
+#include <avr/io.h>
+#include "stepper.h"
+#include "settings.h"
+#include "nuts_bolts.h"
+#include "config.h"
+
+void limits_init() {
+ LIMIT_DDR &= ~(LIMIT_MASK);
+}
+
+static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool reverse_direction, uint32_t microseconds_per_pulse) {
+ // First home the Z axis
+ uint32_t step_delay = microseconds_per_pulse - settings.pulse_microseconds;
+ uint8_t out_bits = DIRECTION_MASK;
+ uint8_t limit_bits;
+
+ if (x_axis) { out_bits |= (1<<X_STEP_BIT); }
+ if (y_axis) { out_bits |= (1<<Y_STEP_BIT); }
+ if (z_axis) { out_bits |= (1<<Z_STEP_BIT); }
+
+ // Invert direction bits if this is a reverse homing_cycle
+ if (reverse_direction) {
+ out_bits ^= DIRECTION_MASK;
+ }
+
+ // Apply the global invert mask
+ out_bits ^= settings.invert_mask;
+
+ // Set direction pins
+ STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
+
+ for(;;) {
+ limit_bits = LIMIT_PIN;
+ if (reverse_direction) {
+ // Invert limit_bits if this is a reverse homing_cycle
+ limit_bits ^= LIMIT_MASK;
+ }
+ if (x_axis && !(LIMIT_PIN & (1<<X_LIMIT_BIT))) {
+ x_axis = false;
+ out_bits ^= (1<<X_STEP_BIT);
+ }
+ if (y_axis && !(LIMIT_PIN & (1<<Y_LIMIT_BIT))) {
+ y_axis = false;
+ out_bits ^= (1<<Y_STEP_BIT);
+ }
+ if (z_axis && !(LIMIT_PIN & (1<<Z_LIMIT_BIT))) {
+ z_axis = false;
+ out_bits ^= (1<<Z_STEP_BIT);
+ }
+ // Check if we are done
+ if(!(x_axis || y_axis || z_axis)) { return; }
+ STEPPING_PORT |= out_bits & STEP_MASK;
+ _delay_us(settings.pulse_microseconds);
+ STEPPING_PORT ^= out_bits & STEP_MASK;
+ _delay_us(step_delay);
+ }
+ return;
+}
+
+static void approach_limit_switch(bool x, bool y, bool z) {
+ homing_cycle(x, y, z, false, 100000);
+}
+
+static void leave_limit_switch(bool x, bool y, bool z) {
+ homing_cycle(x, y, z, true, 500000);
+}
+
+void limits_go_home() {
+ st_synchronize();
+ // Store the current limit switch state
+ uint8_t original_limit_state = LIMIT_PIN;
+ approach_limit_switch(false, false, true); // First home the z axis
+ approach_limit_switch(true, true, false); // Then home the x and y axis
+ // Xor previous and current limit switch state to determine which were high then but have become
+ // low now. These are the actual installed limit switches.
+ uint8_t limit_switches_present = (original_limit_state ^ LIMIT_PIN) & LIMIT_MASK;
+ // Now carefully leave the limit switches
+ leave_limit_switch(
+ limit_switches_present & (1<<X_LIMIT_BIT),
+ limit_switches_present & (1<<Y_LIMIT_BIT),
+ limit_switches_present & (1<<Z_LIMIT_BIT));
+}
View
18 serial_protocol.h → limits.h
@@ -1,5 +1,5 @@
/*
- serial_protocol.h - the serial protocol master control unit
+ limits.h - code pertaining to limit-switches and performing the homing cycle
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
@@ -17,14 +17,14 @@
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
-#ifndef serial_h
-#define serial_h
-// Initialize the serial protocol
-void sp_init();
+#ifndef limits_h
+#define limits_h
-// Read command lines from the serial port and execute them as they
-// come in. Blocks until the serial buffer is emptied.
-void sp_process();
+// initialize the limits module
+void limits_init();
-#endif
+// perform the homing cycle
+void limits_go_home();
+
+#endif
View
25 main.c
@@ -20,33 +20,36 @@
#include <avr/io.h>
#include <avr/sleep.h>
+#include <avr/interrupt.h>
#include <util/delay.h>
+#include "config.h"
#include "planner.h"
#include "stepper.h"
#include "spindle_control.h"
#include "motion_control.h"
#include "gcode.h"
-#include "serial_protocol.h"
+#include "protocol.h"
+#include "limits.h"
#include "settings.h"
-#include "wiring_serial.h"
-
-// #ifndef __AVR_ATmega328P__
-// # error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')"
-// #endif
+#include "serial.h"
int main(void)
{
- sp_init();
+ sei(); // Enable interrupts
+
+ serial_init(BAUD_RATE);
+ protocol_init();
settings_init();
plan_init();
st_init();
spindle_init();
- gc_init();
+ gc_init();
+ limits_init();
- for(;;){
- sleep_mode(); // Wait for it ...
- sp_process(); // ... process the serial protocol
+ while (1) {
+// sleep_mode(); // Wait for it ...
+ protocol_process(); // ... process the serial protocol
}
return 0; /* never reached */
}
View
129 motion_control.c
@@ -3,7 +3,8 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
-
+ Copyright (c) 2011 Sungeun K. Jeon
+
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
@@ -20,6 +21,7 @@
#include <avr/io.h>
#include "settings.h"
+#include "config.h"
#include "motion_control.h"
#include <util/delay.h>
#include <math.h>
@@ -27,56 +29,115 @@
#include "nuts_bolts.h"
#include "stepper.h"
#include "planner.h"
-#include "wiring_serial.h"
-
-void mc_dwell(uint32_t milliseconds)
+// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application.
+void mc_dwell(double seconds)
{
- st_synchronize();
- _delay_ms(milliseconds);
+ uint16_t i = floor(seconds);
+ st_synchronize();
+ _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder
+ while (i > 0) {
+ _delay_ms(1000); // Delay one second
+ i--;
+ }
}
-// 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 circle plane in tool space. Stick the remaining
-// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
-// position is a pointer to a vector representing the current position in millimeters.
-
#ifdef __AVR_ATmega328P__
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
// segment is configured in settings.mm_per_arc_segment.
-void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2,
- int axis_linear, double feed_rate, int invert_feed_rate, double *position)
+void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
+ uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise)
{
- int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
- plan_set_acceleration_manager_enabled(FALSE); // disable acceleration management for the duration of the arc
- double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
+ double center_axis0 = position[axis_0] + offset[axis_0];
+ double center_axis1 = position[axis_1] + offset[axis_1];
+ double linear_travel = target[axis_linear] - position[axis_linear];
+ double r_axis0 = -offset[axis_0]; // Radius vector from center to current location
+ double r_axis1 = -offset[axis_1];
+ double rt_axis0 = target[axis_0] - center_axis0;
+ double rt_axis1 = target[axis_1] - center_axis1;
+
+ // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+ double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
+ if (angular_travel < 0) { angular_travel += 2*M_PI; }
+ if (isclockwise) { angular_travel -= 2*M_PI; }
+
+ double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (millimeters_of_travel == 0.0) { return; }
- uint16_t segments = ceil(millimeters_of_travel/settings.mm_per_arc_segment);
+ uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment);
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
// all segments.
if (invert_feed_rate) { feed_rate *= segments; }
- // The angular motion for each segment
+
double theta_per_segment = angular_travel/segments;
- // The linear motion for each segment
double linear_per_segment = linear_travel/segments;
- // Compute the center of this circle
- double center_x = position[axis_1]-sin(theta)*radius;
- double center_y = position[axis_2]-cos(theta)*radius;
- // a vector to track the end point of each segment
- double target[3];
- int i;
+
+ /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
+ and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
+ r_T = [cos(phi) -sin(phi);
+ sin(phi) cos(phi] * r ;
+
+ For arc generation, the center of the circle is the axis of rotation and the radius vector is
+ defined from the circle center to the initial position. Each line segment is formed by successive
+ vector rotations. This requires only two cos() and sin() computations to form the rotation
+ matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+ all double numbers are single precision on the Arduino. (True double precision will not have
+ round off issues for CNC applications.) Single precision error can accumulate to be greater than
+ tool precision in some cases. Therefore, arc path correction is implemented.
+
+ Small angle approximation may be used to reduce computation overhead further. This approximation
+ holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
+ theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+ to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
+ numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+ issue for CNC machines with the single precision Arduino calculations.
+
+ This approximation also allows mc_arc to immediately insert a line segment into the planner
+ without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+ a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
+ This is important when there are successive arc motions.
+ */
+ // Vector rotation matrix values
+ double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
+ double sin_T = theta_per_segment;
+
+ double arc_target[3];
+ double sin_Ti;
+ double cos_Ti;
+ double r_axisi;
+ uint16_t i;
+ int8_t count = 0;
+
// Initialize the linear axis
- target[axis_linear] = position[axis_linear];
- for (i=0; i<=segments; i++) {
- target[axis_linear] += linear_per_segment;
- theta += theta_per_segment;
- target[axis_1] = center_x+sin(theta)*radius;
- target[axis_2] = center_y+cos(theta)*radius;
- plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+ arc_target[axis_linear] = position[axis_linear];
+
+ for (i = 1; i<segments; i++) { // Increment (segments-1)
+
+ if (count < N_ARC_CORRECTION) {
+ // Apply vector rotation matrix
+ r_axisi = r_axis0*sin_T + r_axis1*cos_T;
+ r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
+ r_axis1 = r_axisi;
+ count++;
+ } else {
+ // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+ // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+ cos_Ti = cos(i*theta_per_segment);
+ sin_Ti = sin(i*theta_per_segment);
+ r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
+ r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
+ count = 0;
+ }
+
+ // Update arc_target location
+ arc_target[axis_0] = center_axis0 + r_axis0;
+ arc_target[axis_1] = center_axis1 + r_axis1;
+ arc_target[axis_linear] += linear_per_segment;
+ plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], feed_rate, invert_feed_rate);
+
}
- plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
+ // Ensure last segment arrives at target location.
+ plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
}
#endif
View
26 motion_control.h
@@ -3,7 +3,8 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
-
+ Copyright (c) 2011 Sungeun K. Jeon
+
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
@@ -24,24 +25,27 @@
#include <avr/io.h>
#include "planner.h"
+// NOTE: Although the following functions structurally belongs in this module, there is nothing to do but
+// to forward the request to the planner.
+
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate)
-// NOTE: Although this function structurally belongs in this module, there is nothing to do but
-// to forward the request to the planner. For efficiency the function is implemented with a macro.
+
+#define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z)
#ifdef __AVR_ATmega328P__
-// 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 circle plane in tool space. Stick the remaining
-// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
-void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2,
- int axis_linear, double feed_rate, int invert_feed_rate, double *position);
+// Execute an arc in offset mode format. position == current xyz, target == target xyz,
+// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
+// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
+// for vector transformation direction.
+void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
+ uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise);
#endif
-// Dwell for a couple of time units
-void mc_dwell(uint32_t milliseconds);
+// Dwell for a specific number of seconds
+void mc_dwell(double seconds);
// Send the tool home (not implemented)
void mc_go_home();
View
38 nuts_bolts.c
@@ -0,0 +1,38 @@
+/*
+ nuts_bolts.c - Shared functions
+ Part of Grbl
+
+ Copyright (c) 2009-2011 Simen Svale Skogsrud
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "nuts_bolts.h"
+#include <stdint.h>
+#include <stdlib.h>
+
+int read_double(char *line, uint8_t *char_counter, double *double_ptr)
+{
+ char *start = line + *char_counter;
+ char *end;
+
+ *double_ptr = strtod(start, &end);
+ if(end == start) {
+ return(false);
+ };
+
+ *char_counter = end - line;
+ return(true);
+}
+
View
16 nuts_bolts.h
@@ -1,8 +1,9 @@
/*
- motion_control.h - cartesian robot controller.
+ nuts_bolts.h - Header file for shared definitions, variables, and functions
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
+ Copyright (c) 2011 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@@ -21,15 +22,24 @@
#ifndef nuts_bolts_h
#define nuts_bolts_h
#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
-#define FALSE 0
-#define TRUE 1
+#define false 0
+#define true 1
#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2
#define clear_vector(a) memset(a, 0, sizeof(a))
+#define clear_vector_double(a) memset(a, 0.0, sizeof(a))
#define max(a,b) (((a) > (b)) ? (a) : (b))
+#define min(a,b) (((a) < (b)) ? (a) : (b))
+
+// Read a floating point value from a string. Line points to the input buffer, char_counter
+// is the indexer pointing to the current character of the line, while double_ptr is
+// a pointer to the result variable. Returns true when it succeeds
+int read_double(char *line, uint8_t *char_counter, double *double_ptr);
#endif
View
548 planner.c
@@ -3,7 +3,8 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
-
+ Copyright (c) 2011 Sungeun K. Jeon
+
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
@@ -20,38 +21,6 @@
/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
-/*
- Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
-
- s == speed, a == acceleration, t == time, d == distance
-
- Basic definitions:
-
- Speed[s_, a_, t_] := s + (a*t)
- Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
-
- Distance to reach a specific speed with a constant acceleration:
-
- Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
- d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
-
- Speed after a given distance of travel with constant acceleration:
-
- Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
- m -> Sqrt[2 a d + s^2]
-
- DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
-
- When to start braking (di) to reach a specified destionation speed (s2) after accelerating
- from initial speed s1 without ever stopping at a plateau:
-
- Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
- di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
-
- IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
-*/
-
-
#include <inttypes.h>
#include <math.h>
#include <stdlib.h>
@@ -61,39 +30,49 @@
#include "stepper.h"
#include "settings.h"
#include "config.h"
-#include "wiring_serial.h"
// The number of linear motions that can be in the plan at any give time
#ifdef __AVR_ATmega328P__
-#define BLOCK_BUFFER_SIZE 20
+#define BLOCK_BUFFER_SIZE 18
#else
#define BLOCK_BUFFER_SIZE 5
#endif
static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
-static volatile int block_buffer_head; // Index of the next block to be pushed
-static volatile int block_buffer_tail; // Index of the block to process now
+static volatile uint8_t block_buffer_head; // Index of the next block to be pushed
+static volatile uint8_t block_buffer_tail; // Index of the block to process now
-// The current position of the tool in absolute steps
-static int32_t position[3];
+static int32_t position[3]; // The current position of the tool in absolute steps
+static double previous_unit_vec[3]; // Unit vector of previous path line segment
+static double previous_nominal_speed; // Nominal speed of previous path line segment
-static uint8_t acceleration_manager_enabled; // Acceleration management active?
-#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
+// Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+static uint8_t next_block_index(uint8_t block_index)
+{
+ block_index++;
+ if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
+ return(block_index);
+}
+
+
+// Returns the index of the previous block in the ring buffer
+static uint8_t prev_block_index(uint8_t block_index)
+{
+ if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
+ block_index--;
+ return(block_index);
+}
+
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration:
-inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) {
- return(
- (target_rate*target_rate-initial_rate*initial_rate)/
- (2L*acceleration)
- );
+static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration)
+{
+ return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
}
-// This function gives you the point at which you must start braking (at the rate of -acceleration) if
-// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
-// a total travel of distance. This can be used to compute the intersection point between acceleration and
-// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
/* + <- some maximum rate we don't care about
/|\
@@ -104,154 +83,99 @@ inline double estimate_acceleration_distance(double initial_rate, double target_
^ ^
| |
intersection_distance distance */
-
-inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
- return(
- (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
- (4*acceleration)
- );
-}
-
-
-// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
-// The factors represent a factor of braking and must be in the range 0.0-1.0.
-
-/*
- +--------+ <- nominal_rate
- / \
- nominal_rate*entry_factor -> + \
- | + <- nominal_rate*exit_factor
- +-------------+
- time -->
-*/
-
-void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
- block->initial_rate = ceil(block->nominal_rate*entry_factor);
- block->final_rate = ceil(block->nominal_rate*exit_factor);
- int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0;
- int32_t accelerate_steps =
- ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
- int32_t decelerate_steps =
- floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute));
-
- // Calculate the size of Plateau of Nominal Rate.
- int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
-
- // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
- // have to use intersection_distance() to calculate when to abort acceleration and start braking
- // in order to reach the final_rate exactly at the end of this block.
- if (plateau_steps < 0) {
- accelerate_steps = ceil(
- intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count));
- plateau_steps = 0;
- }
-
- block->accelerate_until = accelerate_steps;
- block->decelerate_after = accelerate_steps+plateau_steps;
-}
-
-// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
-// acceleration within the allotted distance.
-inline double max_allowable_speed(double acceleration, double target_velocity, double distance) {
- return(
- sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance)
- );
+// This function gives you the point at which you must start braking (at the rate of -acceleration) if
+// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
+// a total travel of distance. This can be used to compute the intersection point between acceleration and
+// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
+static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance)
+{
+ return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
}
-// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
-// This method will calculate the junction jerk as the euclidean distance between the nominal
-// velocities of the respective blocks.
-inline double junction_jerk(block_t *before, block_t *after) {
- return(sqrt(
- pow(before->speed_x-after->speed_x, 2)+
- pow(before->speed_y-after->speed_y, 2)+
- pow(before->speed_z-after->speed_z, 2))
- );
+
+// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
+// using the acceleration within the allotted distance.
+// NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
+// in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
+// BLOCK_BUFFER_SIZE calls per planner cycle.
+static double max_allowable_speed(double acceleration, double target_velocity, double distance)
+{
+ return( sqrt(target_velocity*target_velocity-2*acceleration*distance) );
}
-// Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the
-// speed under which you cannot exceed max_jerk no matter what you do.
-double factor_for_safe_speed(block_t *block) {
- return(settings.max_jerk/block->nominal_speed);
-}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
-void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
- if(!current) { return; }
-
- double entry_factor = 1.0;
- double exit_factor;
- if (next) {
- exit_factor = next->entry_factor;
- } else {
- exit_factor = factor_for_safe_speed(current);
- }
+static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next)
+{
+ if (!current) { return; } // Cannot operate on nothing.
- // Calculate the entry_factor for the current block.
- if (previous) {
- // Reduce speed so that junction_jerk is within the maximum allowed
- double jerk = junction_jerk(previous, current);
- if (jerk > settings.max_jerk) {
- entry_factor = (settings.max_jerk/jerk);
- }
- // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
- if (entry_factor > exit_factor) {
- double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor,
- current->millimeters);
- double max_entry_factor = max_entry_speed/current->nominal_speed;
- if (max_entry_factor < entry_factor) {
- entry_factor = max_entry_factor;
- }
- }
- } else {
- entry_factor = factor_for_safe_speed(current);
- }
+ if (next) {
+ // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+ // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
+ // check for maximum allowable speed reductions to ensure maximum possible planned speed.
+ if (current->entry_speed != current->max_entry_speed) {
+
+ // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+ // for max allowable speed if block is decelerating and nominal length is false.
+ if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
+ current->entry_speed = min( current->max_entry_speed,
+ max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
+ } else {
+ current->entry_speed = current->max_entry_speed;
+ }
+ current->recalculate_flag = true;
- // Store result
- current->entry_factor = entry_factor;
+ }
+ } // Skip last block. Already initialized and set for recalculation.
}
+
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
-void planner_reverse_pass() {
- auto int8_t block_index = block_buffer_head;
+static void planner_reverse_pass()
+{
+ uint8_t block_index = block_buffer_head;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_tail) {
- block_index--;
- if(block_index < 0) {
- block_index = BLOCK_BUFFER_SIZE-1;
- }
+ block_index = prev_block_index( block_index );
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
- planner_reverse_pass_kernel(NULL, block[0], block[1]);
+ // Skip buffer tail/first block to prevent over-writing the initial entry speed.
}
+
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
-void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
- if(!current) { return; }
- if(previous) {
- // If the previous block is an acceleration block, but it is not long enough to
- // complete the full speed change within the block, we need to adjust out entry
- // speed accordingly. Remember current->entry_factor equals the exit factor of
- // the previous block.
- if(previous->entry_factor < current->entry_factor) {
- double max_entry_speed = max_allowable_speed(-settings.acceleration,
- current->nominal_speed*previous->entry_factor, previous->millimeters);
- double max_entry_factor = max_entry_speed/current->nominal_speed;
- if (max_entry_factor < current->entry_factor) {
- current->entry_factor = max_entry_factor;
+static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next)
+{
+ if(!previous) { return; } // Begin planning after buffer_tail
+
+ // If the previous block is an acceleration block, but it is not long enough to complete the
+ // full speed change within the block, we need to adjust the entry speed accordingly. Entry
+ // speeds have already been reset, maximized, and reverse planned by reverse planner.
+ // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
+ if (!previous->nominal_length_flag) {
+ if (previous->entry_speed < current->entry_speed) {
+ double entry_speed = min( current->entry_speed,
+ max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
+
+ // Check for junction speed change
+ if (current->entry_speed != entry_speed) {
+ current->entry_speed = entry_speed;
+ current->recalculate_flag = true;
}
- }
+ }
}
}
+
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass.
-void planner_forward_pass() {
- int8_t block_index = block_buffer_tail;
+static void planner_forward_pass()
+{
+ uint8_t block_index = block_buffer_tail;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_head) {
@@ -259,16 +183,68 @@ void planner_forward_pass() {
block[1] = block[2];
block[2] = &block_buffer[block_index];
planner_forward_pass_kernel(block[0],block[1],block[2]);
- block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
+ block_index = next_block_index( block_index );
}
planner_forward_pass_kernel(block[1], block[2], NULL);
}
-// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
-// entry_factor for each junction. Must be called by planner_recalculate() after
-// updating the blocks.
-void planner_recalculate_trapezoids() {
- int8_t block_index = block_buffer_tail;
+
+/* STEPPER RATE DEFINITION
+ +--------+ <- nominal_rate
+ / \
+ nominal_rate*entry_factor -> + \
+ | + <- nominal_rate*exit_factor
+ +-------------+
+ time -->
+*/
+// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
+// The factors represent a factor of braking and must be in the range 0.0-1.0.
+// This converts the planner parameters to the data required by the stepper controller.
+// NOTE: Final rates must be computed in terms of their respective blocks.
+static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor)
+{
+ block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
+ block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
+ int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2)
+ int32_t accelerate_steps =
+ ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
+ int32_t decelerate_steps =
+ floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute));
+
+ // Calculate the size of Plateau of Nominal Rate.
+ int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
+
+ // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
+ // have to use intersection_distance() to calculate when to abort acceleration and start braking
+ // in order to reach the final_rate exactly at the end of this block.
+ if (plateau_steps < 0) {
+ accelerate_steps = ceil(
+ intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count));
+ accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
+ accelerate_steps = min(accelerate_steps,block->step_event_count);
+ plateau_steps = 0;
+ }
+
+ block->accelerate_until = accelerate_steps;
+ block->decelerate_after = accelerate_steps+plateau_steps;
+}
+
+/* PLANNER SPEED DEFINITION
+ +--------+ <- current->nominal_speed
+ / \
+ current->entry_speed -> + \
+ | + <- next->entry_speed
+ +-------------+
+ time -->
+*/
+// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
+// entry_speed for each junction and the entry_speed of the next junction. Must be called by
+// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
+// compute the two adjacent trapezoids to the junction, since the junction speed corresponds
+// to exit speed and entry speed of one another.
+static void planner_recalculate_trapezoids()
+{
+ uint8_t block_index = block_buffer_tail;
block_t *current;
block_t *next = NULL;
@@ -276,71 +252,78 @@ void planner_recalculate_trapezoids() {
current = next;
next = &block_buffer[block_index];
if (current) {
- calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor);
+ // Recalculate if current block entry or exit junction speed has changed.
+ if (current->recalculate_flag || next->recalculate_flag) {
+ // NOTE: Entry and exit factors always > 0 by all previous logic operations.
+ calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
+ next->entry_speed/current->nominal_speed);
+ current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+ }
}
- block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
+ block_index = next_block_index( block_index );
}
- calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next));
+ // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+ calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
+ MINIMUM_PLANNER_SPEED/next->nominal_speed);
+ next->recalculate_flag = false;
}
// Recalculates the motion plan according to the following algorithm:
//
-// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor)
+// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed)
// so that:
-// a. The junction jerk is within the set limit
+// a. The junction speed is equal to or less than the maximum junction speed limit
// b. No speed reduction within one block requires faster deceleration than the one, true constant
// acceleration.
-// 2. Go over every block in chronological order and dial down junction speed reduction values if
-// a. The speed increase within one block would require faster accelleration than the one, true
+// 2. Go over every block in chronological order and dial down junction speed values if
+// a. The speed increase within one block would require faster acceleration than the one, true
// constant acceleration.
//
-// When these stages are complete all blocks have an entry_factor that will allow all speed changes to
-// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
-// the set limit. Finally it will:
+// When these stages are complete all blocks have an entry speed that will allow all speed changes to
+// be performed using only the one, true constant acceleration, and where no junction speed is greater
+// than the max limit. Finally it will:
//
-// 3. Recalculate trapezoids for all blocks.
+// 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids
+// with no updated junction speeds will not be recalculated and assumed ok as is.
+//
+// All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
+// off errors. Only when planned values are converted to stepper rate parameters, these are integers.
-void planner_recalculate() {
+static void planner_recalculate()
+{
planner_reverse_pass();
planner_forward_pass();
planner_recalculate_trapezoids();
}
-void plan_init() {
+void plan_init()
+{
block_buffer_head = 0;
block_buffer_tail = 0;
- plan_set_acceleration_manager_enabled(TRUE);
clear_vector(position);
+ clear_vector_double(previous_unit_vec);
+ previous_nominal_speed = 0.0;
}
-void plan_set_acceleration_manager_enabled(int enabled) {
- if ((!!acceleration_manager_enabled) != (!!enabled)) {
- st_synchronize();
- acceleration_manager_enabled = !!enabled;
- }
-}
-
-int plan_is_acceleration_manager_enabled() {
- return(acceleration_manager_enabled);
-}
-
-inline void plan_discard_current_block() {
+void plan_discard_current_block()
+{
if (block_buffer_head != block_buffer_tail) {
- block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;
+ block_buffer_tail = next_block_index( block_buffer_tail );
}
}
-inline block_t *plan_get_current_block() {
+block_t *plan_get_current_block()
+{
if (block_buffer_head == block_buffer_tail) { return(NULL); }
return(&block_buffer[block_buffer_tail]);
}
-// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
-// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
-// calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
- // The target position of the tool in absolute steps
-
+
+// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
+// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate)
+{
// Calculate target position in absolute steps
int32_t target[3];
target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
@@ -348,41 +331,48 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
// Calculate the buffer head after we push this byte
- int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;
- // If the buffer is full: good! That means we are well ahead of the robot.
- // Rest here until there is room in the buffer.
+ uint8_t next_buffer_head = next_block_index( block_buffer_head );
+ // If the buffer is full: good! That means we are well ahead of the robot.
+ // Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
+
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
+
+ // Compute direction bits for this block
+ block->direction_bits = 0;
+ if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
+ if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
+ if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
+
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
+
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; };
- double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
- double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
- double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
- block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm));
-
+ // Compute path vector in terms of absolute step target and current positions
+ double delta_mm[3];
+ delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
+ delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
+ delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
+ block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) +
+ square(delta_mm[Z_AXIS]));
+ double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
- uint32_t microseconds;
+ // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
+ // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
+ double inverse_minute;
if (!invert_feed_rate) {
- microseconds = lround((block->millimeters/feed_rate)*1000000);
+ inverse_minute = feed_rate * inverse_millimeters;
} else {
- microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate);
+ inverse_minute = 1.0 / feed_rate;
}
-
- // Calculate speed in mm/minute for each axis
- double multiplier = 60.0*1000000.0/microseconds;
- block->speed_x = delta_x_mm * multiplier;
- block->speed_y = delta_y_mm * multiplier;
- block->speed_z = delta_z_mm * multiplier;
- block->nominal_speed = block->millimeters * multiplier;
- block->nominal_rate = ceil(block->step_event_count * multiplier);
- block->entry_factor = 0.0;
+ block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
+ block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
// average travel per step event changes. For a line along one axis the travel per step event
@@ -390,34 +380,84 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
// To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
// specifically for each line to compensate for this phenomenon:
- double travel_per_step = block->millimeters/block->step_event_count;
- block->rate_delta = ceil(
- ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
- travel_per_step); // convert to: acceleration steps/min/acceleration_tick
- if (acceleration_manager_enabled) {
- // compute a preliminary conservative acceleration trapezoid
- double safe_speed_factor = factor_for_safe_speed(block);
- calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor);
- } else {
- block->initial_rate = block->nominal_rate;
- block->final_rate = block->nominal_rate;
- block->accelerate_until = 0;
- block->decelerate_after = block->step_event_count;
- block->rate_delta = 0;
- }
+ // Convert universal acceleration for direction-dependent stepper rate change parameter
+ block->rate_delta = ceil( block->step_event_count*inverse_millimeters *
+ settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
+
+ // Compute path unit vector
+ double unit_vec[3];
+
+ unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
+ unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
+ unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
+
+ // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+ // Let a circle be tangent to both previous and current path line segments, where the junction
+ // deviation is defined as the distance from the junction to the closest edge of the circle,
+ // colinear with the circle center. The circular segment joining the two paths represents the
+ // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
+ // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
+ // path width or max_jerk in the previous grbl version. This approach does not actually deviate
+ // from path, but used as a robust way to compute cornering speeds, as it takes into account the
+ // nonlinearities of both the junction angle and junction velocity.
+ double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
+
+ // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
+ if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+ // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+ // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+ double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
+ - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
+ - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+
+ // Skip and use default max junction speed for 0 degree acute junction.
+ if (cos_theta < 0.95) {
+ vmax_junction = min(previous_nominal_speed,block->nominal_speed);
+ // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
+ if (cos_theta > -0.95) {
+ // Compute maximum junction velocity based on maximum acceleration and junction deviation
+ double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
+ vmax_junction = min(vmax_junction,
+ sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
+ }
+ }
+ }
+ block->max_entry_speed = vmax_junction;
- // Compute direction bits for this block
- block->direction_bits = 0;
- if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
- if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
- if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
+ // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
+ double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
+ block->entry_speed = min(vmax_junction, v_allowable);
+
+ // Initialize planner efficiency flags
+ // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
+ // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+ // the current block and next block junction speeds are guaranteed to always be at their maximum
+ // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+ // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+ // the reverse and forward planners, the corresponding block junction speed will always be at the
+ // the maximum junction speed and may always be ignored for any speed reduction checks.
+ if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
+ else { block->nominal_length_flag = false; }
+ block->recalculate_flag = true; // Always calculate trapezoid for new block
+
+ // Update previous path unit_vector and nominal speed
+ memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
+ previous_nominal_speed = block->nominal_speed;
// Move buffer head
block_buffer_head = next_buffer_head;
- // Update position
+ // Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
-
- if (acceleration_manager_enabled) { planner_recalculate(); }
- st_wake_up();
+
+ planner_recalculate();
+ st_cycle_start();
}
+// Reset the planner position vector and planner speed
+void plan_set_current_position(double x, double y, double z) {
+ position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
+ position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
+ position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
+ previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
+ clear_vector_double(previous_unit_vec);
+}
View
42 planner.h
@@ -3,6 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
+ Copyright (c) 2011 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@@ -18,9 +19,6 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
-// This module is to be considered a sub-module of stepper.c. Please don't include
-// this file from any other module.
-
#ifndef planner_h
#define planner_h
@@ -29,27 +27,28 @@
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
// the source g-code and may never actually be reached if acceleration management is active.
typedef struct {
+
// Fields used by the bresenham algorithm for tracing the line
- uint32_t steps_x, steps_y, steps_z; // Step count along each axis
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
+ uint32_t steps_x, steps_y, steps_z; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block
- uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
-
+
// Fields used by the motion planner to manage acceleration
- double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis
double nominal_speed; // The nominal speed for this block in mm/min
+ double entry_speed; // Entry speed at previous-current junction in mm/min
+ double max_entry_speed; // Maximum allowable junction entry speed in mm/min
double millimeters; // The total travel of this block in mm
- double entry_factor; // The factor representing the change in speed at the start of this trapezoid.
- // (The end of the curren speed trapezoid is defined by the entry_factor of the
- // next block)
-
+ uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
+ uint8_t nominal_length_flag; // Planner flag for nominal speed always reached
+
// Settings for the trapezoid generator
- uint32_t initial_rate; // The jerk-adjusted step rate at start of block
- uint32_t final_rate; // The minimal rate at exit
+ uint32_t initial_rate; // The step rate at start of block
+ uint32_t final_rate; // The step rate at end of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint32_t accelerate_until; // The index of the step event on which to stop acceleration
uint32_t decelerate_after; // The index of the step event on which to start decelerating
-
+ uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
+
} block_t;
// Initialize the motion plan subsystem
@@ -58,19 +57,16 @@ void plan_init();
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
-void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate);
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks.
-inline void plan_discard_current_block();
+void plan_discard_current_block();
// Gets the current block. Returns NULL if buffer empty
-inline block_t *plan_get_current_block();
-
-// Enables or disables acceleration-management for upcoming blocks
-void plan_set_acceleration_manager_enabled(int enabled);
+block_t *plan_get_current_block();
-// Is acceleration-management currently enabled?
-int plan_is_acceleration_manager_enabled();
+// Reset the position vector
+void plan_set_current_position(double x, double y, double z);
-#endif
+#endif
View
104 print.c
@@ -0,0 +1,104 @@
+/*
+ print.c - Functions for formatting output strings
+ Part of Grbl
+
+ Copyright (c) 2009-2011 Simen Svale Skogsrud
+ Copyright (c) 2011 Sungeun K. Jeon
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/* This code was initially inspired by the wiring_serial module by David A. Mellis which
+ used to be a part of the Arduino project. */
+
+
+#include <math.h>
+#include <avr/pgmspace.h>
+#include "serial.h"
+
+#ifndef DECIMAL_PLACES
+#define DECIMAL_PLACES 3
+#define DECIMAL_MULTIPLIER 10*10*10
+#endif
+
+void printString(const char *s)
+{
+ while (*s)
+ serial_write(*s++);
+}
+
+// Print a string stored in PGM-memory
+void printPgmString(const char *s)
+{
+ char c;
+ while ((c = pgm_read_byte_near(s++)))
+ serial_write(c);
+}
+
+void printIntegerInBase(unsigned long n, unsigned long base)
+{
+ unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars.
+ unsigned long i = 0;
+
+ if (n == 0) {
+ serial_write('0');
+ return;
+ }
+
+ while (n > 0) {
+ buf[i++] = n % base;
+ n /= base;
+ }
+
+ for (; i > 0; i--)
+ serial_write(buf[i - 1] < 10 ?
+ '0' + buf[i - 1] :
+ 'A' + buf[i - 1] - 10);
+}
+
+void printInteger(long n)
+{
+ if (n < 0) {
+ serial_write('-');
+ n = -n;
+ }
+
+ printIntegerInBase(n, 10);
+}
+
+// A very simple
+void printFloat(double n)
+{
+ if (n < 0) {
+ serial_write('-');
+ n = -n;
+ }
+ n += 0.5/DECIMAL_MULTIPLIER; // Add rounding factor
+
+ long integer_part;
+ integer_part = (int)n;
+ printIntegerInBase(integer_part,10);
+
+ serial_write('.');
+
+ n -= integer_part;
+ int decimals = DECIMAL_PLACES;
+ uint8_t decimal_part;
+ while(decimals-- > 0) {
+ n *= 10;
+ decimal_part = (int) n;
+ serial_write('0'+decimal_part);
+ n -= decimal_part;
+ }
+}
View
37 print.h
@@ -0,0 +1,37 @@
+/*
+ print.h - Functions for formatting output strings
+ Part of Grbl
+
+ Copyright (c) 2009-2011 Simen Svale Skogsrud
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/* This code was initially inspired by the wiring_serial module by David A. Mellis which
+ used to be a part of the Arduino project. */
+
+#ifndef print_h
+#define print_h
+
+void printString(const char *s);
+
+void printPgmString(const char *s);
+
+void printInteger(long n);
+
+void printIntegerInBase(unsigned long n, unsigned long base);
+
+void printFloat(double n);
+
+#endif
View
122 protocol.c
@@ -0,0 +1,122 @@
+/*
+ protocol.c - the serial protocol master control unit
+ Part of Grbl
+
+ Copyright (c) 2009-2011 Simen Svale Skogsrud
+ Copyright (c) 2011 Sungeun K. Jeon
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <avr/io.h>
+#include "protocol.h"
+#include "gcode.h"
+#include "serial.h"
+#include "print.h"
+#include "settings.h"
+#include "config.h"
+#include <math.h>
+#include "nuts_bolts.h"
+#include <avr/pgmspace.h>
+#define LINE_BUFFER_SIZE 50
+
+static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated.
+static uint8_t char_counter; // Last character counter in line variable.
+static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters.
+
+static void status_message(int status_code)
+{
+ if (status_code == 0) {
+ printPgmString(PSTR("ok\r\n"));
+ } else {
+ printPgmString(PSTR("error: "));
+ switch(status_code) {
+ case STATUS_BAD_NUMBER_FORMAT:
+ printPgmString(PSTR("Bad number format\r\n")); break;
+ case STATUS_EXPECTED_COMMAND_LETTER:
+ printPgmString(PSTR("Expected command letter\r\n")); break;
+ case STATUS_UNSUPPORTED_STATEMENT:
+ printPgmString(PSTR("Unsupported statement\r\n")); break;
+ case STATUS_FLOATING_POINT_ERROR:
+ printPgmString(PSTR("Floating point error\r\n")); break;
+ default:
+ printInteger(status_code);
+ printPgmString(PSTR("\r\n"));
+ }
+ }
+}
+
+void protocol_init()
+{
+ char_counter = 0; // Reset line input
+ iscomment = false;
+ printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
+ printPgmString(PSTR("\r\n"));
+}
+
+// Executes one line of input according to protocol
+uint8_t protocol_execute_line(char *line)
+{
+ if(line[0] == '$') {
+ return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module
+ } else {
+ return(gc_execute_line(line)); // Everything else is gcode
+ }
+}
+
+
+// Process one line of incoming serial data. Remove unneeded characters and capitalize.
+void protocol_process()
+{
+ char c;
+ while((c = serial_read()) != SERIAL_NO_DATA)
+ {
+ if ((c == '\n') || (c == '\r')) { // End of line reached
+ if (char_counter > 0) {// Line is complete. Then execute!
+ line[char_counter] = 0; // Terminate string
+ status_message(protocol_execute_line(line));
+ } else {
+ // Empty or comment line. Skip block.
+ status_message(STATUS_OK); // Send status message for syncing purposes.
+ }
+ char_counter = 0; // Reset line buffer index
+ iscomment = false; // Reset comment flag
+ } else {
+ if (iscomment) {
+ // Throw away all comment characters
+ if (c == ')') {
+ // End of comment. Resume line.
+ iscomment = false;
+ }
+ } else {
+ if (c <= ' ') {
+ // Throw away whitepace and control characters
+ } else if (c == '/') {
+ // Disable block delete and throw away character
+ // To enable block delete, uncomment following line. Will ignore until EOL.
+ // iscomment = true;
+ } else if (c == '(') {
+ // Enable comments flag and ignore all characters until ')' or EOL.
+ iscomment = true;
+ } else if (char_counter >= LINE_BUFFER_SIZE-1) {
+ // Throw away any characters beyond the end of the line buffer
+ } else if (c >= 'a' && c <= 'z') { // Upcase lowercase
+ line[char_counter++] = c-'a'+'A';
+ } else {
+ line[char_counter++] = c;
+ }
+ }
+ }
+ }
+}
View
40 protocol.h
@@ -0,0 +1,40 @@
+/*
+ protocol.h - the serial protocol master control unit
+ Part of Grbl
+
+ Copyright (c) 2009-2011 Simen Svale Skogsrud
+ Copyright (c) 2011 Sungeun K. Jeon
+
+ Grbl is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef protocol_h
+#define protocol_h
+
+#define STATUS_OK 0
+#define STATUS_BAD_NUMBER_FORMAT 1
+#define STATUS_EXPECTED_COMMAND_LETTER 2
+#define STATUS_UNSUPPORTED_STATEMENT 3
+#define STATUS_FLOATING_POINT_ERROR 4
+
+// Initialize the serial protocol
+void protocol_init();
+
+// Read command lines from the serial port and execute them as they
+// come in. Blocks until the serial buffer is emptied.
+void protocol_process();
+
+// Executes one line of input according to protocol
+uint8_t protocol_execute_line(char *line);
+
+#endif
View
16 readme.textile
@@ -4,12 +4,20 @@ Grbl is a no-compromise, high performance, low cost alternative to parallel-port
The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to maintain more than 30kHz of stable, jitter free control pulses.
-It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported - but no support for tool offsets, functions or variables as these are apocryphal and fell into disuse after humans left G-code authoring to machines some time in the 80s.
+It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported, as well as, other basic functional g-code commands. Functions and variables are not currently supported, but may be included in future releases in a form of a pre-processor.
-Grbl includes full acceleration management with look ahead. That means the controller will look up to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering.
+Grbl includes full acceleration management with look ahead. That means the controller will look up to 16 to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering.
-*Important note for Atmega 168 users:* Grbl used to be compatible with both the older Ardunios running atmega 168 and the newer with 328p. This had to go, as I was unable to fit the acceleration management into the 16k code space of the 168. If you want to run Grbl on an 168 I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51.
+*Changelog for v0.7 from v0.6:*
+ - Significantly improved and optimized planner re-factoring.
+ - New robust cornering algorithm, enabling smoother and faster motions.
+ - Arc acceleration planning enabled by efficient vector transformation implementation.
+ - Stepper subsystem re-factoring to help remove some motion issues from pre-v0.7 builds.
+ - Increased dwell times.
+ - G92 Coordinate offset support.
+ - (Beta) Limit switch and homing cycle support.
+ - Many other bug fixes and efficiency improvements.
-*Note for users upgrading from 0.51 to 0.6:* The new version has new and improved default pin-out. If nothing works when you upgrade, that is because the pulse trains are coming from the wrong pins. This is a simple matter of editing config.h – the whole legacy pin assignment is there for you to uncomment.
+*Important note for Atmega 168 users:* Going forward, support for Atmega 168 will be dropped due to its limited memory and speed. However, legacy Grbl v0.51 "in the branch called 'v0_51' is still available for use.
_The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_
View
33 script/simple_stream.py
@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+"""\
+Simple g-code streaming script for grbl
+"""
+
+import serial
+import time
+
+# Open grbl serial port
+s = serial.Serial('/dev/tty.usbmodem1811',9600)
+
+# Open g-code file
+f = open('grbl.gcode','r');
+
+# Wake up grbl
+s.write("\r\n\r\n")
+time.sleep(2) # Wait for grbl to initialize
+s.flushInput() # Flush startup text in serial input
+
+# Stream g-code to grbl
+for line in f:
+ l = line.strip() # Strip all EOL characters for consistency
+ print 'Sending: ' + l,
+ s.write(l + '\n') # Send g-code block to grbl
+ grbl_out = s.readline() # Wait for grbl response with carriage return
+ print ' : ' + grbl_out.strip()
+
+# Wait here until grbl is finished to close serial port and file.
+raw_input(" Press <Enter> to exit and disable grbl.")
+
+# Close file and serial port
+f.close()
+s.close()
View
125 serial.c
@@ -0,0 +1,125 @@