kopia lustrzana https://github.com/gnea/grbl
Extended position reporting with both home and work coordinates. Home position now retained after reset. Other minor changes/fixes.
- 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.pull/1/head
Sonny Jeon
rodzic
6f27e2cdb1
commit
89a3b37e02
3
limits.c
3
limits.c
|
|
@ -27,7 +27,8 @@
|
|||
#include "motion_control.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() {
|
||||
LIMIT_DDR &= ~(LIMIT_MASK);
|
||||
|
|
|
|||
16
main.c
16
main.c
|
|
@ -43,6 +43,7 @@ int main(void)
|
|||
serial_init(BAUD_RATE); // Setup serial baud rate and interrupts
|
||||
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
|
||||
|
||||
while(1) {
|
||||
|
|
@ -51,10 +52,21 @@ int main(void)
|
|||
// Once here, it is safe to re-initialize the system. At startup, the system will automatically
|
||||
// reset to finish the initialization process.
|
||||
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
|
||||
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.
|
||||
serial_reset_read_buffer(); // Clear serial read buffer
|
||||
settings_init(); // Load grbl settings from EEPROM
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// 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.
|
||||
// Remain in this loop until there is room in the buffer.
|
||||
|
|
|
|||
|
|
@ -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 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
|
||||
// variable, if problems arise. Subject to change. Need to add coordinate offset
|
||||
// functionality to correctly track part zero and machine zero.
|
||||
int32_t position[3]; // Real-time machine (aka home) position vector in steps.
|
||||
// NOTE: This may need to be a volatile variable, if problems arise.
|
||||
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 execute; // Global system runtime executor bitflag variable. See EXEC bitmasks.
|
||||
|
|
|
|||
91
planner.c
91
planner.c
|
|
@ -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 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
|
||||
// static int32_t coord_offset[3]; // Current coordinate offset from machine zero 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
|
||||
|
||||
// Define planner variables
|
||||
typedef struct {
|
||||
int32_t position[3]; // The planner position of the tool in absolute steps. Kept separate
|
||||
// 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
|
||||
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
|
||||
|
|
@ -305,10 +309,7 @@ void plan_reset_buffer()
|
|||
void plan_init()
|
||||
{
|
||||
plan_reset_buffer();
|
||||
clear_vector(position);
|
||||
// clear_vector(coord_offset);
|
||||
clear_vector_double(previous_unit_vec);
|
||||
previous_nominal_speed = 0.0;
|
||||
memset(&pl, 0, sizeof(pl)); // Clear planner struct
|
||||
}
|
||||
|
||||
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
|
||||
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); }
|
||||
if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
|
||||
if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
|
||||
if (target[Z_AXIS] < pl.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->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
|
||||
block->steps_y = labs(target[Y_AXIS]-pl.position[Y_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));
|
||||
|
||||
// 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
|
||||
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];
|
||||
delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_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]-pl.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
|
||||
|
|
@ -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
|
||||
|
||||
// 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)
|
||||
// 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] ;
|
||||
double cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
|
||||
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
|
||||
- pl.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);
|
||||
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.
|
||||
if (cos_theta > -0.95) {
|
||||
// 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
|
||||
|
||||
// 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;
|
||||
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
|
||||
pl.previous_nominal_speed = block->nominal_speed;
|
||||
|
||||
// Update buffer head and next buffer head indices
|
||||
block_buffer_head = next_buffer_head;
|
||||
next_buffer_head = next_block_index(block_buffer_head);
|
||||
|
||||
// Update position
|
||||
memcpy(position, target, sizeof(target)); // position[] = target[]
|
||||
// Update planner position
|
||||
memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
|
||||
|
||||
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
|
||||
void plan_set_current_position(double x, double y, double z)
|
||||
{
|
||||
// Track the position offset from the initial position
|
||||
// TODO: Need to make sure coord_offset is robust and/or needed. Can be used for a soft reset,
|
||||
// where the machine position is retained after a system abort/reset. However, this is not
|
||||
// correlated to the actual machine position after a soft reset and may not be needed. This could
|
||||
// be left to a user interface to maintain.
|
||||
// coord_offset[X_AXIS] += position[X_AXIS];
|
||||
// coord_offset[Y_AXIS] += position[Y_AXIS];
|
||||
// coord_offset[Z_AXIS] += position[Z_AXIS];
|
||||
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]);
|
||||
// coord_offset[X_AXIS] -= position[X_AXIS];
|
||||
// coord_offset[Y_AXIS] -= position[Y_AXIS];
|
||||
// coord_offset[Z_AXIS] -= position[Z_AXIS];
|
||||
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
|
||||
clear_vector_double(previous_unit_vec);
|
||||
// To correlate status reporting work position correctly, the planner must force the steppers to
|
||||
// empty the block buffer and synchronize with the planner, as the real-time machine position and
|
||||
// the planner position at the end of the buffer are different. This will only be called with a
|
||||
// G92 is executed, which typically is used only at the beginning of a g-code program.
|
||||
// TODO: Find a robust way to avoid a planner synchronize, but may require a bit of ingenuity.
|
||||
plan_synchronize();
|
||||
|
||||
// Update the system coordinate offsets from machine zero
|
||||
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];
|
||||
|
||||
memset(&pl, 0, sizeof(pl)); // Clear planner variables. Assume start from rest.
|
||||
|
||||
pl.position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); // Update planner position
|
||||
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.
|
||||
|
|
|
|||
40
protocol.c
40
protocol.c
|
|
@ -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
|
||||
// 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).
|
||||
// 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.
|
||||
|
||||
|
||||
// Bare-bones status report. Provides real-time machine position relative to the initialization
|
||||
// or system reset location (0,0,0), not a home position. This section is under construction and
|
||||
// the following are needed: coordinate offsets/updating of machine position relative to home, work
|
||||
// coordinate position?, user setting of output units (mm|inch), compressed (non-human readable)
|
||||
// data for interfaces?, save last known position in EEPROM?
|
||||
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
|
||||
// the system power on location (0,0,0) and work coordinate position, updatable by the G92 command.
|
||||
// The following are still needed: user setting of output units (mm|inch), compressed (non-human
|
||||
// readable) data for interfaces?, save last known position in EEPROM?, code optimizations, solidify
|
||||
// the reporting schemes, move to a separate .c file for easy user accessibility, and setting the
|
||||
// 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
|
||||
printString("x"); printFloat(sys.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(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]*MM_PER_INCH));
|
||||
printString("MPos: x"); printFloat(print_position[X_AXIS]/(settings.steps_per_mm[X_AXIS]*MM_PER_INCH));
|
||||
printString(",y"); printFloat(print_position[Y_AXIS]/(settings.steps_per_mm[Y_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
|
||||
printString("x"); printFloat(sys.position[X_AXIS]/(settings.steps_per_mm[X_AXIS]));
|
||||
printString(",y"); printFloat(sys.position[Y_AXIS]/(settings.steps_per_mm[Y_AXIS]));
|
||||
printString(",z"); printFloat(sys.position[Z_AXIS]/(settings.steps_per_mm[Z_AXIS]));
|
||||
printString("MPos: x"); printFloat(print_position[X_AXIS]/(settings.steps_per_mm[X_AXIS]));
|
||||
printString(",y"); printFloat(print_position[Y_AXIS]/(settings.steps_per_mm[Y_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
|
||||
printString("\r\n");
|
||||
}
|
||||
|
|
@ -128,8 +134,8 @@ void protocol_execute_runtime()
|
|||
|
||||
// Execute and serial print status
|
||||
if (rt_exec & EXEC_STATUS_REPORT) {
|
||||
bit_false(sys.execute,EXEC_STATUS_REPORT);
|
||||
protocol_status_report();
|
||||
bit_false(sys.execute,EXEC_STATUS_REPORT);
|
||||
}
|
||||
|
||||
// Initiate stepper feed hold
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ buffer layer to prevent buffer starvation.
|
|||
|
||||
TODO: - Add runtime command capabilities
|
||||
|
||||
Version: SKJ.20120104
|
||||
Version: SKJ.20120110
|
||||
"""
|
||||
|
||||
import serial
|
||||
|
|
@ -32,6 +32,13 @@ parser.add_argument('-q','--quiet',action='store_true', default=False,
|
|||
help='suppress output text')
|
||||
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
|
||||
s = serial.Serial(args.device_file,9600)
|
||||
f = args.gcode_file
|
||||
|
|
@ -51,6 +58,7 @@ print "Streaming ", args.gcode_file.name, " to ", args.device_file
|
|||
l_count = 0
|
||||
g_count = 0
|
||||
c_line = []
|
||||
# periodic() # Start status report periodic timer
|
||||
for line in f:
|
||||
l_count += 1 # Iterate line counter
|
||||
# l_block = re.sub('\s|\(.*?\)','',line).upper() # Strip comments/spaces/new line and capitalize
|
||||
|
|
|
|||
3
serial.c
3
serial.c
|
|
@ -71,8 +71,7 @@ void serial_write(uint8_t data) {
|
|||
|
||||
// Wait until there is space in the buffer
|
||||
while (next_head == tx_buffer_tail) {
|
||||
protocol_execute_runtime(); // Check for any run-time commands
|
||||
if (sys.abort) { return; } // Bail, if system abort.
|
||||
if (sys.execute & EXEC_RESET) { return; } // Only check for abort to avoid an endless loop.
|
||||
}
|
||||
|
||||
// Store data and advance head
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// 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).
|
||||
// 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)
|
||||
{
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue