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

cleanup

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2011-01-25 14:07:01 +01:00
parent 0c262b03c2
commit 5f005f59f1
1 changed files with 59 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
stepper_plan.c
View File

@ -18,6 +18,38 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>. along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/ */
/*
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 <inttypes.h> #include <inttypes.h>
#include <math.h> #include <math.h>
#include <stdlib.h> #include <stdlib.h>
@ -40,7 +72,10 @@ uint8_t acceleration_management; // Acceleration management active?
// 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:
inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) {
return((target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration)); return(
(target_rate*target_rate-initial_rate*initial_rate)/
(2L*acceleration)
);
} }
// This function gives you the point at which you must start braking (at the rate of -acceleration) if // This function gives you the point at which you must start braking (at the rate of -acceleration) if
@ -59,7 +94,10 @@ inline double estimate_acceleration_distance(double initial_rate, double target_
intersection_distance distance */ intersection_distance distance */
inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { inline 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)
);
} }
@ -103,7 +141,9 @@ void calculate_trapezoid_for_block(struct Block *block, double entry_factor, dou
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance. // acceleration within the allotted distance.
inline double max_allowable_speed(double acceleration, double target_velocity, double distance) { inline 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)
);
} }
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
@ -119,7 +159,7 @@ inline double junction_jerk(struct Block *before, struct Block *after) {
// 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.
void planner_reverse_pass_kernel(struct Block *previous, struct Block *current, struct Block *next) { void planner_reverse_pass_kernel(struct Block *previous, struct Block *current, struct Block *next) {
if(!current){return;} if(!current) { return; }
double entry_factor = 1.0; double entry_factor = 1.0;
double exit_factor; double exit_factor;
@ -153,7 +193,7 @@ void planner_reverse_pass_kernel(struct Block *previous, struct Block *current,
current->entry_factor = entry_factor; current->entry_factor = entry_factor;
} }
// recalculate_plan() 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.
void planner_reverse_pass() { void planner_reverse_pass() {
auto int8_t block_index = block_buffer_head; auto int8_t block_index = block_buffer_head;
@ -170,7 +210,7 @@ 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.
void planner_forward_pass_kernel(struct Block *previous, struct Block *current, struct Block *next) { void planner_forward_pass_kernel(struct Block *previous, struct Block *current, struct Block *next) {
if(!current){return;} if(!current) { return; }
// If the previous block is an acceleration block, but it is not long enough to // If the previous block is an acceleration block, but it is not long enough to
// complete the full speed change within the block, we need to adjust out entry // complete the full speed change within the block, we need to adjust out entry
// speed accordingly. Remember current->entry_factor equals the exit factor of // speed accordingly. Remember current->entry_factor equals the exit factor of
@ -185,7 +225,7 @@ void planner_forward_pass_kernel(struct Block *previous, struct Block *current,
} }
} }
// recalculate_plan() 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.
void planner_forward_pass() { void planner_forward_pass() {
int8_t block_index = block_buffer_tail; int8_t block_index = block_buffer_tail;
@ -221,25 +261,26 @@ void planner_recalculate_trapezoids() {
} }
// Recalculates the motion plan according to the following algorithm: // Recalculates the motion plan according to the following algorithm:
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. Block.entry_factor) //
// so that: // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. Block.entry_factor)
// a. The junction jerk is within the set limit // so that:
// b. No speed reduction within one block requires faster accelleration than the one, true constant // a. The junction jerk is within the set limit
// acceleration. // b. No speed reduction within one block requires faster deceleration than the one, true constant
// 2. Go over every block in chronological order and dial down junction speed reduction values if // acceleration.
// a. The speed increase within one block would require faster accelleration than the one, true // 2. Go over every block in chronological order and dial down junction speed reduction values if
// constant acceleration. // a. The speed increase within one block would require faster accelleration than the one, true
// constant acceleration.
//
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to // When these stages are complete all blocks have an entry_factor that will allow all speed changes to
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than
// the set limit. Finally it will: // the set limit. Finally it will:
// 3. Recalculate trapezoids for all blocks. //
// 3. Recalculate trapezoids for all blocks.
void planner_recalculate() { void planner_recalculate() {
PORTD ^= (1<<2);
planner_reverse_pass(); planner_reverse_pass();
planner_forward_pass(); planner_forward_pass();
planner_recalculate_trapezoids(); planner_recalculate_trapezoids();
PORTD ^= (1<<2);
} }
void plan_init() { void plan_init() {
@ -322,34 +363,3 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
} }
} }
/*
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)
*/