diff --git a/doc/planner-maths.txt b/doc/planner-maths.txt new file mode 100644 index 0000000..49f371f --- /dev/null +++ b/doc/planner-maths.txt @@ -0,0 +1,30 @@ +Reasoning behind the mathematics in 'planner' module (in the key of 'Mathematica') +================================================================================== + + +s == speed, a == acceleration, t == time, d == distance + +Basic definitions: + + Speed[s_, a_, t_] := s + (a*t) + Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] + +Distance to reach a specific speed with a constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] + d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + +Speed after a given distance of travel with constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] + m -> Sqrt[2 a d + s^2] + + DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] + +When to start braking (di) to reach a specified destionation speed (s2) after accelerating +from initial speed s1 without ever stopping at a plateau: + + Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] + di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + + IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) diff --git a/doc/structure.txt b/doc/structure.txt index 21c5757..f87feb3 100644 --- a/doc/structure.txt +++ b/doc/structure.txt @@ -3,7 +3,7 @@ The general structure of Grbl The main processing stack: -'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. +'protocol' : Accepts command lines from the serial port and passes them to 'gcode' for execution. Provides status responses for each command. 'gcode' : Recieves gcode from 'protocol', parses it according to the current state diff --git a/motion_control.h b/motion_control.h index d2f409b..c205e2f 100644 --- a/motion_control.h +++ b/motion_control.h @@ -27,14 +27,16 @@ // 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. -#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) // NOTE: Although this function structurally belongs in this module, there is nothing to do but // to forward the request to the planner. For efficiency the function is implemented with a macro. +#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) + // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the // circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. + void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, int axis_linear, double feed_rate, int invert_feed_rate, double *position); diff --git a/planner.c b/planner.c index ce7f184..4054f58 100644 --- a/planner.c +++ b/planner.c @@ -20,38 +20,6 @@ /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ -/* - Reasoning behind the mathematics in this module (in the key of 'Mathematica'): - - s == speed, a == acceleration, t == time, d == distance - - Basic definitions: - - Speed[s_, a_, t_] := s + (a*t) - Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] - - Distance to reach a specific speed with a constant acceleration: - - Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] - d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() - - Speed after a given distance of travel with constant acceleration: - - Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] - m -> Sqrt[2 a d + s^2] - - DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] - - When to start braking (di) to reach a specified destionation speed (s2) after accelerating - from initial speed s1 without ever stopping at a plateau: - - Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] - di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() - - IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) -*/ - - #include #include #include @@ -67,8 +35,8 @@ #define BLOCK_BUFFER_SIZE 16 static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static volatile uint8_t block_buffer_head; // Index of the next block to be pushed -static volatile uint8_t block_buffer_tail; // Index of the block to process now +static volatile uint8_t block_buffer_head; // Index of the next block to be pushed +static volatile uint8_t block_buffer_tail; // Index of the block to process now // The current position of the tool in absolute steps static int32_t position[3]; @@ -86,11 +54,6 @@ double estimate_acceleration_distance(double initial_rate, double target_rate, d ); } -// This function gives you the point at which you must start braking (at the rate of -acceleration) if -// 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 -// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - /* + <- some maximum rate we don't care about /|\ / | \ @@ -101,6 +64,12 @@ double estimate_acceleration_distance(double initial_rate, double target_rate, d | | intersection_distance distance */ + +// This function gives you the point at which you must start braking (at the rate of -acceleration) if +// 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 +// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) + 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)/ @@ -108,10 +77,6 @@ double intersection_distance(double initial_rate, double final_rate, double acce ); } - -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// The factors represent a factor of braking and must be in the range 0.0-1.0. - /* +--------+ <- nominal_rate / \ @@ -121,6 +86,9 @@ double intersection_distance(double initial_rate, double final_rate, double acce time --> */ +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +// The factors represent a factor of braking and must be in the range 0.0-1.0. + void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); block->final_rate = ceil(block->nominal_rate*exit_factor); @@ -418,4 +386,3 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); } - diff --git a/planner.h b/planner.h index a132118..ab622d5 100644 --- a/planner.h +++ b/planner.h @@ -18,9 +18,6 @@ along with Grbl. If not, see . */ -// This module is to be considered a sub-module of stepper.c. Please don't include -// this file from any other module. - #ifndef planner_h #define planner_h