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

Extended position reporting with both home and work coordinates. Home position now retained after reset. Other minor changes/fixes.

Browse Source 
- Grbl now tracks both home and work (G92) coordinate systems and does
live updates when G92 is called.
- Rudimentary home and work position status reporting. Works but still
under major construction.
- Updated the main streaming script. Has a disabled periodic timer for
querying status reports, disabled only because the Python timer doesn't
consistently restart after the script exits. Add here only for user
testing.
- Fixed a bug to prevent an endless serial_write loop during status
reports.
- Refactored the planner variables to make it more clear what they are
and make it easier for clear them.
This commit is contained in:
Sonny Jeon 2012-01-10 08:34:48 -07:00
parent 6f27e2cdb1
commit 89a3b37e02
9 changed files with 106 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
limits.c
View File

@ -27,7 +27,8 @@
#include "motion_control.h" #include "motion_control.h"
#include "planner.h" #include "planner.h"
// TODO: Deprecated. Need to update for new version. // TODO: Deprecated. Need to update for new version. Sys.position now tracks position relative
// to the home position. Limits should update this vector directly.
void limits_init() { void limits_init() {
LIMIT_DDR &= ~(LIMIT_MASK); LIMIT_DDR &= ~(LIMIT_MASK);

12
main.c
View File

@ -43,6 +43,7 @@ int main(void)
serial_init(BAUD_RATE); // Setup serial baud rate and interrupts serial_init(BAUD_RATE); // Setup serial baud rate and interrupts
st_init(); // Setup stepper pins and interrupt timers st_init(); // Setup stepper pins and interrupt timers
memset(&sys, 0, sizeof(sys)); // Clear all system variables
sys.abort = true; // Set abort to complete initialization sys.abort = true; // Set abort to complete initialization
while(1) { while(1) {
@ -52,9 +53,20 @@ int main(void)
// reset to finish the initialization process. // reset to finish the initialization process.
if (sys.abort) { if (sys.abort) {
// Retain last known machine position. If the system abort occurred while in motion, machine
// position is not guaranteed, since a hard stop can cause the steppers to lose steps. Always
// perform a feedhold before an abort, if maintaining accurate machine position is required.
int32_t last_position[3];
memcpy(last_position, sys.position, sizeof(sys.position)); // last_position[] = sys.position[]
// Clear all system variables // Clear all system variables
memset(&sys, 0, sizeof(sys)); memset(&sys, 0, sizeof(sys));
// Update last known machine position. Set the post-abort work position as the origin [0,0,0],
// which corresponds to the g-code parser and planner positions after re-initialization.
memcpy(sys.position, last_position, sizeof(last_position)); // sys.position[] = last_position[]
memcpy(sys.coord_offset, last_position, sizeof(last_position)); // sys.coord_offset[] = last_position[]
// Reset system. // Reset system.
serial_reset_read_buffer(); // Clear serial read buffer serial_reset_read_buffer(); // Clear serial read buffer
settings_init(); // Load grbl settings from EEPROM settings_init(); // Load grbl settings from EEPROM

5
motion_control.c
View File

@ -48,7 +48,10 @@ void mc_line(double x, double y, double z, double feed_rate, uint8_t invert_feed
{ {
// TODO: Backlash compensation may be installed here. Only need direction info to track when // TODO: Backlash compensation may be installed here. Only need direction info to track when
// to insert a backlash line motion(s) before the intended line motion. Requires its own // to insert a backlash line motion(s) before the intended line motion. Requires its own
// plan_check_full_buffer() and check for system abort loop. // plan_check_full_buffer() and check for system abort loop. Also for position reporting
// backlash steps will need to be also tracked. Not sure what the best strategy is for this,
// i.e. keep the planner independent and do the computations in the status reporting, or let
// the planner handle the position corrections. The latter may get complicated.
// If the buffer is full: good! That means we are well ahead of the robot. // If the buffer is full: good! That means we are well ahead of the robot.
// Remain in this loop until there is room in the buffer. // Remain in this loop until there is room in the buffer.

7
nuts_bolts.h
View File

@ -68,9 +68,10 @@ typedef struct {
uint8_t feed_hold; // Feed hold flag. Held true during feed hold. Released when ready to resume. uint8_t feed_hold; // Feed hold flag. Held true during feed hold. Released when ready to resume.
uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings. uint8_t auto_start; // Planner auto-start flag. Toggled off during feed hold. Defaulted by settings.
int32_t position[3]; // Real-time machine position vector in steps. This may need to be a volatile int32_t position[3]; // Real-time machine (aka home) position vector in steps.
// variable, if problems arise. Subject to change. Need to add coordinate offset // NOTE: This may need to be a volatile variable, if problems arise.
// functionality to correctly track part zero and machine zero. int32_t coord_offset[3]; // Retains the G92 coordinate offset (work coordinates) relative to
// machine zero in steps.
volatile uint8_t cycle_start; // Cycle start flag. Set by stepper subsystem or main program. volatile uint8_t cycle_start; // Cycle start flag. Set by stepper subsystem or main program.
volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks. volatile uint8_t execute; // Global system runtime executor bitflag variable. See EXEC bitmasks.

91
planner.c
View File

@ -41,11 +41,15 @@ static volatile uint8_t block_buffer_head; // Index of the next block to b
static volatile uint8_t block_buffer_tail; // Index of the block to process now static volatile uint8_t block_buffer_tail; // Index of the block to process now
static uint8_t next_buffer_head; // Index of the next buffer head static uint8_t next_buffer_head; // Index of the next buffer head
static int32_t position[3]; // The planner position of the tool in absolute steps // Define planner variables
// static int32_t coord_offset[3]; // Current coordinate offset from machine zero in absolute steps typedef struct {
static double previous_unit_vec[3]; // Unit vector of previous path line segment int32_t position[3]; // The planner position of the tool in absolute steps. Kept separate
static double previous_nominal_speed; // Nominal speed of previous path line segment // from g-code position for movements requiring multiple line motions,
// i.e. arcs, canned cycles, and backlash compensation.
double previous_unit_vec[3]; // Unit vector of previous path line segment
double previous_nominal_speed; // Nominal speed of previous path line segment
} planner_t;
static planner_t pl;
// Returns the index of the next block in the ring buffer // Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
@ -305,10 +309,7 @@ void plan_reset_buffer()
void plan_init() void plan_init()
{ {
plan_reset_buffer(); plan_reset_buffer();
clear_vector(position); memset(&pl, 0, sizeof(pl)); // Clear planner struct
// clear_vector(coord_offset);
clear_vector_double(previous_unit_vec);
previous_nominal_speed = 0.0;
} }
void plan_discard_current_block() void plan_discard_current_block()
@ -357,14 +358,14 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
// Compute direction bits for this block // Compute direction bits for this block
block->direction_bits = 0; block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); } if (target[X_AXIS] < pl.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[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); } if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
// Number of steps for each axis // Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
// Bail if this is a zero-length block // Bail if this is a zero-length block
@ -372,9 +373,9 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
// Compute path vector in terms of absolute step target and current positions // Compute path vector in terms of absolute step target and current positions
double delta_mm[3]; double delta_mm[3];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; delta_mm[X_AXIS] = (target[X_AXIS]-pl.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[Y_AXIS] = (target[Y_AXIS]-pl.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]; delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) +
square(delta_mm[Z_AXIS])); square(delta_mm[Z_AXIS]));
double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
@ -419,16 +420,16 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed 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. // 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)) { if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // 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. // 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] double cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction. // Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) { if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed); vmax_junction = min(pl.previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) { if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation // Compute maximum junction velocity based on maximum acceleration and junction deviation
@ -457,15 +458,15 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
block->recalculate_flag = true; // Always calculate trapezoid for new block block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed
memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
previous_nominal_speed = block->nominal_speed; pl.previous_nominal_speed = block->nominal_speed;
// Update buffer head and next buffer head indices // Update buffer head and next buffer head indices
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head); next_buffer_head = next_block_index(block_buffer_head);
// Update position // Update planner position
memcpy(position, target, sizeof(target)); // position[] = target[] memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
planner_recalculate(); planner_recalculate();
} }
@ -473,22 +474,26 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
// Reset the planner position vector and planner speed // Reset the planner position vector and planner speed
void plan_set_current_position(double x, double y, double z) void plan_set_current_position(double x, double y, double z)
{ {
// Track the position offset from the initial position // To correlate status reporting work position correctly, the planner must force the steppers to
// TODO: Need to make sure coord_offset is robust and/or needed. Can be used for a soft reset, // empty the block buffer and synchronize with the planner, as the real-time machine position and
// where the machine position is retained after a system abort/reset. However, this is not // the planner position at the end of the buffer are different. This will only be called with a
// correlated to the actual machine position after a soft reset and may not be needed. This could // G92 is executed, which typically is used only at the beginning of a g-code program.
// be left to a user interface to maintain. // TODO: Find a robust way to avoid a planner synchronize, but may require a bit of ingenuity.
// coord_offset[X_AXIS] += position[X_AXIS]; plan_synchronize();
// coord_offset[Y_AXIS] += position[Y_AXIS];
// coord_offset[Z_AXIS] += position[Z_AXIS]; // Update the system coordinate offsets from machine zero
position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); sys.coord_offset[X_AXIS] += pl.position[X_AXIS];
position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); sys.coord_offset[Y_AXIS] += pl.position[Y_AXIS];
position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); sys.coord_offset[Z_AXIS] += pl.position[Z_AXIS];
// coord_offset[X_AXIS] -= position[X_AXIS];
// coord_offset[Y_AXIS] -= position[Y_AXIS]; memset(&pl, 0, sizeof(pl)); // Clear planner variables. Assume start from rest.
// coord_offset[Z_AXIS] -= position[Z_AXIS];
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. pl.position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); // Update planner position
clear_vector_double(previous_unit_vec); pl.position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
pl.position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
sys.coord_offset[X_AXIS] -= pl.position[X_AXIS];
sys.coord_offset[Y_AXIS] -= pl.position[Y_AXIS];
sys.coord_offset[Z_AXIS] -= pl.position[Z_AXIS];
} }
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.

40
protocol.c
View File

@ -72,25 +72,31 @@ void protocol_status_report()
// information, i.e. 'x0.23,y120.4,z2.4'. This is necessary as it minimizes the computational // information, i.e. 'x0.23,y120.4,z2.4'. This is necessary as it minimizes the computational
// overhead and allows grbl to keep running smoothly, especially with g-code programs with fast, // overhead and allows grbl to keep running smoothly, especially with g-code programs with fast,
// short line segments and interface setups that require real-time status reports (5-20Hz). // short line segments and interface setups that require real-time status reports (5-20Hz).
// Additionally, during an abort, the steppers are immediately stopped regardless of what they
// are doing. If they are moving, the abort stop can cause grbl to lose steps. However, if a feed
// hold is performed before a system abort, the steppers will steadily decelerate at the max
// acceleration rate, hence the stopped machine position will be maintained and correct.
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
// Bare-bones status report. Provides real-time machine position relative to the initialization // the system power on location (0,0,0) and work coordinate position, updatable by the G92 command.
// or system reset location (0,0,0), not a home position. This section is under construction and // The following are still needed: user setting of output units (mm|inch), compressed (non-human
// the following are needed: coordinate offsets/updating of machine position relative to home, work // readable) data for interfaces?, save last known position in EEPROM?, code optimizations, solidify
// coordinate position?, user setting of output units (mm|inch), compressed (non-human readable) // the reporting schemes, move to a separate .c file for easy user accessibility, and setting the
// data for interfaces?, save last known position in EEPROM? // home position by the user (likely through '$' setting interface).
// Successfully tested at a query rate of 10-20Hz while running a gauntlet of programs at various
// speeds.
int32_t print_position[3];
memcpy(print_position,sys.position,sizeof(sys.position));
#if REPORT_INCH_MODE #if REPORT_INCH_MODE
printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH)); printString("MPos: x"); printFloat(print_position[X_AXIS]/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH));
printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH)); printString(",y"); printFloat(print_position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH));
printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH)); printString(",z"); printFloat(print_position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH));
printString(" WPos: x"); printFloat((print_position[X_AXIS]-sys.coord_offset[X_AXIS])/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH));
printString(",y"); printFloat((print_position[Y_AXIS]-sys.coord_offset[Y_AXIS])/(settings.steps_per_mm[Y_AXIS]*MM_PER_INCH));
printString(",z"); printFloat((print_position[Z_AXIS]-sys.coord_offset[Z_AXIS])/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH));
#else #else
printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS])); printString("MPos: x"); printFloat(print_position[X_AXIS]/(settings.steps_per_mm[X_AXIS]));
printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS])); printString(",y"); printFloat(print_position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]));
printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS])); printString(",z"); printFloat(print_position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]));
printString(" WPos: x"); printFloat((print_position[X_AXIS]-sys.coord_offset[X_AXIS])/(settings.steps_per_mm[X_AXIS]));
printString(",y"); printFloat((print_position[Y_AXIS]-sys.coord_offset[Y_AXIS])/(settings.steps_per_mm[Y_AXIS]));
printString(",z"); printFloat((print_position[Z_AXIS]-sys.coord_offset[Z_AXIS])/(settings.steps_per_mm[Z_AXIS]));
#endif #endif
printString("\r\n"); printString("\r\n");
} }
@ -128,8 +134,8 @@ void protocol_execute_runtime()
// Execute and serial print status // Execute and serial print status
if (rt_exec & EXEC_STATUS_REPORT) { if (rt_exec & EXEC_STATUS_REPORT) {
bit_false(sys.execute,EXEC_STATUS_REPORT);
protocol_status_report(); protocol_status_report();
bit_false(sys.execute,EXEC_STATUS_REPORT);
} }
// Initiate stepper feed hold // Initiate stepper feed hold

10
script/stream.py
View File

@ -11,7 +11,7 @@ buffer layer to prevent buffer starvation.
TODO: - Add runtime command capabilities TODO: - Add runtime command capabilities
Version: SKJ.20120104 Version: SKJ.20120110
""" """
import serial import serial
@ -32,6 +32,13 @@ parser.add_argument('-q','--quiet',action='store_true', default=False,
help='suppress output text') help='suppress output text')
args = parser.parse_args() args = parser.parse_args()
# Periodic timer to query for status reports
# TODO: Need to track down why this doesn't restart consistently before a release.
# def periodic():
# s.write('?')
# t = threading.Timer(0.1, periodic) # In seconds
# t.start()
# Initialize # Initialize
s = serial.Serial(args.device_file,9600) s = serial.Serial(args.device_file,9600)
f = args.gcode_file f = args.gcode_file
@ -51,6 +58,7 @@ print "Streaming ", args.gcode_file.name, " to ", args.device_file
l_count = 0 l_count = 0
g_count = 0 g_count = 0
c_line = [] c_line = []
# periodic() # Start status report periodic timer
for line in f: for line in f:
l_count += 1 # Iterate line counter l_count += 1 # Iterate line counter
# l_block = re.sub('\s|\(.*?\)','',line).upper() # Strip comments/spaces/new line and capitalize # l_block = re.sub('\s|\(.*?\)','',line).upper() # Strip comments/spaces/new line and capitalize

3
serial.c
View File

@ -71,8 +71,7 @@ void serial_write(uint8_t data) {
// Wait until there is space in the buffer // Wait until there is space in the buffer
while (next_head == tx_buffer_tail) { while (next_head == tx_buffer_tail) {
protocol_execute_runtime(); // Check for any run-time commands if (sys.execute & EXEC_RESET) { return; } // Only check for abort to avoid an endless loop.
if (sys.abort) { return; } // Bail, if system abort.
} }
// Store data and advance head // Store data and advance head

2
stepper.c
View File

@ -139,7 +139,7 @@ static uint8_t iterate_trapezoid_cycle_counter()
// interrupt doing its thing, not that big of a deal, but the latter cause is unknown and worrisome. Need // interrupt doing its thing, not that big of a deal, but the latter cause is unknown and worrisome. Need
// to track down what is causing this problem. Functionally, this shouldn't cause any noticeable issues // to track down what is causing this problem. Functionally, this shouldn't cause any noticeable issues
// as long as stepper drivers have a pulse minimum of 1usec or so (Pololu and any Allegro IC are ok). // as long as stepper drivers have a pulse minimum of 1usec or so (Pololu and any Allegro IC are ok).
// This seems to be an inherent issue that dates all the way back to Simen's v0.6b. // ** This seems to be an inherent issue that dates all the way back to Simen's v0.6b or earlier. **
ISR(TIMER1_COMPA_vect,ISR_NOBLOCK) ISR(TIMER1_COMPA_vect,ISR_NOBLOCK)
{ {