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

Various minor updates and variable definition corrections. Removed deprecated acceleration manager.

Browse Source 
- Removed deprecated acceleration manager (non-functional since v0.7b)
- Updated variable types and function headers. - Updated stepper
interrupt to ISR() from SIGNAL()+sei(). - General code cleanup.
This commit is contained in:
Sonny Jeon 2011-12-10 11:18:24 -07:00
parent 4bf0085ae6
commit 12bae58994
10 changed files with 210 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
main.c
View File

@ -36,7 +36,7 @@
int main(void) int main(void)
{ {
sei(); sei(); // Enable interrupts
serial_init(BAUD_RATE); serial_init(BAUD_RATE);
protocol_init(); protocol_init();
@ -47,8 +47,8 @@ int main(void)
gc_init(); gc_init();
limits_init(); limits_init();
for(;;){ while (1) {
sleep_mode(); // Wait for it ... // sleep_mode(); // Wait for it ...
protocol_process(); // ... process the serial protocol protocol_process(); // ... process the serial protocol
} }
return 0; /* never reached */ return 0; /* never reached */

5
motion_control.c
View File

@ -48,9 +48,6 @@ void mc_dwell(double seconds)
void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise) uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise)
{ {
// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
double center_axis0 = position[axis_0] + offset[axis_0]; double center_axis0 = position[axis_0] + offset[axis_0];
double center_axis1 = position[axis_1] + offset[axis_1]; double center_axis1 = position[axis_1] + offset[axis_1];
double linear_travel = target[axis_linear] - position[axis_linear]; double linear_travel = target[axis_linear] - position[axis_linear];
@ -141,8 +138,6 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
} }
#endif #endif

20
nuts_bolts.c
View File

@ -1,3 +1,23 @@
/*
nuts_bolts.c - Shared functions
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include "nuts_bolts.h" #include "nuts_bolts.h"
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>

2
nuts_bolts.h
View File

@ -1,5 +1,5 @@
/* /*
motion_control.h - cartesian robot controller. nuts_bolts.h - Header file for shared definitions, variables, and functions
Part of Grbl Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2009-2011 Simen Svale Skogsrud

194
planner.c
View File

@ -46,12 +46,11 @@ static int32_t position[3]; // The current position of the tool in a
static double previous_unit_vec[3]; // Unit vector of previous path line segment static double previous_unit_vec[3]; // Unit vector of previous path line segment
static double previous_nominal_speed; // Nominal speed of previous path line segment static double previous_nominal_speed; // Nominal speed of previous path line segment
static uint8_t acceleration_manager_enabled; // Acceleration management active?
// 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.
static int8_t next_block_index(int8_t block_index) { static uint8_t next_block_index(uint8_t block_index)
{
block_index++; block_index++;
if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
return(block_index); return(block_index);
@ -59,7 +58,8 @@ static int8_t next_block_index(int8_t block_index) {
// Returns the index of the previous block in the ring buffer // Returns the index of the previous block in the ring buffer
static int8_t prev_block_index(int8_t block_index) { static uint8_t prev_block_index(uint8_t block_index)
{
if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
block_index--; block_index--;
return(block_index); return(block_index);
@ -68,7 +68,8 @@ static int8_t prev_block_index(int8_t block_index) {
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
// given acceleration: // given acceleration:
static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration)
{
return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) ); return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
} }
@ -86,7 +87,8 @@ static double estimate_acceleration_distance(double initial_rate, double target_
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and // a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance)
{
return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) ); return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
} }
@ -96,13 +98,15 @@ static double intersection_distance(double initial_rate, double final_rate, doub
// NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed // NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
// in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed // in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
// BLOCK_BUFFER_SIZE calls per planner cycle. // BLOCK_BUFFER_SIZE calls per planner cycle.
static double max_allowable_speed(double acceleration, double target_velocity, double distance) { static double max_allowable_speed(double acceleration, double target_velocity, double distance)
{
return( sqrt(target_velocity*target_velocity-2*acceleration*distance) ); return( sqrt(target_velocity*target_velocity-2*acceleration*distance) );
} }
// The kernel called by planner_recalculate() when scanning the plan from last to first entry. // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next)
{
if (!current) { return; } // Cannot operate on nothing. if (!current) { return; } // Cannot operate on nothing.
if (next) { if (next) {
@ -128,8 +132,9 @@ static void planner_reverse_pass_kernel(block_t *previous, block_t *current, blo
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass. // implements the reverse pass.
static void planner_reverse_pass() { static void planner_reverse_pass()
auto int8_t block_index = block_buffer_head; {
uint8_t block_index = block_buffer_head;
block_t *block[3] = {NULL, NULL, NULL}; block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_tail) { while(block_index != block_buffer_tail) {
block_index = prev_block_index( block_index ); block_index = prev_block_index( block_index );
@ -143,7 +148,8 @@ static void planner_reverse_pass() {
// The kernel called by planner_recalculate() when scanning the plan from first to last entry. // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next)
{
if(!previous) { return; } // Begin planning after buffer_tail if(!previous) { return; } // Begin planning after buffer_tail
// If the previous block is an acceleration block, but it is not long enough to complete the // If the previous block is an acceleration block, but it is not long enough to complete the
@ -167,8 +173,9 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass. // implements the forward pass.
static void planner_forward_pass() { static void planner_forward_pass()
int8_t block_index = block_buffer_tail; {
uint8_t block_index = block_buffer_tail;
block_t *block[3] = {NULL, NULL, NULL}; block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_head) { while(block_index != block_buffer_head) {
@ -194,8 +201,8 @@ static void planner_forward_pass() {
// The factors represent a factor of braking and must be in the range 0.0-1.0. // The factors represent a factor of braking and must be in the range 0.0-1.0.
// This converts the planner parameters to the data required by the stepper controller. // This converts the planner parameters to the data required by the stepper controller.
// NOTE: Final rates must be computed in terms of their respective blocks. // NOTE: Final rates must be computed in terms of their respective blocks.
static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor)
{
block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2) int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2)
@ -235,8 +242,9 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d
// planner_recalculate() after updating the blocks. Any recalulate flagged junction will // planner_recalculate() after updating the blocks. Any recalulate flagged junction will
// compute the two adjacent trapezoids to the junction, since the junction speed corresponds // compute the two adjacent trapezoids to the junction, since the junction speed corresponds
// to exit speed and entry speed of one another. // to exit speed and entry speed of one another.
static void planner_recalculate_trapezoids() { static void planner_recalculate_trapezoids()
int8_t block_index = block_buffer_tail; {
uint8_t block_index = block_buffer_tail;
block_t *current; block_t *current;
block_t *next = NULL; block_t *next = NULL;
@ -281,49 +289,41 @@ static void planner_recalculate_trapezoids() {
// All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
// off errors. Only when planned values are converted to stepper rate parameters, these are integers. // off errors. Only when planned values are converted to stepper rate parameters, these are integers.
static void planner_recalculate() { static void planner_recalculate()
{
planner_reverse_pass(); planner_reverse_pass();
planner_forward_pass(); planner_forward_pass();
planner_recalculate_trapezoids(); planner_recalculate_trapezoids();
} }
void plan_init() { void plan_init()
{
block_buffer_head = 0; block_buffer_head = 0;
block_buffer_tail = 0; block_buffer_tail = 0;
plan_set_acceleration_manager_enabled(true);
clear_vector(position); clear_vector(position);
clear_vector_double(previous_unit_vec); clear_vector_double(previous_unit_vec);
previous_nominal_speed = 0.0; previous_nominal_speed = 0.0;
} }
void plan_set_acceleration_manager_enabled(uint8_t enabled) { void plan_discard_current_block()
if ((!!acceleration_manager_enabled) != (!!enabled)) { {
st_synchronize();
acceleration_manager_enabled = !!enabled;
}
}
int plan_is_acceleration_manager_enabled() {
return(acceleration_manager_enabled);
}
void plan_discard_current_block() {
if (block_buffer_head != block_buffer_tail) { if (block_buffer_head != block_buffer_tail) {
block_buffer_tail = next_block_index( block_buffer_tail ); block_buffer_tail = next_block_index( block_buffer_tail );
} }
} }
block_t *plan_get_current_block() { block_t *plan_get_current_block()
{
if (block_buffer_head == block_buffer_tail) { return(NULL); } if (block_buffer_head == block_buffer_tail) { return(NULL); }
return(&block_buffer[block_buffer_tail]); return(&block_buffer[block_buffer_tail]);
} }
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // 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. // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) { void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate)
{
// Calculate target position in absolute steps // Calculate target position in absolute steps
int32_t target[3]; int32_t target[3];
target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
@ -331,7 +331,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
// Calculate the buffer head after we push this byte // Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index( block_buffer_head ); uint8_t next_buffer_head = next_block_index( block_buffer_head );
// If the buffer is full: good! That means we are well ahead of the robot. // If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer. // Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) { sleep_mode(); } while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
@ -384,84 +384,72 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
block->rate_delta = ceil( block->step_event_count*inverse_millimeters * block->rate_delta = ceil( block->step_event_count*inverse_millimeters *
settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick) settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
// Perform planner-enabled calculations // Compute path unit vector
if (acceleration_manager_enabled) { double unit_vec[3];
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Let a circle be tangent to both previous and current path line segments, where the junction
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // deviation is defined as the distance from the junction to the closest edge of the circle,
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. // colinear with the circle center. The circular segment joining the two paths represents the
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// Skip and use default max junction speed for 0 degree acute junction. // nonlinearities of both the junction angle and junction velocity.
if (cos_theta < 0.95) { double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if (cos_theta > -0.95) { if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
vmax_junction = min(vmax_junction, double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
} - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
} }
} }
block->max_entry_speed = vmax_junction; }
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
previous_nominal_speed = block->nominal_speed; block->entry_speed = min(vmax_junction, v_allowable);
} else { // Initialize planner efficiency flags
// Acceleration planner disabled. Set minimum that is required. // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
block->initial_rate = block->nominal_rate; // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
block->final_rate = block->nominal_rate; // the current block and next block junction speeds are guaranteed to always be at their maximum
block->accelerate_until = 0; // junction speeds in deceleration and acceleration, respectively. This is due to how the current
block->decelerate_after = block->step_event_count; // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
block->rate_delta = 0; // the reverse and forward planners, the corresponding block junction speed will always be at the
} // the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
previous_nominal_speed = block->nominal_speed;
// Move buffer head // Move buffer head
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
// Update position // Update position
memcpy(position, target, sizeof(target)); // position[] = target[] memcpy(position, target, sizeof(target)); // position[] = target[]
if (acceleration_manager_enabled) { planner_recalculate(); } planner_recalculate();
st_cycle_start(); st_cycle_start();
} }

19
planner.h
View File

@ -27,12 +27,12 @@
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
// the source g-code and may never actually be reached if acceleration management is active. // the source g-code and may never actually be reached if acceleration management is active.
typedef struct { typedef struct {
// Fields used by the bresenham algorithm for tracing the line // Fields used by the bresenham algorithm for tracing the line
uint32_t steps_x, steps_y, steps_z; // Step count along each axis
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
uint32_t steps_x, steps_y, steps_z; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block int32_t step_event_count; // The number of step events required to complete this block
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
// Fields used by the motion planner to manage acceleration // Fields used by the motion planner to manage acceleration
double nominal_speed; // The nominal speed for this block in mm/min double nominal_speed; // The nominal speed for this block in mm/min
double entry_speed; // Entry speed at previous-current junction in mm/min double entry_speed; // Entry speed at previous-current junction in mm/min
@ -42,12 +42,13 @@ typedef struct {
uint8_t nominal_length_flag; // Planner flag for nominal speed always reached uint8_t nominal_length_flag; // Planner flag for nominal speed always reached
// Settings for the trapezoid generator // Settings for the trapezoid generator
uint32_t initial_rate; // The jerk-adjusted step rate at start of block uint32_t initial_rate; // The step rate at start of block
uint32_t final_rate; // The minimal rate at exit uint32_t final_rate; // The step rate at end of block
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint32_t accelerate_until; // The index of the step event on which to stop acceleration uint32_t accelerate_until; // The index of the step event on which to stop acceleration
uint32_t decelerate_after; // The index of the step event on which to start decelerating uint32_t decelerate_after; // The index of the step event on which to start decelerating
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
} block_t; } block_t;
// Initialize the motion plan subsystem // Initialize the motion plan subsystem
@ -65,12 +66,6 @@ void plan_discard_current_block();
// Gets the current block. Returns NULL if buffer empty // Gets the current block. Returns NULL if buffer empty
block_t *plan_get_current_block(); block_t *plan_get_current_block();
// Enables or disables acceleration-management for upcoming blocks
void plan_set_acceleration_manager_enabled(uint8_t enabled);
// Is acceleration-management currently enabled?
int plan_is_acceleration_manager_enabled();
// Reset the position vector // Reset the position vector
void plan_set_current_position(double x, double y, double z); void plan_set_current_position(double x, double y, double z);

20
protocol.c
View File

@ -31,10 +31,12 @@
#include <avr/pgmspace.h> #include <avr/pgmspace.h>
#define LINE_BUFFER_SIZE 50 #define LINE_BUFFER_SIZE 50
static char line[LINE_BUFFER_SIZE]; static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated.
static uint8_t char_counter; static uint8_t char_counter; // Last character counter in line variable.
static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters.
static void status_message(int status_code) { static void status_message(int status_code)
{
if (status_code == 0) { if (status_code == 0) {
printPgmString(PSTR("ok\r\n")); printPgmString(PSTR("ok\r\n"));
} else { } else {
@ -57,12 +59,15 @@ static void status_message(int status_code) {
void protocol_init() void protocol_init()
{ {
char_counter = 0; // Reset line input
iscomment = false;
printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
printPgmString(PSTR("\r\n")); printPgmString(PSTR("\r\n"));
} }
// Executes one line of input according to protocol // Executes one line of input according to protocol
uint8_t protocol_execute_line(char *line) { uint8_t protocol_execute_line(char *line)
{
if(line[0] == '$') { if(line[0] == '$') {
return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module
} else { } else {
@ -70,15 +75,16 @@ uint8_t protocol_execute_line(char *line) {
} }
} }
// Process one line of incoming serial data. Remove unneeded characters and capitalize.
void protocol_process() void protocol_process()
{ {
char c; char c;
uint8_t iscomment = false;
while((c = serial_read()) != SERIAL_NO_DATA) while((c = serial_read()) != SERIAL_NO_DATA)
{ {
if ((c == '\n') || (c == '\r')) { // End of block reached if ((c == '\n') || (c == '\r')) { // End of line reached
if (char_counter > 0) {// Line is complete. Then execute! if (char_counter > 0) {// Line is complete. Then execute!
line[char_counter] = 0; // terminate string line[char_counter] = 0; // Terminate string
status_message(protocol_execute_line(line)); status_message(protocol_execute_line(line));
} else { } else {
// Empty or comment line. Skip block. // Empty or comment line. Skip block.

1
protocol.h
View File

@ -3,6 +3,7 @@
Part of Grbl Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by

52
serial.c
View File

@ -44,25 +44,25 @@ volatile uint8_t tx_buffer_tail = 0;
static void set_baud_rate(long baud) { static void set_baud_rate(long baud) {
uint16_t UBRR0_value = ((F_CPU / 16 + baud / 2) / baud - 1); uint16_t UBRR0_value = ((F_CPU / 16 + baud / 2) / baud - 1);
UBRR0H = UBRR0_value >> 8; UBRR0H = UBRR0_value >> 8;
UBRR0L = UBRR0_value; UBRR0L = UBRR0_value;
} }
void serial_init(long baud) void serial_init(long baud)
{ {
set_baud_rate(baud); set_baud_rate(baud);
/* baud doubler off - Only needed on Uno XXX */ /* baud doubler off - Only needed on Uno XXX */
UCSR0A &= ~(1 << U2X0); UCSR0A &= ~(1 << U2X0);
// enable rx and tx // enable rx and tx
UCSR0B |= 1<<RXEN0; UCSR0B |= 1<<RXEN0;
UCSR0B |= 1<<TXEN0; UCSR0B |= 1<<TXEN0;
// enable interrupt on complete reception of a byte // enable interrupt on complete reception of a byte
UCSR0B |= 1<<RXCIE0; UCSR0B |= 1<<RXCIE0;
// defaults to 8-bit, no parity, 1 stop bit // defaults to 8-bit, no parity, 1 stop bit
} }
void serial_write(uint8_t data) { void serial_write(uint8_t data) {
@ -71,19 +71,19 @@ void serial_write(uint8_t data) {
if (next_head == TX_BUFFER_SIZE) { next_head = 0; } if (next_head == TX_BUFFER_SIZE) { next_head = 0; }
// Wait until there's a space in the buffer // Wait until there's a space in the buffer
while (next_head == tx_buffer_tail) { sleep_mode(); }; while (next_head == tx_buffer_tail) { };//sleep_mode(); };
// Store data and advance head // Store data and advance head
tx_buffer[tx_buffer_head] = data; tx_buffer[tx_buffer_head] = data;
tx_buffer_head = next_head; tx_buffer_head = next_head;
// Enable Data Register Empty Interrupt to make sure tx-streaming is running // Enable Data Register Empty Interrupt to make sure tx-streaming is running
UCSR0B |= (1 << UDRIE0); UCSR0B |= (1 << UDRIE0);
} }
// Data Register Empty Interrupt handler // Data Register Empty Interrupt handler
SIGNAL(USART_UDRE_vect) { ISR(USART_UDRE_vect) {
// temporary tx_buffer_tail (to optimize for volatile) // Temporary tx_buffer_tail (to optimize for volatile)
uint8_t tail = tx_buffer_tail; uint8_t tail = tx_buffer_tail;
// Send a byte from the buffer // Send a byte from the buffer
@ -101,25 +101,25 @@ SIGNAL(USART_UDRE_vect) {
uint8_t serial_read() uint8_t serial_read()
{ {
if (rx_buffer_head == rx_buffer_tail) { if (rx_buffer_head == rx_buffer_tail) {
return SERIAL_NO_DATA; return SERIAL_NO_DATA;
} else { } else {
uint8_t data = rx_buffer[rx_buffer_tail]; uint8_t data = rx_buffer[rx_buffer_tail];
rx_buffer_tail++; rx_buffer_tail++;
if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; } if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
return data; return data;
} }
} }
SIGNAL(USART_RX_vect) ISR(USART_RX_vect)
{ {
uint8_t data = UDR0; uint8_t data = UDR0;
uint8_t next_head = rx_buffer_head + 1; uint8_t next_head = rx_buffer_head + 1;
if (next_head == RX_BUFFER_SIZE) { next_head = 0; } if (next_head == RX_BUFFER_SIZE) { next_head = 0; }
// Write data to buffer unless it is full. // Write data to buffer unless it is full.
if (next_head != rx_buffer_tail) { if (next_head != rx_buffer_tail) {
rx_buffer[rx_buffer_head] = data; rx_buffer[rx_buffer_head] = data;
rx_buffer_head = next_head; rx_buffer_head = next_head;
} }
} }

96
stepper.c
View File

@ -79,10 +79,10 @@ static uint8_t cycle_start; // Cycle start flag to indicate program start an
static void set_step_events_per_minute(uint32_t steps_per_minute); static void set_step_events_per_minute(uint32_t steps_per_minute);
// Stepper state initialization // Stepper state initialization
void st_wake_up() static void st_wake_up()
{ {
// Initialize stepper output bits // Initialize stepper output bits
out_bits = (0) ^ (settings.invert_mask); out_bits = (0) ^ (settings.invert_mask);
// Enable steppers by resetting the stepper disable port // Enable steppers by resetting the stepper disable port
STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
// Enable stepper driver interrupt // Enable stepper driver interrupt
@ -90,7 +90,7 @@ void st_wake_up()
} }
// Stepper shutdown // Stepper shutdown
void st_go_idle() static void st_go_idle()
{ {
// Cycle finished. Set flag to false. // Cycle finished. Set flag to false.
cycle_start = false; cycle_start = false;
@ -133,28 +133,24 @@ static uint8_t iterate_trapezoid_cycle_counter()
// config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
// It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse.
// The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts.
SIGNAL(TIMER1_COMPA_vect) // NOTE: ISR_NOBLOCK allows SIG_OVERFLOW2 to trigger on-time regardless of time in this handler. This is
// the compiler optimizable equivalent of the old SIGNAL() and sei() method.
ISR(TIMER1_COMPA_vect,ISR_NOBLOCK)
{ {
// TODO: Check if the busy-flag can be eliminated by just disabling this interrupt while we are in it if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
busy = true;
if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt
// Set the direction pins a couple of nanoseconds before we step the steppers // Set the direction pins a couple of nanoseconds before we step the steppers
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
// Then pulse the stepping pins // Then pulse the stepping pins
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits; STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
// Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
// exactly settings.pulse_microseconds microseconds. // exactly settings.pulse_microseconds microseconds.
// TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Bit shift divide by 8.
busy = true;
sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
// ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered
// at exactly the right time even if we occasionally spend a lot of time inside this handler.))
// If there is no current block, attempt to pop one from the buffer // If there is no current block, attempt to pop one from the buffer
if (current_block == NULL) { if (current_block == NULL) {
// Anything in the buffer? // Anything in the buffer? If so, initialize next motion.
current_block = plan_get_current_block(); current_block = plan_get_current_block();
if (current_block != NULL) { if (current_block != NULL) {
trapezoid_generator_reset(); trapezoid_generator_reset();
@ -256,37 +252,39 @@ SIGNAL(TIMER1_COMPA_vect)
// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
// the motor port after a short period (settings.pulse_microseconds) completing one step cycle. // the motor port after a short period (settings.pulse_microseconds) completing one step cycle.
SIGNAL(TIMER2_OVF_vect) ISR(TIMER2_OVF_vect)
{ {
// reset stepping pins (leave the direction pins) // Reset stepping pins (leave the direction pins)
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK);
} }
// Initialize and start the stepper motor subsystem // Initialize and start the stepper motor subsystem
void st_init() void st_init()
{ {
// Configure directions of interface pins // Configure directions of interface pins
STEPPING_DDR |= STEPPING_MASK; STEPPING_DDR |= STEPPING_MASK;
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT; STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
// waveform generation = 0100 = CTC // waveform generation = 0100 = CTC
TCCR1B &= ~(1<<WGM13); TCCR1B &= ~(1<<WGM13);
TCCR1B |= (1<<WGM12); TCCR1B |= (1<<WGM12);
TCCR1A &= ~(1<<WGM11); TCCR1A &= ~(1<<WGM11);
TCCR1A &= ~(1<<WGM10); TCCR1A &= ~(1<<WGM10);
// output mode = 00 (disconnected) // output mode = 00 (disconnected)
TCCR1A &= ~(3<<COM1A0); TCCR1A &= ~(3<<COM1A0);
TCCR1A &= ~(3<<COM1B0); TCCR1A &= ~(3<<COM1B0);
// Configure Timer 2 // Configure Timer 2
TCCR2A = 0; // Normal operation TCCR2A = 0; // Normal operation
TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
TIMSK2 |= (1<<TOIE2); TIMSK2 |= (1<<TOIE2);
set_step_events_per_minute(6000); set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE);
trapezoid_tick_cycle_counter = 0; trapezoid_tick_cycle_counter = 0;
current_block = NULL;
busy = false;
// Start in the idle state // Start in the idle state
st_go_idle(); st_go_idle();
@ -306,30 +304,30 @@ static uint32_t config_step_timer(uint32_t cycles)
uint16_t prescaler; uint16_t prescaler;
uint32_t actual_cycles; uint32_t actual_cycles;
if (cycles <= 0xffffL) { if (cycles <= 0xffffL) {
ceiling = cycles; ceiling = cycles;
prescaler = 0; // prescaler: 0 prescaler = 0; // prescaler: 0
actual_cycles = ceiling; actual_cycles = ceiling;
} else if (cycles <= 0x7ffffL) { } else if (cycles <= 0x7ffffL) {
ceiling = cycles >> 3; ceiling = cycles >> 3;
prescaler = 1; // prescaler: 8 prescaler = 1; // prescaler: 8
actual_cycles = ceiling * 8L; actual_cycles = ceiling * 8L;
} else if (cycles <= 0x3fffffL) { } else if (cycles <= 0x3fffffL) {
ceiling = cycles >> 6; ceiling = cycles >> 6;
prescaler = 2; // prescaler: 64 prescaler = 2; // prescaler: 64
actual_cycles = ceiling * 64L; actual_cycles = ceiling * 64L;
} else if (cycles <= 0xffffffL) { } else if (cycles <= 0xffffffL) {
ceiling = (cycles >> 8); ceiling = (cycles >> 8);
prescaler = 3; // prescaler: 256 prescaler = 3; // prescaler: 256
actual_cycles = ceiling * 256L; actual_cycles = ceiling * 256L;
} else if (cycles <= 0x3ffffffL) { } else if (cycles <= 0x3ffffffL) {
ceiling = (cycles >> 10); ceiling = (cycles >> 10);
prescaler = 4; // prescaler: 1024 prescaler = 4; // prescaler: 1024
actual_cycles = ceiling * 1024L; actual_cycles = ceiling * 1024L;
} else { } else {
// Okay, that was slower than we actually go. Just set the slowest speed // Okay, that was slower than we actually go. Just set the slowest speed
ceiling = 0xffff; ceiling = 0xffff;
prescaler = 4; prescaler = 4;
actual_cycles = 0xffff * 1024; actual_cycles = 0xffff * 1024;
} }
// Set prescaler // Set prescaler
TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10); TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
@ -338,7 +336,8 @@ static uint32_t config_step_timer(uint32_t cycles)
return(actual_cycles); return(actual_cycles);
} }
static void set_step_events_per_minute(uint32_t steps_per_minute) { static void set_step_events_per_minute(uint32_t steps_per_minute)
{
if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; } if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; }
cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute); cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
} }
@ -350,7 +349,8 @@ void st_go_home()
} }
// Planner external interface to start stepper interrupt and execute the blocks in queue. // Planner external interface to start stepper interrupt and execute the blocks in queue.
void st_cycle_start() { void st_cycle_start()
{
if (!cycle_start) { if (!cycle_start) {
cycle_start = true; cycle_start = true;
st_wake_up(); st_wake_up();