grbl-LPC-CoreXY/doc/test/grbl_sim.m

438 lines
17 KiB
Mathematica
Raw Normal View History

% ----------------------------------------------------------------------------------------
% 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