0x90/Marlin
12
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Fixed AD595 define

Browse source
This commit is contained in:
Erik van der Zalm 2012-06-11 17:33:42 +02:00
parent d3002ef741
commit b67dacdc8f
3 changed files with 264 additions and 216 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

472
Marlin/planner.cpp
View file

@ -1,56 +1,56 @@
/* /*
planner.c - buffers movement commands and manages the acceleration profile plan planner.c - buffers movement commands and manages the acceleration profile plan
Part of Grbl Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2009-2011 Simen Svale Skogsrud
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
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
Grbl is distributed in the hope that it will be useful, Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>. along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/ */
/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ /* 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'): Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
s == speed, a == acceleration, t == time, d == distance 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)
*/
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 "Marlin.h" #include "Marlin.h"
#include "planner.h" #include "planner.h"
#include "stepper.h" #include "stepper.h"
@ -83,10 +83,10 @@ static float previous_nominal_speed; // Nominal speed of previous path line segm
extern volatile int extrudemultiply; // Sets extrude multiply factor (in percent) extern volatile int extrudemultiply; // Sets extrude multiply factor (in percent)
#ifdef AUTOTEMP #ifdef AUTOTEMP
float autotemp_max=250; float autotemp_max=250;
float autotemp_min=210; float autotemp_min=210;
float autotemp_factor=0.1; float autotemp_factor=0.1;
bool autotemp_enabled=false; bool autotemp_enabled=false;
#endif #endif
//=========================================================================== //===========================================================================
@ -100,27 +100,33 @@ volatile unsigned char block_buffer_tail; // Index of the block to pro
//=============================private variables ============================ //=============================private variables ============================
//=========================================================================== //===========================================================================
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
bool allow_cold_extrude=false; bool allow_cold_extrude=false;
#endif #endif
#ifdef XY_FREQUENCY_LIMIT #ifdef XY_FREQUENCY_LIMIT
// Used for the frequency limit // Used for the frequency limit
static unsigned char old_direction_bits = 0; // Old direction bits. Used for speed calculations static unsigned char old_direction_bits = 0; // Old direction bits. Used for speed calculations
static long x_segment_time[3]={0,0,0}; // Segment times (in us). Used for speed calculations static long x_segment_time[3]={
static long y_segment_time[3]={0,0,0}; 0,0,0}; // Segment times (in us). Used for speed calculations
static long y_segment_time[3]={
0,0,0};
#endif #endif
// 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 int8_t next_block_index(int8_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);
} }
// 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 int8_t prev_block_index(int8_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);
} }
@ -134,8 +140,8 @@ static int8_t prev_block_index(int8_t block_index) {
FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
{ {
if (acceleration!=0) { if (acceleration!=0) {
return((target_rate*target_rate-initial_rate*initial_rate)/ return((target_rate*target_rate-initial_rate*initial_rate)/
(2.0*acceleration)); (2.0*acceleration));
} }
else { else {
return 0.0; // acceleration was 0, set acceleration distance to 0 return 0.0; // acceleration was 0, set acceleration distance to 0
@ -149,9 +155,9 @@ FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float targ
FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance)
{ {
if (acceleration!=0) { if (acceleration!=0) {
return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
(4.0*acceleration) ); (4.0*acceleration) );
} }
else { else {
return 0.0; // acceleration was 0, set intersection distance to 0 return 0.0; // acceleration was 0, set intersection distance to 0
@ -165,46 +171,50 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
// Limit minimal step rate (Otherwise the timer will overflow.) // Limit minimal step rate (Otherwise the timer will overflow.)
if(initial_rate <120) {initial_rate=120; } if(initial_rate <120) {
if(final_rate < 120) {final_rate=120; } initial_rate=120;
}
if(final_rate < 120) {
final_rate=120;
}
long acceleration = block->acceleration_st; long acceleration = block->acceleration_st;
int32_t accelerate_steps = int32_t accelerate_steps =
ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration)); ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration));
int32_t decelerate_steps = int32_t decelerate_steps =
floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration)); floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration));
// Calculate the size of Plateau of Nominal Rate. // Calculate the size of Plateau of Nominal Rate.
int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking // have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block. // in order to reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) { if (plateau_steps < 0) {
accelerate_steps = ceil( accelerate_steps = ceil(
intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count)); intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count));
accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
accelerate_steps = min(accelerate_steps,block->step_event_count); accelerate_steps = min(accelerate_steps,block->step_event_count);
plateau_steps = 0; plateau_steps = 0;
} }
#ifdef ADVANCE #ifdef ADVANCE
volatile long initial_advance = block->advance*entry_factor*entry_factor; volatile long initial_advance = block->advance*entry_factor*entry_factor;
volatile long final_advance = block->advance*exit_factor*exit_factor; volatile long final_advance = block->advance*exit_factor*exit_factor;
#endif // ADVANCE #endif // ADVANCE
// block->accelerate_until = accelerate_steps; // block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps; // block->decelerate_after = accelerate_steps+plateau_steps;
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
if(block->busy == false) { // Don't update variables if block is busy. if(block->busy == false) { // Don't update variables if block is busy.
block->accelerate_until = accelerate_steps; block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps+plateau_steps; block->decelerate_after = accelerate_steps+plateau_steps;
block->initial_rate = initial_rate; block->initial_rate = initial_rate;
block->final_rate = final_rate; block->final_rate = final_rate;
#ifdef ADVANCE #ifdef ADVANCE
block->initial_advance = initial_advance; block->initial_advance = initial_advance;
block->final_advance = final_advance; block->final_advance = final_advance;
#endif //ADVANCE #endif //ADVANCE
} }
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
@ -226,24 +236,27 @@ FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity
// 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(block_t *previous, block_t *current, block_t *next) { void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!current) { return; } if(!current) {
return;
if (next) { }
if (next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed. // check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed != current->max_entry_speed) { if (current->entry_speed != current->max_entry_speed) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute // If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false. // for max allowable speed if block is decelerating and nominal length is false.
if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
current->entry_speed = min( current->max_entry_speed, current->entry_speed = min( current->max_entry_speed,
max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters));
} else { }
else {
current->entry_speed = current->max_entry_speed; current->entry_speed = current->max_entry_speed;
} }
current->recalculate_flag = true; current->recalculate_flag = true;
} }
} // Skip last block. Already initialized and set for recalculation. } // Skip last block. Already initialized and set for recalculation.
} }
@ -252,10 +265,17 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
// implements the reverse pass. // implements the reverse pass.
void planner_reverse_pass() { void planner_reverse_pass() {
uint8_t block_index = block_buffer_head; uint8_t block_index = block_buffer_head;
if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
//Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START;
unsigned char tail = block_buffer_tail;
CRITICAL_SECTION_END
if(((block_buffer_head-tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1); block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);
block_t *block[3] = { NULL, NULL, NULL }; block_t *block[3] = {
while(block_index != block_buffer_tail) { NULL, NULL, NULL };
while(block_index != tail) {
block_index = prev_block_index(block_index); block_index = prev_block_index(block_index);
block[2]= block[1]; block[2]= block[1];
block[1]= block[0]; block[1]= block[0];
@ -267,8 +287,10 @@ 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(block_t *previous, block_t *current, block_t *next) { void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!previous) { return; } if(!previous) {
return;
}
// 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
// full speed change within the block, we need to adjust the entry speed accordingly. Entry // full speed change within the block, we need to adjust the entry speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner. // speeds have already been reset, maximized, and reverse planned by reverse planner.
@ -276,7 +298,7 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n
if (!previous->nominal_length_flag) { if (!previous->nominal_length_flag) {
if (previous->entry_speed < current->entry_speed) { if (previous->entry_speed < current->entry_speed) {
double entry_speed = min( current->entry_speed, double entry_speed = min( current->entry_speed,
max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) );
// Check for junction speed change // Check for junction speed change
if (current->entry_speed != entry_speed) { if (current->entry_speed != entry_speed) {
@ -291,7 +313,8 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n
// implements the forward pass. // implements the forward pass.
void planner_forward_pass() { void planner_forward_pass() {
uint8_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) {
block[0] = block[1]; block[0] = block[1];
@ -310,7 +333,7 @@ void planner_recalculate_trapezoids() {
int8_t block_index = block_buffer_tail; int8_t block_index = block_buffer_tail;
block_t *current; block_t *current;
block_t *next = NULL; block_t *next = NULL;
while(block_index != block_buffer_head) { while(block_index != block_buffer_head) {
current = next; current = next;
next = &block_buffer[block_index]; next = &block_buffer[block_index];
@ -319,7 +342,7 @@ void planner_recalculate_trapezoids() {
if (current->recalculate_flag || next->recalculate_flag) { if (current->recalculate_flag || next->recalculate_flag) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
next->entry_speed/current->nominal_speed); next->entry_speed/current->nominal_speed);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
} }
} }
@ -328,7 +351,7 @@ void planner_recalculate_trapezoids() {
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
if(next != NULL) { if(next != NULL) {
calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
MINIMUM_PLANNER_SPEED/next->nominal_speed); MINIMUM_PLANNER_SPEED/next->nominal_speed);
next->recalculate_flag = false; next->recalculate_flag = false;
} }
} }
@ -380,14 +403,14 @@ void getHighESpeed()
if(degTargetHotend0()+2<autotemp_min) { //probably temperature set to zero. if(degTargetHotend0()+2<autotemp_min) { //probably temperature set to zero.
return; //do nothing return; //do nothing
} }
float high=0.0; float high=0.0;
uint8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail;
while(block_index != block_buffer_head) { while(block_index != block_buffer_head) {
if((block_buffer[block_index].steps_x != 0) || if((block_buffer[block_index].steps_x != 0) ||
(block_buffer[block_index].steps_y != 0) || (block_buffer[block_index].steps_y != 0) ||
(block_buffer[block_index].steps_z != 0)) { (block_buffer[block_index].steps_z != 0)) {
float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed; float se=(float(block_buffer[block_index].steps_e)/float(block_buffer[block_index].step_event_count))*block_buffer[block_index].nominal_speed;
//se; mm/sec; //se; mm/sec;
if(se>high) if(se>high)
@ -397,7 +420,7 @@ void getHighESpeed()
} }
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
} }
float g=autotemp_min+high*autotemp_factor; float g=autotemp_min+high*autotemp_factor;
float t=g; float t=g;
if(t<autotemp_min) if(t<autotemp_min)
@ -436,17 +459,21 @@ void check_axes_activity() {
} }
} }
else { else {
#if FAN_PIN > -1 #if FAN_PIN > -1
if (FanSpeed != 0){ if (FanSpeed != 0){
analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed
} }
#endif #endif
} }
if((DISABLE_X) && (x_active == 0)) disable_x(); if((DISABLE_X) && (x_active == 0)) disable_x();
if((DISABLE_Y) && (y_active == 0)) disable_y(); if((DISABLE_Y) && (y_active == 0)) disable_y();
if((DISABLE_Z) && (z_active == 0)) disable_z(); if((DISABLE_Z) && (z_active == 0)) disable_z();
if((DISABLE_E) && (e_active == 0)) { disable_e0();disable_e1();disable_e2(); } if((DISABLE_E) && (e_active == 0)) {
#if FAN_PIN > -1 disable_e0();
disable_e1();
disable_e2();
}
#if FAN_PIN > -1
if((FanSpeed == 0) && (fan_speed ==0)) { if((FanSpeed == 0) && (fan_speed ==0)) {
analogWrite(FAN_PIN, 0); analogWrite(FAN_PIN, 0);
} }
@ -454,10 +481,10 @@ void check_axes_activity() {
if (FanSpeed != 0 && tail_fan_speed !=0) { if (FanSpeed != 0 && tail_fan_speed !=0) {
analogWrite(FAN_PIN,tail_fan_speed); analogWrite(FAN_PIN,tail_fan_speed);
} }
#endif #endif
#ifdef AUTOTEMP #ifdef AUTOTEMP
getHighESpeed(); getHighESpeed();
#endif #endif
} }
@ -477,7 +504,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
manage_inactivity(1); manage_inactivity(1);
LCD_STATUS; LCD_STATUS;
} }
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
// Calculate target position in absolute steps // Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
@ -486,28 +513,28 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS]) if(target[E_AXIS]!=position[E_AXIS])
if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude) if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude)
{ {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
} }
#ifdef PREVENT_LENGTHY_EXTRUDE #ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH) if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{ {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
} }
#endif #endif
#endif #endif
// Prepare to set up new block // Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head]; block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt) // Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false; block->busy = false;
@ -521,36 +548,50 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block // Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) { return; }; if (block->step_event_count <= dropsegments) {
return;
};
block->fan_speed = FanSpeed; block->fan_speed = FanSpeed;
// Compute direction bits for this block // Compute direction bits for this block
block->direction_bits = 0; block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); } if (target[X_AXIS] < position[X_AXIS]) {
if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); } block->direction_bits |= (1<<X_AXIS);
if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); } }
if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); } if (target[Y_AXIS] < position[Y_AXIS]) {
block->direction_bits |= (1<<Y_AXIS);
}
if (target[Z_AXIS] < position[Z_AXIS]) {
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS]) {
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder; block->active_extruder = extruder;
//enable active axes //enable active axes
if(block->steps_x != 0) enable_x(); if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y(); if(block->steps_y != 0) enable_y();
#ifndef Z_LATE_ENABLE #ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z(); if(block->steps_z != 0) enable_z();
#endif #endif
// Enable all // Enable all
if(block->steps_e != 0) { enable_e0();enable_e1();enable_e2(); } if(block->steps_e != 0) {
enable_e0();
enable_e1();
enable_e2();
}
if (block->steps_e == 0) { if (block->steps_e == 0) {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
} }
else { else {
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
} }
float delta_mm[4]; float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
@ -558,37 +599,38 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0; delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) { if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) {
block->millimeters = fabs(delta_mm[E_AXIS]); block->millimeters = fabs(delta_mm[E_AXIS]);
} else { }
else {
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
} }
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds // segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second); unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) { if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) {
if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued)); inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
} }
} }
#endif #endif
// END OF SLOW DOWN SECTION // END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
// Calculate and limit speed in mm/sec for each axis // Calculate and limit speed in mm/sec for each axis
float current_speed[4]; float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++) { for(int i=0; i < 4; i++) {
@ -597,7 +639,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
} }
// Max segement time in us. // Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT #ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
@ -606,7 +648,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
old_direction_bits = block->direction_bits; old_direction_bits = block->direction_bits;
if((direction_change & (1<<X_AXIS)) == 0) { if((direction_change & (1<<X_AXIS)) == 0) {
x_segment_time[0] += segment_time; x_segment_time[0] += segment_time;
} }
else { else {
x_segment_time[2] = x_segment_time[1]; x_segment_time[2] = x_segment_time[1];
@ -614,7 +656,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
x_segment_time[0] = segment_time; x_segment_time[0] = segment_time;
} }
if((direction_change & (1<<Y_AXIS)) == 0) { if((direction_change & (1<<Y_AXIS)) == 0) {
y_segment_time[0] += segment_time; y_segment_time[0] += segment_time;
} }
else { else {
y_segment_time[2] = y_segment_time[1]; y_segment_time[2] = y_segment_time[1];
@ -655,7 +697,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
} }
block->acceleration = block->acceleration_st / steps_per_mm; block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
#if 0 // Use old jerk for now #if 0 // Use old jerk for now
// Compute path unit vector // Compute path unit vector
double unit_vec[3]; double unit_vec[3];
@ -663,7 +705,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
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. // 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 // 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, // deviation is defined as the distance from the junction to the closest edge of the circle,
@ -680,9 +722,9 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction. // Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) { if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed); vmax_junction = min(previous_nominal_speed,block->nominal_speed);
@ -691,36 +733,39 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
// Compute maximum junction velocity based on maximum acceleration and junction deviation // 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. double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction, vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
} }
} }
} }
#endif #endif
// Start with a safe speed // Start with a safe speed
float vmax_junction = max_xy_jerk/2; float vmax_junction = max_xy_jerk/2;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
vmax_junction = max_z_jerk/2; vmax_junction = min(vmax_junction, max_z_jerk/2);
vmax_junction = min(vmax_junction, block->nominal_speed);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
vmax_junction = min(vmax_junction, max_e_jerk/2); vmax_junction = min(vmax_junction, max_e_jerk/2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { // if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed; vmax_junction = block->nominal_speed;
} // }
if (jerk > max_xy_jerk) { if (jerk > max_xy_jerk) {
vmax_junction *= (max_xy_jerk/jerk); vmax_junction_factor = (max_xy_jerk/jerk);
} }
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction *= (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])); vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
} }
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) { if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction *= (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])); vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
} }
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
} }
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. // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable); block->entry_speed = min(vmax_junction, v_allowable);
@ -733,48 +778,52 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both // 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 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. // 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; } if (block->nominal_speed <= v_allowable) {
else { block->nominal_length_flag = false; } block->nominal_length_flag = true;
}
else {
block->nominal_length_flag = false;
}
block->recalculate_flag = true; // Always calculate trapezoid for new block block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed; previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE #ifdef ADVANCE
// Calculate advance rate // Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0; block->advance_rate = 0;
block->advance = 0; }
}
else { else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); block->advance_rate = advance / (float)acc_dist;
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
} }
/* }
/*
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :"); SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0); SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :"); SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0); SERIAL_ECHOLN(block->advance_rate/256.0);
*/ */
#endif // ADVANCE #endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
MINIMUM_PLANNER_SPEED/block->nominal_speed); safe_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[]
@ -805,12 +854,13 @@ void plan_set_e_position(const float &e)
uint8_t movesplanned() uint8_t movesplanned()
{ {
return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); return (block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
} }
void allow_cold_extrudes(bool allow) void allow_cold_extrudes(bool allow)
{ {
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
allow_cold_extrude=allow; allow_cold_extrude=allow;
#endif #endif
} }

4
Marlin/stepper.cpp
View file

@ -261,12 +261,10 @@ FORCE_INLINE void trapezoid_generator_reset() {
#endif #endif
deceleration_time = 0; deceleration_time = 0;
// step_rate to timer interval // step_rate to timer interval
OCR1A_nominal = calc_timer(current_block->nominal_rate);
acc_step_rate = current_block->initial_rate; acc_step_rate = current_block->initial_rate;
acceleration_time = calc_timer(acc_step_rate); acceleration_time = calc_timer(acc_step_rate);
OCR1A = acceleration_time; OCR1A = acceleration_time;
OCR1A_nominal = calc_timer(current_block->nominal_rate);
// SERIAL_ECHO_START; // SERIAL_ECHO_START;
// SERIAL_ECHOPGM("advance :"); // SERIAL_ECHOPGM("advance :");

4
Marlin/ultralcd.pde
View file

@ -957,7 +957,7 @@ enum {
#if EXTRUDERS > 2 #if EXTRUDERS > 2
ItemCT_nozzle2, ItemCT_nozzle2,
#endif #endif
#if defined BED_USES_THERMISTOR || BED_USES_AD595 #if defined BED_USES_THERMISTOR || defined BED_USES_AD595
ItemCT_bed, ItemCT_bed,
#endif #endif
ItemCT_fan, ItemCT_fan,
@ -1212,7 +1212,7 @@ void MainMenu::showControlTemp()
}break; }break;
#endif //autotemp #endif //autotemp
#if defined BED_USES_THERMISTOR || BED_USES_AD595 #if defined BED_USES_THERMISTOR || defined BED_USES_AD595
case ItemCT_bed: case ItemCT_bed:
{ {
if(force_lcd_update) if(force_lcd_update)