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

Added grbl planner Matlab simulator for test reference. Updated line number compile-time option.

Browse Source 
- Added a grbl planner simulation tool that was written in Matlab and
Python. It was used to visualize the inner workings of the planner as a
program is streamed to it. The simulation assumes that the planner
buffer is empty, then filled, and kept filled. This is mainly for users
to see how the planner works.

- Updated some of the compile-time ifdefs when enabling line numbers.
The leaving the un-used line numbers in the function calls eats a
non-neglible amount of flash memory. So the new if-defs remove them.
This commit is contained in:
Sonny Jeon 2014-02-26 12:10:07 -07:00
parent 1fd45791a5
commit 8f8d8e2887
11 changed files with 5549 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config.h
View File

@ -86,12 +86,12 @@
// Allows GRBL to tranck and report gcode line numbers. Enabling this means that the planning buffer
// goes from 18 or 16 to make room for the additional line number data in the plan_block_t struct
// #define USE_LINE_NUMBERS
// #define USE_LINE_NUMBERS // Disabled by default. Uncomment to enable.
// Enables a second coolant control pin via the mist coolant g-code command M7 on the Arduino Uno
// analog pin 5. Only use this option if you require a second coolant control pin.
// NOTE: The M8 flood coolant control pin on analog pin 4 will still be functional regardless.
// #define ENABLE_M7 // Mist coolant disabled by default. See config.h to enable/disable.
// #define ENABLE_M7 // Disabled by default. Uncomment to enable.
// ---------------------------------------------------------------------------------------
// ADVANCED CONFIGURATION OPTIONS:

437
doc/test/grbl_sim.m Normal file
View File

@ -0,0 +1,437 @@
% ----------------------------------------------------------------------------------------
% The MIT License (MIT)
%
% Copyright (c) 2014 Sungeun K. Jeon
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.
% ----------------------------------------------------------------------------------------
% This MATLAB script was written for the purpose of being a GRBL planner simulator. This
% simulator is a rough representation of the actual workings of Grbl on the Arduino, but
% was used to hone and proof the actual planner by providing quick visual feedback on its
% functionality when experimented on. This script should be considered for educational
% purposes only. This script requires and executes a pre-parsed g-code file from the
% matlab_convert.py script that is in a specific non-g-code format.
% There will be two figures plotted. The first is the line motion paths of the complete
% g-code program. The second is a representation of Grbl's planner buffer as new line
% motions are fed to it, plotting the velocity profiles the stepper motors will execute.
% Every time the user inputs an <Enter>, this feeds the simulator planner one line motion
% block. The left side is the first block in the buffer and the one that will be executed
% by the stepper module first. The right side is the end of the planner buffer, where the
% most recent streamed block is appended onto the planner buffer. Grbl's planner
% optimizes the velocity profiles between the beginning and end of the buffer based on
% the acceleration limits, intended velocity/feedrate, and line motion junction angles
% with their corresponding velocity limits (i.e. junctions with acute angles needs to come
% to a complete stop vs straight junctions can continue through at full speed.)
% ----------------------------------------------------------------------------------------
% Main function
% NOTE: This is just a way to keep all functions in one place, but all non-global variables
% are cleared as soon as this script completes.
function main()
% Load pre-parsed gcode moves.
close all;
warning off;
clearvars -global
fid = fopen('matlab.gcode','r');
gcode = textscan(fid,'%d8%f32%f32%f32%f32');
nblock = length(gcode{1});
% Plot all g-code moves.
figure
line(gcode{3},gcode{4},gcode{5});
axis equal;
% axis([min(gcode{3}) max(gcode{3}) min(gcode{4}) max(gcode{4}) min(gcode{5}) max(gcode{5})]);
title('G-code programming line motions');
view(3);
% Set up figure for planner queue
figure
% Print help.
disp('<NOTE: Press Enter to Advance One G-Code Line Motion>');
disp(' BLUE line indicates completed planner blocks that require no recalculation.');
disp(' RED line indicates planner blocks that have been recalculated.');
disp(' GREEN line indicates the location of the BPLANNED pointer. Always a recalculated block.');
disp(' BLACK dotted-line and ''x'' indicates block nominal speed and max junction velocity, respectively.');
disp(' CYAN ''.'' indicates block initial entry speed.');
% Define Grbl settings.
BUFFER_SIZE = 18; % Number of planner blocks in its ring buffer.
steps_per_mm = 200;
seekrate = 2500; % mm/min
acceleration = [100 100 100]; % mm/sec^2 [ X Y Z ] axes
junction_deviation = 0.1; % mm. See Grbl documentation on this parameter.
inch_2_mm = 25.4;
ACCELERATION_TICKS_PER_SECOND = 100;
gcode{2} = gcode{2};
gcode{2} = inch_2_mm*gcode{2};
gcode{3} = inch_2_mm*gcode{3};
gcode{4} = inch_2_mm*gcode{4};
gcode{5} = inch_2_mm*gcode{5};
% Initialize blocks
block.steps = [];
block.step_event_count = [];
block.delta_mm = [];
block.millimeters = [];
block.acceleration = [];
block.speed = [];
block.nominal_speed = [];
block.max_entry_speed = [];
block.entry_speed = [];
block.recalculate_flag = false;
for i = 2:BUFFER_SIZE
block(i) = block(1);
end
% Initialize planner
position = [0 0 0];
prev_unit_vec = [0 0 0];
previous_nominal_speed = 0;
pos = 0;
% BHEAD and BTAIL act as pointers to the block head and tail.
% BPLANNED acts as a pointer of the location of the end of a completed/optimized plan.
bhead = 1;
btail = 1;
bplanned = 1;
global block bhead btail bplanned nind acceleration BUFFER_SIZE pos ACCELERATION_TICKS_PER_SECOND
% Main loop. Simulates plan_buffer_line(). All of the precalculations for the newest incoming
% block occurs here. Anything independent of the planner changes.
for i = 1:nblock
target = round([gcode{3}(i) gcode{4}(i) gcode{5}(i)].*steps_per_mm);
if gcode{1}(i) == 1
feedrate = gcode{2}(i);
else
feedrate = seekrate;
end
nind = next_block_index(bhead);
if nind == btail
% Simulate a constantly full buffer. Move buffer tail.
bind = next_block_index(btail);
% Push planned pointer if encountered. Prevents it from looping back around the ring buffer.
if btail == bplanned; bplanned = bind; end
btail = bind;
end
block(bhead).steps = abs(target-position);
block(bhead).step_event_count = max(block(bhead).steps);
% Bail if this is a zero-length block
if block(bhead).step_event_count == 0
disp(['Zero-length block in line ',int2str(i)]);
else
% Compute path vector in terms of absolute step target and current positions
delta_mm = single((target-position)./steps_per_mm);
block(bhead).millimeters = single(norm(delta_mm));
inverse_millimeters = single(1/block(bhead).millimeters);
% Compute path unit vector
unit_vec = delta_mm/block(bhead).millimeters;
% Calculate speed in mm/minute for each axis
inverse_minute = single(feedrate * inverse_millimeters);
block(bhead).speed = delta_mm*inverse_minute;
block(bhead).nominal_speed = block(bhead).millimeters*inverse_minute;
% Calculate block acceleration. Operates on absolute value of unit vector.
[max_acc,ind] = max(abs(unit_vec)./acceleration); % Determine limiting acceleration
block(bhead).acceleration = acceleration(ind)/abs(unit_vec(ind));
% Compute maximum junction speed
block(bhead).max_entry_speed = 0.0;
if previous_nominal_speed > 0.0
cos_theta = dot(-previous_unit_vec,unit_vec);
if (cos_theta < 0.95)
block(bhead).max_entry_speed = min([block(bhead).nominal_speed,previous_nominal_speed]);
if (cos_theta > -0.95)
sin_theta_d2 = sqrt(0.5*(1.0-cos_theta));
block(bhead).max_entry_speed = min([block(bhead).max_entry_speed,sqrt(block(bhead).acceleration*3600*junction_deviation*sin_theta_d2/(1.0-sin_theta_d2))]);
end
end
end
block(bhead).entry_speed = 0; % Just initialize. Set accurately in the replanning function.
block(bhead).recalculate_flag = true; % Plotting flag to indicate this block has been updated.
previous_unit_vec = unit_vec;
previous_nominal_speed = block(bhead).nominal_speed;
position = target;
bhead = nind; % Block complete. Push buffer pointer.
planner_recalculate();
plot_buffer_velocities();
end
end
return
% Computes the next block index in the planner ring buffer
function block_index = next_block_index(block_index)
global BUFFER_SIZE
block_index = block_index + 1;
if block_index > BUFFER_SIZE
block_index = 1;
end
return
% Computes the previous block index in the planner ring buffer
function block_index = prev_block_index(block_index)
global BUFFER_SIZE
block_index = block_index-1;
if block_index < 1
block_index = BUFFER_SIZE;
end
return
% Planner recalculate function. The magic happens here.
function planner_recalculate(block)
global block bhead btail bplanned acceleration
bind = prev_block_index(bhead);
if bind == bplanned; return; end % Bail, if only one block in buffer. Can't be operated on.
% Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
% block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
% NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
next = [];
curr = bind; % Last block in buffer.
% Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
block(curr).entry_speed = min([block(curr).max_entry_speed,sqrt(2*block(curr).acceleration*60*60*block(curr).millimeters)]);
bind = prev_block_index(bind); % Btail or second to last block
if (bind == bplanned)
% Only two plannable blocks in buffer. Reverse pass complete.
% Check if the first block is the tail. If so, notify stepper module to update its current parameters.
% if bind == btail; update_tail_block; end
else
% Three or more plannable blocks in buffer. Loop it.
while bind ~= bplanned % Loop until bplanned point hits. Replans to last plan point.
next = curr;
curr = bind;
bind = prev_block_index( bind ); % Previous block pointer.
% Check if the first block is the tail. If so, notify stepper module to update its current parameters.
% if bind == btail; update_tail_block; end
% Compute maximum entry speed decelerating over the current block from its exit speed.
if block(curr).entry_speed ~= block(curr).max_entry_speed
block(curr).recalculate_flag = true; % Plotting flag to indicate this block has been updated.
block(curr).entry_speed = min([ block(curr).max_entry_speed,...
sqrt(block(next).entry_speed^2 + 2*block(curr).acceleration*60*60*block(curr).millimeters)]);
end
end
end
% For two blocks, reverse pass is skipped, but forward pass plans second block entry speed
% onward. This prevents the first, or the potentially executing block, from being over-written.
% NOTE: Can never be bhead, since bsafe is always in active buffer.
next = bplanned;
bind = next_block_index(bplanned); % Start at bplanned
while bind ~= bhead
curr = next;
next = bind;
% An acceleration block is always an optimally planned block since it starts from the first
% block's current speed or a maximum junction speed. Compute accelerations from this block
% and update the next block's entry speed.
if (block(curr).entry_speed < block(next).entry_speed)
% Once speed is set by forward planner, the plan for this block is finished and optimal.
% Increment the planner pointer forward one block.
entry_speed = sqrt(block(curr).entry_speed^2 + 2*block(curr).acceleration*60*60*block(curr).millimeters);
if (block(next).entry_speed > entry_speed)
block(next).entry_speed = entry_speed;
bplanned = bind;
end
end
% Check if the next block entry speed is at max_entry_speed. If so, move the planned pointer, since
% this entry speed cannot be improved anymore and all prior blocks have been completed and optimally planned.
if block(next).entry_speed == block(next).max_entry_speed
bplanned = bind;
end
% Recalculate trapezoid can be installed here, since it scans through all of the plannable blocks.
% NOTE: Eventually this will only be computed when being executed.
bind = next_block_index( bind );
end
return
% ----------------------------------------------------------------------------------------
% PLOTTING FUNCTIONS
% Plots the entire buffer plan into a MATLAB figure to visual the plan.
% BLUE line indicates completed planner blocks that require no recalculation.
% RED line indicates planner blocks that have been recalculated.
% GREEN line indicates the location of the BPLANNED pointer. Always a recalculated block.
% BLACK dotted-line and 'x' indicates block nominal speed and max junction velocity, respectively.
% CYAN '.' indicates block initial entry speed.
function plot_buffer_velocities()
global block bhead btail bplanned acceleration pos ACCELERATION_TICKS_PER_SECOND
bind = btail;
curr = [];
next = [];
pos_initial = 0;
pos = 0;
while bind ~= bhead
curr = next;
next = bind;
hold on;
if ~isempty(curr)
accel_d = estimate_acceleration_distance(block(curr).entry_speed, block(curr).nominal_speed, block(curr).acceleration*60*60);
decel_d = estimate_acceleration_distance(block(curr).nominal_speed, block(next).entry_speed,-block(curr).acceleration*60*60);
plateau_d = block(curr).millimeters-accel_d-decel_d;
if plateau_d < 0
accel_d = intersection_distance(block(curr).entry_speed, block(next).entry_speed, block(curr).acceleration*60*60, block(curr).millimeters);
if accel_d < 0
accel_d = 0;
elseif accel_d > block(curr).millimeters
accel_d = block(curr).millimeters;
end
plateau_d = 0;
end
color = 'b';
if (block(curr).recalculate_flag || block(next).recalculate_flag)
block(curr).recalculate_flag = false;
color = 'r';
end
if bplanned == curr
color = 'g';
end
plot_trap(pos,block(curr).entry_speed,block(next).entry_speed,block(curr).nominal_speed,block(curr).acceleration,accel_d,plateau_d,block(curr).millimeters,color)
plot([pos pos+block(curr).millimeters],block(curr).nominal_speed*[1 1],'k:') % BLACK dotted indicates
plot(pos,block(curr).max_entry_speed,'kx')
pos = pos + block(curr).millimeters;
plot(pos,block(next).entry_speed,'c.');
end
bind = next_block_index( bind );
end
accel_d = estimate_acceleration_distance(block(next).entry_speed, block(next).nominal_speed, block(next).acceleration*60*60);
decel_d = estimate_acceleration_distance(block(next).nominal_speed, 0, -block(next).acceleration*60*60);
plateau_d = block(next).millimeters-accel_d-decel_d;
if plateau_d < 0
accel_d = intersection_distance(block(next).entry_speed, 0, block(next).acceleration*60*60, block(next).millimeters);
if accel_d < 0
accel_d = 0;
elseif accel_d > block(next).millimeters
accel_d = block(next).millimeters;
end
plateau_d = 0;
end
block(next).recalculate_flag = false;
color = 'r';
if bplanned == next
color= 'g';
end
plot_trap(pos,block(next).entry_speed,0,block(next).nominal_speed,block(next).acceleration,accel_d,plateau_d,block(next).millimeters,color)
plot([pos pos+block(next).millimeters],block(next).nominal_speed*[1 1],'k:')
plot(pos,block(next).max_entry_speed,'kx')
plot(pos,block(next).entry_speed,'.');
pos = pos + block(next).millimeters;
plot(pos,0,'rx');
xlabel('mm');
ylabel('mm/sec');
xlim([pos_initial pos])
title('Planner buffer optimized velocity profile');
pause();
hold off;
plot(pos,0)
return
function d_a = estimate_acceleration_distance(initial_rate, target_rate, acceleration,rate_delta)
d_a = (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration);
return
function d_i = intersection_distance(initial_rate, final_rate, acceleration, distance, rate_delta)
d_i = (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration);
return
% Simply plots the ac/de-celeration curves and plateaus of a trapezoid.
function plot_trap(pos,initial_rate,final_rate,rate,accel,accel_d,plateau_d,millimeters,color)
dx = 1.0; % Line segment length
linex = [pos]; liney = [initial_rate];
% Acceleration
np = floor(accel_d/dx);
if np
v = initial_rate;
for i = 1:np
v = sqrt(v^2+2*accel*60*60*dx);
linex = [linex pos+i*dx];
liney = [liney v];
end
end
% Plateau
v = sqrt(initial_rate^2 + 2*accel*60*60*accel_d);
if v < rate
rate = v;
end
linex = [linex pos+[accel_d accel_d+plateau_d]];
liney = [liney [rate rate]];
% Deceleration
np = floor((millimeters-accel_d-plateau_d)/dx);
if np
v = rate;
for i = 1:np
v = sqrt(v^2-2*accel*60*60*dx);
linex = [linex pos+i*dx+accel_d+plateau_d];
liney = [liney v];
end
end
linex = [linex pos+millimeters];
liney = [ liney final_rate];
plot(linex,liney,color);
return

2362
doc/test/matlab.gcode Normal file
View File

File diff suppressed because it is too large Load Diff

270
doc/test/matlab_convert.py Executable file
View File

@ -0,0 +1,270 @@
#!/usr/bin/env python
"""\
The MIT License (MIT)
Copyright (c) 2014 Sungeun K. Jeon
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
----------------------------------------------------------------------------------------
"""
"""\
G-code preprocessor for the grbl_sim.m MATLAB script. Parses the g-code program to a
specific file format for the MATLAB script to use. Based on PreGrbl by @chamnit.
How to use: When running this python script, it will process the g-code program under
the filename "test.gcode" (may be changed below) and produces a file called "matlab.gcode"
that the grbl_sim.m MATLAB script will search for and execute.
"""
import re
from math import *
from copy import *
# -= SETTINGS =-
filein = 'test.gcode' # Input file name
fileout = 'matlab.gcode' # Output file name
ndigits_in = 4 # inch significant digits after '.'
ndigits_mm = 2 # mm significant digits after '.'
# mm_per_arc_segment = 0.38 # mm per arc segment
arc_tolerance = 0.00005*25.4
n_arc_correction = 20
inch2mm = 25.4 # inch to mm conversion scalar
verbose = False # Verbose flag to show all progress
remove_unsupported = True # Removal flag for all unsupported statements
# Initialize parser state
gc = { 'current_xyz' : [0,0,0],
'feed_rate' : 0, # F0
'motion_mode' : 'SEEK', # G00
'plane_axis' : [0,1,2], # G17
'inches_mode' : False, # G21
'inverse_feedrate_mode' : False, # G94
'absolute_mode' : True} # G90
def unit_conv(val) : # Converts value to mm
if gc['inches_mode'] : val *= inch2mm
return(val)
def fout_conv(val) : # Returns converted value as rounded string for output file.
if gc['inches_mode'] : return( str(round(val/inch2mm,ndigits_in)) )
else : return( str(round(val,ndigits_mm)) )
# Open g-code file
fin = open(filein,'r');
fout = open(fileout,'w');
# Iterate through g-code file
l_count = 0
for line in fin:
l_count += 1 # Iterate line counter
# Strip comments/spaces/tabs/new line and capitalize. Comment MSG not supported.
block = re.sub('\s|\(.*?\)','',line).upper()
block = re.sub('\\\\','',block) # Strip \ block delete character
block = re.sub('%','',block) # Strip % program start/stop character
if len(block) == 0 : # Ignore empty blocks
print "Skipping: " + line.strip()
else : # Process valid g-code clean block. Assumes no block delete characters or comments
g_cmd = re.findall(r'[^0-9\.\-]+',block) # Extract block command characters
g_num = re.findall(r'[0-9\.\-]+',block) # Extract block numbers
# G-code block error checks
# if len(g_cmd) != len(g_num) : print block; raise Exception('Invalid block. Unbalanced word and values.')
if 'N' in g_cmd :
if g_cmd[0]!='N': raise Exception('Line number must be first command in line.')
if g_cmd.count('N') > 1: raise Exception('More than one line number in block.')
g_cmd = g_cmd[1:] # Remove line number word
g_num = g_num[1:]
# Block item repeat checks? (0<=n'M'<5, G/M modal groups)
# Initialize block state
blk = { 'next_action' : 'DEFAULT',
'absolute_override' : False,
'target_xyz' : deepcopy(gc['current_xyz']),
'offset_ijk' : [0,0,0],
'radius_mode' : False,
'unsupported': [] }
# Pass 1
for cmd,num in zip(g_cmd,g_num) :
fnum = float(num)
inum = int(fnum)
if cmd is 'G' :
if inum is 0 : gc['motion_mode'] = 'SEEK'
elif inum is 1 : gc['motion_mode'] = 'LINEAR'
elif inum is 2 : gc['motion_mode'] = 'CW_ARC'
elif inum is 3 : gc['motion_mode'] = 'CCW_ARC'
elif inum is 4 : blk['next_action'] = 'DWELL'
elif inum is 17 : gc['plane_axis'] = [0,1,2] # Select XY Plane
elif inum is 18 : gc['plane_axis'] = [0,2,1] # Select XZ Plane
elif inum is 19 : gc['plane_axis'] = [1,2,0] # Select YZ Plane
elif inum is 20 : gc['inches_mode'] = True
elif inum is 21 : gc['inches_mode'] = False
elif inum in [28,30] : blk['next_action'] = 'GO_HOME'
elif inum is 53 : blk['absolute_override'] = True
elif inum is 54 : pass
elif inum is 80 : gc['motion_mode'] = 'MOTION_CANCEL'
elif inum is 90 : gc['absolute_mode'] = True
elif inum is 91 : gc['absolute_mode'] = False
elif inum is 92 : blk['next_action'] = 'SET_OFFSET'
elif inum is 93 : gc['inverse_feedrate_mode'] = True
elif inum is 94 : gc['inverse_feedrate_mode'] = False
else :
print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count)
if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num)))
elif cmd is 'M' :
if inum in [0,1] : pass # Program Pause
elif inum in [2,30,60] : pass # Program Completed
elif inum is 3 : pass # Spindle Direction 1
elif inum is 4 : pass # Spindle Direction -1
elif inum is 5 : pass # Spindle Direction 0
else :
print 'Unsupported command ' + cmd + num + ' on line ' + str(l_count)
if remove_unsupported : blk['unsupported'].append(zip(g_cmd,g_num).index((cmd,num)))
elif cmd is 'T' : pass # Tool Number
# Pass 2
for cmd,num in zip(g_cmd,g_num) :
fnum = float(num)
if cmd is 'F' : gc['feed_rate'] = unit_conv(fnum) # Feed Rate
elif cmd in ['I','J','K'] : blk['offset_ijk'][ord(cmd)-ord('I')] = unit_conv(fnum) # Arc Center Offset
elif cmd is 'N' : pass
elif cmd is 'P' : p = fnum # Misc value parameter
elif cmd is 'R' : r = unit_conv(fnum); blk['radius_mode'] = True # Arc Radius Mode
elif cmd is 'S' : pass # Spindle Speed
elif cmd in ['X','Y','Z'] : # Target Coordinates
if (gc['absolute_mode'] | blk['absolute_override']) :
blk['target_xyz'][ord(cmd)-ord('X')] = unit_conv(fnum)
else :
blk['target_xyz'][ord(cmd)-ord('X')] += unit_conv(fnum)
# Execute actions
if blk['next_action'] is 'GO_HOME' :
gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position
elif blk['next_action'] is 'SET_OFFSET' :
pass
elif blk['next_action'] is 'DWELL' :
if p < 0 : raise Exception('Dwell time negative.')
else : # 'DEFAULT'
if gc['motion_mode'] is 'SEEK' :
fout.write('0 '+fout_conv(gc['feed_rate']))
fout.write(' '+fout_conv(blk['target_xyz'][0]))
fout.write(' '+fout_conv(blk['target_xyz'][1]))
fout.write(' '+fout_conv(blk['target_xyz'][2]))
fout.write('\n')
gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position
elif gc['motion_mode'] is 'LINEAR' :
fout.write('1 '+fout_conv(gc['feed_rate']))
fout.write(' '+fout_conv(blk['target_xyz'][0]))
fout.write(' '+fout_conv(blk['target_xyz'][1]))
fout.write(' '+fout_conv(blk['target_xyz'][2]))
fout.write('\n')
gc['current_xyz'] = deepcopy(blk['target_xyz']) # Update position
elif gc['motion_mode'] in ['CW_ARC','CCW_ARC'] :
axis = gc['plane_axis']
# Convert radius mode to ijk mode
if blk['radius_mode'] :
x = blk['target_xyz'][axis[0]]-gc['current_xyz'][axis[0]]
y = blk['target_xyz'][axis[1]]-gc['current_xyz'][axis[1]]
if not (x==0 and y==0) : raise Exception('Same target and current XYZ not allowed in arc radius mode.')
h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y)
if isnan(h_x2_div_d) : raise Exception('Floating point error in arc conversion')
if gc['motion_mode'] is 'CCW_ARC' : h_x2_div_d = -h_x2_div_d
if r < 0 : h_x2_div_d = -h_x2_div_d
blk['offset_ijk'][axis[0]] = (x-(y*h_x2_div_d))/2;
blk['offset_ijk'][axis[1]] = (y+(x*h_x2_div_d))/2;
else :
radius = sqrt(blk['offset_ijk'][axis[0]]**2+blk['offset_ijk'][axis[1]]**2)
center_axis0 = gc['current_xyz'][axis[0]]+blk['offset_ijk'][axis[0]]
center_axis1 = gc['current_xyz'][axis[1]]+blk['offset_ijk'][axis[1]]
linear_travel = blk['target_xyz'][axis[2]]-gc['current_xyz'][axis[2]]
r_axis0 = -blk['offset_ijk'][axis[0]]
r_axis1 = -blk['offset_ijk'][axis[1]]
rt_axis0 = blk['target_xyz'][axis[0]] - center_axis0;
rt_axis1 = blk['target_xyz'][axis[1]] - center_axis1;
clockwise_sign = 1
if gc['motion_mode'] is 'CW_ARC' : clockwise_sign = -1
angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1)
if gc['motion_mode'] is 'CW_ARC' :
if angular_travel >= 0 :
angular_travel -= 2*pi
else :
if angular_travel <= 0 :
angular_travel += 2*pi
millimeters_of_travel = sqrt((angular_travel*radius)**2 + abs(linear_travel)**2)
mm_per_arc_segment = sqrt(4*(2*radius*arc_tolerance-arc_tolerance**2))
segments = int(millimeters_of_travel/mm_per_arc_segment)
print segments
print l_count
theta_per_segment = angular_travel/segments
linear_per_segment = linear_travel/segments
cos_T = 1-0.5*theta_per_segment*theta_per_segment
sin_T = theta_per_segment-theta_per_segment**3/6
print(fout_conv(mm_per_arc_segment))
print theta_per_segment*180/pi
arc_target = [0,0,0]
arc_target[axis[2]] = gc['current_xyz'][axis[2]]
count = 0
for i in range(1,segments+1) :
if i < segments :
if count < n_arc_correction :
r_axisi = r_axis0*sin_T + r_axis1*cos_T
r_axis0 = r_axis0*cos_T - r_axis1*sin_T
r_axis1 = deepcopy(r_axisi)
count += 1
else :
cos_Ti = cos((i-1)*theta_per_segment)
sin_Ti = sin((i-1)*theta_per_segment)
print n_arc_correction*(r_axis0 -( -blk['offset_ijk'][axis[0]]*cos_Ti + blk['offset_ijk'][axis[1]]*sin_Ti))
print n_arc_correction*(r_axis1 -( -blk['offset_ijk'][axis[0]]*sin_Ti - blk['offset_ijk'][axis[1]]*cos_Ti))
cos_Ti = cos(i*theta_per_segment)
sin_Ti = sin(i*theta_per_segment)
r_axis0 = -blk['offset_ijk'][axis[0]]*cos_Ti + blk['offset_ijk'][axis[1]]*sin_Ti
r_axis1 = -blk['offset_ijk'][axis[0]]*sin_Ti - blk['offset_ijk'][axis[1]]*cos_Ti
count = 0
arc_target[axis[0]] = center_axis0 + r_axis0
arc_target[axis[1]] = center_axis1 + r_axis1
arc_target[axis[2]] += linear_per_segment
else :
arc_target = deepcopy(blk['target_xyz']) # Last segment at target_xyz
# Write only changed variables.
fout.write('1 '+fout_conv(gc['feed_rate']))
fout.write(' '+fout_conv(arc_target[0]))
fout.write(' '+fout_conv(arc_target[1]))
fout.write(' '+fout_conv(arc_target[2]))
fout.write('\n')
gc['current_xyz'] = deepcopy(arc_target) # Update position
print 'Done!'
# Close files
fin.close()
fout.close()

2363
doc/test/test.gcode Normal file
View File

File diff suppressed because it is too large Load Diff

38
gcode.c
View File

@ -101,7 +101,9 @@ uint8_t gc_execute_line(char *line)
float target[N_AXIS];
clear_vector(target); // XYZ(ABC) axes parameters.
#ifdef USE_LINE_NUMBERS
int32_t line_number = 0;
#endif
gc.arc_radius = 0;
clear_vector(gc.arc_offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
@ -227,7 +229,11 @@ uint8_t gc_execute_line(char *line)
break;
case 'I': case 'J': case 'K': gc.arc_offset[letter-'I'] = to_millimeters(value); break;
case 'L': l = trunc(value); break;
case 'N': line_number = trunc(value); break;
case 'N':
#ifdef USE_LINE_NUMBERS
line_number = trunc(value);
#endif
break;
case 'P': p = value; break;
case 'R': gc.arc_radius = to_millimeters(value); break;
case 'S':
@ -331,7 +337,11 @@ uint8_t gc_execute_line(char *line)
target[idx] = gc.position[idx];
}
}
#ifdef USE_LINE_NUMBERS
mc_line(target, -1.0, false, line_number);
#else
mc_line(target, -1.0, false);
#endif
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
@ -340,7 +350,11 @@ uint8_t gc_execute_line(char *line)
} else {
if (!settings_read_coord_data(SETTING_INDEX_G30,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
}
#ifdef USE_LINE_NUMBERS
mc_line(coord_data, -1.0, false, line_number);
#else
mc_line(coord_data, -1.0, false);
#endif
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
@ -411,7 +425,13 @@ uint8_t gc_execute_line(char *line)
break;
case MOTION_MODE_SEEK:
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target, -1.0, false, line_number); }
else {
#ifdef USE_LINE_NUMBERS
mc_line(target, -1.0, false, line_number);
#else
mc_line(target, -1.0, false);
#endif
}
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
@ -419,7 +439,13 @@ uint8_t gc_execute_line(char *line)
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number); }
else {
#ifdef USE_LINE_NUMBERS
mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, line_number);
#else
mc_line(target, (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
#endif
}
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// Check if at least one of the axes of the selected plane has been specified. If in center
@ -441,9 +467,15 @@ uint8_t gc_execute_line(char *line)
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
#ifdef USE_LINE_NUMBERS
mc_arc(gc.position, target, gc.arc_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.arc_radius, isclockwise, line_number);
#else
mc_arc(gc.position, target, gc.arc_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.arc_radius, isclockwise);
#endif
}
break;
}

72
limits.c
View File

@ -50,9 +50,9 @@ void limits_init()
}
#ifdef ENABLE_SOFTWARE_DEBOUNCE
MCUSR &= ~(1<<WDRF);
WDTCSR |= (1<<WDCE) | (1<<WDE);
WDTCSR = (1<<WDP0); // Set time-out at ~32msec.
MCUSR &= ~(1<<WDRF);
WDTCSR |= (1<<WDCE) | (1<<WDE);
WDTCSR = (1<<WDP0); // Set time-out at ~32msec.
#endif
}
@ -76,38 +76,38 @@ void limits_disable()
// special pinout for an e-stop, but it is generally recommended to just directly connect
// your e-stop switch to the Arduino reset pin, since it is the most correct way to do this.
#ifndef ENABLE_SOFTWARE_DEBOUNCE
ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
{
// Ignore limit switches if already in an alarm state or in-process of executing an alarm.
// When in the alarm state, Grbl should have been reset or will force a reset, so any pending
// moves in the planner and serial buffers are all cleared and newly sent blocks will be
// locked out until a homing cycle or a kill lock command. Allows the user to disable the hard
// limit setting if their limits are constantly triggering after a reset and move their axes.
if (sys.state != STATE_ALARM) {
if (bit_isfalse(sys.execute,EXEC_ALARM)) {
ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
{
// Ignore limit switches if already in an alarm state or in-process of executing an alarm.
// When in the alarm state, Grbl should have been reset or will force a reset, so any pending
// moves in the planner and serial buffers are all cleared and newly sent blocks will be
// locked out until a homing cycle or a kill lock command. Allows the user to disable the hard
// limit setting if their limits are constantly triggering after a reset and move their axes.
if (sys.state != STATE_ALARM) {
if (bit_isfalse(sys.execute,EXEC_ALARM)) {
mc_reset(); // Initiate system kill.
sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event
}
}
}
#else // OPTIONAL: Software debounce limit pin routine.
// Upon limit pin change, enable watchdog timer to create a short delay.
ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1<<WDIE))) { WDTCSR |= (1<<WDIE); } }
ISR(WDT_vect) // Watchdog timer ISR
{
WDTCSR &= ~(1<<WDIE); // Disable watchdog timer.
if (sys.state != STATE_ALARM) { // Ignore if already in alarm state.
if (bit_isfalse(sys.execute,EXEC_ALARM)) {
uint8_t bits = LIMIT_PIN;
// Check limit pin state.
if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { bits ^= LIMIT_MASK; }
if (bits & LIMIT_MASK) {
mc_reset(); // Initiate system kill.
sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event
}
}
}
#else // OPTIONAL: Software debounce limit pin routine.
// Upon limit pin change, enable watchdog timer to create a short delay.
ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1<<WDIE))) { WDTCSR |= (1<<WDIE); } }
ISR(WDT_vect) // Watchdog timer ISR
{
WDTCSR &= ~(1<<WDIE); // Disable watchdog timer.
if (sys.state != STATE_ALARM) { // Ignore if already in alarm state.
if (bit_isfalse(sys.execute,EXEC_ALARM)) {
uint8_t bits = LIMIT_PIN;
// Check limit pin state.
if (bit_istrue(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { bits ^= LIMIT_MASK; }
if (bits & LIMIT_MASK) {
mc_reset(); // Initiate system kill.
sys.execute |= EXEC_CRIT_EVENT; // Indicate hard limit critical event
}
}
}
}
}
}
#endif
@ -168,7 +168,11 @@ void limits_go_home(uint8_t cycle_mask)
// Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
uint8_t limit_state;
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, homing_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion.
#else
plan_buffer_line(target, homing_rate, false); // Bypass mc_line(). Directly plan homing motion.
#endif
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
st_wake_up(); // Initiate motion
do {
@ -225,8 +229,12 @@ void limits_go_home(uint8_t cycle_mask)
}
}
plan_sync_position(); // Sync planner position to current machine position for pull-off move.
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, settings.homing_seek_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan motion.
#else
plan_buffer_line(target, settings.homing_seek_rate, false); // Bypass mc_line(). Directly plan motion.
#endif
// Initiate pull-off using main motion control routines.
// TODO : Clean up state routines so that this motion still shows homing state.
sys.state = STATE_QUEUED;

24
motion_control.c
View File

@ -39,7 +39,11 @@
// segments, must pass through this routine before being passed to the planner. The seperation of
// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being
// in the planner and to let backlash compensation or canned cycle integration simple and direct.
#ifdef USE_LINE_NUMBERS
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
#else
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
#endif
{
// If enabled, check for soft limit violations. Placed here all line motions are picked up
// from everywhere in Grbl.
@ -71,8 +75,12 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t l
else { break; }
} while (1);
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, feed_rate, invert_feed_rate, line_number);
#else
plan_buffer_line(target, feed_rate, invert_feed_rate);
#endif
// If idle, indicate to the system there is now a planned block in the buffer ready to cycle
// start. Otherwise ignore and continue on.
if (!sys.state) { sys.state = STATE_QUEUED; }
@ -86,8 +94,13 @@ void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t l
// The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance
// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal
// distance from segment to the circle when the end points both lie on the circle.
#ifdef USE_LINE_NUMBERS
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise, int32_t line_number)
#else
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise)
#endif
{
float center_axis0 = position[axis_0] + offset[axis_0];
float center_axis1 = position[axis_1] + offset[axis_1];
@ -183,14 +196,23 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
arc_target[axis_0] = center_axis0 + r_axis0;
arc_target[axis_1] = center_axis1 + r_axis1;
arc_target[axis_linear] += linear_per_segment;
#ifdef USE_LINE_NUMBERS
mc_line(arc_target, feed_rate, invert_feed_rate, line_number);
#else
mc_line(arc_target, feed_rate, invert_feed_rate);
#endif
// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
if (sys.abort) { return; }
}
}
// Ensure last segment arrives at target location.
#ifdef USE_LINE_NUMBERS
mc_line(target, feed_rate, invert_feed_rate, line_number);
#else
mc_line(target, feed_rate, invert_feed_rate);
#endif
}

10
motion_control.h
View File

@ -24,18 +24,26 @@
#define HOMING_CYCLE_LINE_NUMBER -1
// 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.
#ifdef USE_LINE_NUMBERS
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number);
#else
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate);
#endif
// 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.
#ifdef USE_LINE_NUMBERS
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise, int32_t line_number);
#else
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise);
#endif
// Dwell for a specific number of seconds
void mc_dwell(float seconds);

4
planner.c
View File

@ -259,7 +259,11 @@ uint8_t plan_check_full_buffer()
is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
invert_feed_rate always false). */
#ifdef USE_LINE_NUMBERS
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
#else
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
#endif
{
// Prepare and initialize new block
plan_block_t *block = &block_buffer[block_buffer_head];

4
planner.h
View File

@ -64,7 +64,11 @@ void plan_reset();
// Add a new linear movement to the buffer. target[N_AXIS] 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.
#ifdef USE_LINE_NUMBERS
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number);
#else
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate);
#endif
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks.