diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index f0dbe6a6e7..f1f0c651cf 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -9,6 +9,19 @@ //#define BAUDRATE 230400 +// Frequency limit +// See nophead's blog for more info +// Not working OK +//#define XY_FREQUENCY_LIMIT 15 + +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec) + +// If defined the movements slow down when the look ahead buffer is only half full +#define SLOWDOWN + // BASIC SETTINGS: select your board type, thermistor type, axis scaling, and endstop configuration //// The following define selects which electronics board you have. Please choose the one that matches your setup @@ -45,7 +58,6 @@ //#define BED_USES_THERMISTOR //#define BED_USES_AD595 -#define HEATER_CHECK_INTERVAL 50 //ms #define BED_CHECK_INTERVAL 5000 //ms //// Experimental watchdog and minimal temp @@ -103,11 +115,15 @@ #ifdef PID_PID //PID according to Ziegler-Nichols method - #define DEFAULT_Kp (0.6*PID_CRITIAL_GAIN) - #define DEFAULT_Ki (2*Kp/PID_SWING_AT_CRITIAL*PID_dT) - #define DEFAULT_Kd (PID_SWING_AT_CRITIAL/8./PID_dT) +// #define DEFAULT_Kp (0.6*PID_CRITIAL_GAIN) +// #define DEFAULT_Ki (2*Kp/PID_SWING_AT_CRITIAL*PID_dT) +// #define DEFAULT_Kd (PID_SWING_AT_CRITIAL/8./PID_dT) + + #define DEFAULT_Kp 22.2 + #define DEFAULT_Ki (1.25*PID_dT) + #define DEFAULT_Kd (99/PID_dT) #endif - + #ifdef PID_PI //PI according to Ziegler-Nichols method #define DEFAULT_Kp (PID_CRITIAL_GAIN/2.2) @@ -156,6 +172,11 @@ const bool ENDSTOPS_INVERTING = true; // set to true to invert the logic of the #define DISABLE_E false // Inverting axis direction +//#define INVERT_X_DIR false // for Mendel set to false, for Orca set to true +//#define INVERT_Y_DIR true // for Mendel set to true, for Orca set to false +//#define INVERT_Z_DIR false // for Mendel set to false, for Orca set to true +//#define INVERT_E_DIR true // for direct drive extruder v9 set to true, for geared extruder set to false + #define INVERT_X_DIR true // for Mendel set to false, for Orca set to true #define INVERT_Y_DIR false // for Mendel set to true, for Orca set to false #define INVERT_Z_DIR true // for Mendel set to false, for Orca set to true @@ -176,7 +197,7 @@ const bool ENDSTOPS_INVERTING = true; // set to true to invert the logic of the //// MOVEMENT SETTINGS #define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E //note: on bernhards ultimaker 200 200 12 are working well. -#define HOMING_FEEDRATE {50*60, 50*60, 12*60, 0} // set the homing speeds +#define HOMING_FEEDRATE {50*60, 50*60, 4*60, 0} // set the homing speeds (mm/min) #define AXIS_RELATIVE_MODES {false, false, false, false} @@ -184,20 +205,21 @@ const bool ENDSTOPS_INVERTING = true; // set to true to invert the logic of the // default settings -#define DEFAULT_AXIS_STEPS_PER_UNIT {79.87220447,79.87220447,200*8/3,14} // default steps per unit for ultimaker -#define DEFAULT_MAX_FEEDRATE {160*60, 160*60, 10*60, 500000} -#define DEFAULT_MAX_ACCELERATION {9000,9000,150,10000} // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for skeinforge 40+, for older versions raise them a lot. +#define DEFAULT_AXIS_STEPS_PER_UNIT {79.87220447,79.87220447,200*8/3,760*1.1} // default steps per unit for ultimaker +//#define DEFAULT_AXIS_STEPS_PER_UNIT {40, 40, 3333.92, 67} +#define DEFAULT_MAX_FEEDRATE {500, 500, 5, 200000} // (mm/sec) +#define DEFAULT_MAX_ACCELERATION {9000,9000,100,10000} // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for skeinforge 40+, for older versions raise them a lot. #define DEFAULT_ACCELERATION 3000 // X, Y, Z and E max acceleration in mm/s^2 for printing moves #define DEFAULT_RETRACT_ACCELERATION 7000 // X, Y, Z and E max acceleration in mm/s^2 for r retracts -#define DEFAULT_MINIMUMFEEDRATE 10 // minimum feedrate -#define DEFAULT_MINTRAVELFEEDRATE 10 +#define DEFAULT_MINIMUMFEEDRATE 0.0 // minimum feedrate +#define DEFAULT_MINTRAVELFEEDRATE 0.0 // minimum time in microseconds that a movement needs to take if the buffer is emptied. Increase this number if you see blobs while printing high speed & high detail. It will slowdown on the detailed stuff. -#define DEFAULT_MINSEGMENTTIME 20000 -#define DEFAULT_XYJERK 30.0*60 -#define DEFAULT_ZJERK 10.0*60 +#define DEFAULT_MINSEGMENTTIME 20000 // Obsolete delete this +#define DEFAULT_XYJERK 30.0 // (mm/sec) +#define DEFAULT_ZJERK 0.4 // (mm/sec) diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde index cacd728742..f808742c9c 100644 --- a/Marlin/Marlin.pde +++ b/Marlin/Marlin.pde @@ -116,7 +116,9 @@ extern float HeaterPower; //=========================================================================== //=============================public variables============================= //=========================================================================== +#ifdef SDSUPPORT CardReader card; +#endif float homing_feedrate[] = HOMING_FEEDRATE; bool axis_relative_modes[] = AXIS_RELATIVE_MODES; volatile int feedmultiply=100; //100->1 200->2 @@ -193,32 +195,8 @@ extern "C"{ -inline void get_coordinates() -{ - for(int8_t i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i]; - else destination[i] = current_position[i]; //Are these else lines really needed? - } - if(code_seen('F')) { - next_feedrate = code_value(); - if(next_feedrate > 0.0) feedrate = next_feedrate; - } -} -inline void get_arc_coordinates() -{ - get_coordinates(); - if(code_seen('I')) offset[0] = code_value(); - if(code_seen('J')) offset[1] = code_value(); -} -void prepare_move() -{ - plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60.0/100.0); - for(int8_t i=0; i < NUM_AXIS; i++) { - current_position[i] = destination[i]; - } -} @@ -273,7 +251,9 @@ void loop() { if(buflen<3) get_command(); + #ifdef SDSUPPORT card.checkautostart(false); + #endif if(buflen) { #ifdef SDSUPPORT @@ -1008,13 +988,38 @@ void ClearToSend() SERIAL_PROTOCOLLNPGM("ok"); } +inline void get_coordinates() +{ + for(int8_t i=0; i < NUM_AXIS; i++) { + if(code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i]; + else destination[i] = current_position[i]; //Are these else lines really needed? + } + if(code_seen('F')) { + next_feedrate = code_value(); + if(next_feedrate > 0.0) feedrate = next_feedrate; + } +} +inline void get_arc_coordinates() +{ + get_coordinates(); + if(code_seen('I')) offset[0] = code_value(); + if(code_seen('J')) offset[1] = code_value(); +} + +void prepare_move() +{ + plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0); + for(int8_t i=0; i < NUM_AXIS; i++) { + current_position[i] = destination[i]; + } +} void prepare_arc_move(char isclockwise) { float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc // Trace the arc - mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60.0/100.0, r, isclockwise); + mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise); // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 863b116a5b..c27d586017 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1,629 +1,747 @@ -/* - planner.c - buffers movement commands and manages the acceleration profile plan - 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 . -*/ - -/* 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 - -#include "Marlin.h" -#include "Configuration.h" -#include "pins.h" -#include "fastio.h" -#include "planner.h" -#include "stepper.h" -#include "temperature.h" -#include "ultralcd.h" - -//=========================================================================== -//=============================public variables ============================ -//=========================================================================== - -unsigned long minsegmenttime; -float max_feedrate[4]; // set the max speeds -float axis_steps_per_unit[4]; -long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software -float minimumfeedrate; -float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX -float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX -float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. -float max_z_jerk; -float mintravelfeedrate; -unsigned long axis_steps_per_sqr_second[NUM_AXIS]; - -// The current position of the tool in absolute steps -long position[4]; //rescaled from extern when axis_steps_per_unit are changed by gcode - -#ifdef AUTOTEMP -float high_e_speed=0; -#endif - - -//=========================================================================== -//=============================private variables ============================ -//=========================================================================== -static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions -static volatile unsigned char block_buffer_head; // Index of the next block to be pushed -static volatile unsigned char block_buffer_tail; // Index of the block to process now - - - -//=========================================================================== -//=============================functions ============================ -//=========================================================================== -#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 - -// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the -// given acceleration: -inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) { - if (acceleration!=0) { - return((target_rate*target_rate-initial_rate*initial_rate)/ - (2.0*acceleration)); - } - else { - return 0.0; // acceleration was 0, set acceleration distance to 0 - } -} - -// 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) - -inline float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) { - if (acceleration!=0) { - return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ - (4.0*acceleration) ); - } - else { - return 0.0; // acceleration was 0, set intersection distance to 0 - } -} - -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. - -void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) { - if(block->busy == true) return; // If block is busy then bail out. - float entry_factor = entry_speed / block->nominal_speed; - float exit_factor = exit_speed / block->nominal_speed; - long initial_rate = ceil(block->nominal_rate*entry_factor); - long final_rate = ceil(block->nominal_rate*exit_factor); - - #ifdef ADVANCE - long initial_advance = block->advance*entry_factor*entry_factor; - long final_advance = block->advance*exit_factor*exit_factor; - #endif // ADVANCE - - // Limit minimal step rate (Otherwise the timer will overflow.) - if(initial_rate <120) initial_rate=120; - if(final_rate < 120) final_rate=120; - - // Calculate the acceleration steps - long acceleration = block->acceleration_st; - long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration); - long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration); - // Calculate the size of Plateau of Nominal Rate. - long 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 - // 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. - if (plateau_steps < 0) { - accelerate_steps = intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count); - plateau_steps = 0; - } - - long decelerate_after = accelerate_steps+plateau_steps; - - 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. - block->accelerate_until = accelerate_steps; - block->decelerate_after = decelerate_after; - block->initial_rate = initial_rate; - block->final_rate = final_rate; - #ifdef ADVANCE - block->initial_advance = initial_advance; - block->final_advance = final_advance; - #endif //ADVANCE - } - CRITICAL_SECTION_END; -} - -// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the -// acceleration within the allotted distance. -inline float max_allowable_speed(float acceleration, float target_velocity, float distance) { - return sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance); -} - -// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. -// This method will calculate the junction jerk as the euclidean distance between the nominal -// velocities of the respective blocks. -inline float junction_jerk(block_t *before, block_t *after) { - return sqrt( - pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2)); -} - -// Return the safe speed which is max_jerk/2, e.g. the -// speed under which you cannot exceed max_jerk no matter what you do. -float safe_speed(block_t *block) { - float safe_speed; - safe_speed = max_xy_jerk/2; - if(abs(block->speed_z) > max_z_jerk/2) - safe_speed = max_z_jerk/2; - if (safe_speed > block->nominal_speed) - safe_speed = block->nominal_speed; - return safe_speed; -} - -// 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) { - if(!current) { - return; - } - - float entry_speed = current->nominal_speed; - float exit_factor; - float exit_speed; - if (next) { - exit_speed = next->entry_speed; - } - else { - exit_speed = safe_speed(current); - } - - // Calculate the entry_factor for the current block. - if (previous) { - // Reduce speed so that junction_jerk is within the maximum allowed - float jerk = junction_jerk(previous, current); - if((previous->steps_x == 0) && (previous->steps_y == 0)) { - entry_speed = safe_speed(current); - } - else if (jerk > max_xy_jerk) { - entry_speed = (max_xy_jerk/jerk) * entry_speed; - } - if(abs(previous->speed_z - current->speed_z) > max_z_jerk) { - entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed; - } - // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. - if (entry_speed > exit_speed) { - float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters); - if (max_entry_speed < entry_speed) { - entry_speed = max_entry_speed; - } - } - } - else { - entry_speed = safe_speed(current); - } - // Store result - current->entry_speed = entry_speed; -} - -// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This -// implements the reverse pass. -void planner_reverse_pass() { - char block_index = block_buffer_head; - if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) { - block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1); - block_t *block[5] = { - NULL, NULL, NULL, NULL, NULL }; - while(block_index != block_buffer_tail) { - block_index = (block_index-1) & (BLOCK_BUFFER_SIZE -1); - block[2]= block[1]; - block[1]= block[0]; - block[0] = &block_buffer[block_index]; - planner_reverse_pass_kernel(block[0], block[1], block[2]); - } - planner_reverse_pass_kernel(NULL, block[0], block[1]); - } -} - -// 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) { - if(!current) { - return; - } - if(previous) { - // 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 - // speed accordingly. Remember current->entry_factor equals the exit factor of - // the previous block. - if(previous->entry_speed < current->entry_speed) { - float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters); - if (max_entry_speed < current->entry_speed) { - current->entry_speed = max_entry_speed; - } - } - } -} - -// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This -// implements the forward pass. -void planner_forward_pass() { - char block_index = block_buffer_tail; - block_t *block[3] = { - NULL, NULL, NULL }; - - while(block_index != block_buffer_head) { - block[0] = block[1]; - block[1] = block[2]; - block[2] = &block_buffer[block_index]; - planner_forward_pass_kernel(block[0],block[1],block[2]); - block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); - } - planner_forward_pass_kernel(block[1], block[2], NULL); -} - -// Recalculates the trapezoid speed profiles for all blocks in the plan according to the -// entry_factor for each junction. Must be called by planner_recalculate() after -// updating the blocks. -void planner_recalculate_trapezoids() { - char block_index = block_buffer_tail; - block_t *current; - block_t *next = NULL; - while(block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; - if (current) { - calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed); - } - block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); - } - calculate_trapezoid_for_block(next, next->entry_speed, safe_speed(next)); -} - -// 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_t.entry_factor) -// so that: -// a. The junction jerk is within the set limit -// b. No speed reduction within one block requires faster deceleration than the one, true constant -// acceleration. -// 2. Go over every block in chronological order and dial down junction speed reduction values if -// 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 -// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than -// the set limit. Finally it will: -// -// 3. Recalculate trapezoids for all blocks. - -void planner_recalculate() { - planner_reverse_pass(); - planner_forward_pass(); - planner_recalculate_trapezoids(); -} - -void plan_init() { - block_buffer_head = 0; - block_buffer_tail = 0; - memset(position, 0, sizeof(position)); // clear position -} - - -void plan_discard_current_block() { - if (block_buffer_head != block_buffer_tail) { - block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1); - } -} - -block_t *plan_get_current_block() { - if (block_buffer_head == block_buffer_tail) { - return(NULL); - } - block_t *block = &block_buffer[block_buffer_tail]; - block->busy = true; - return(block); -} - -#ifdef AUTOTEMP -void getHighESpeed() -{ - if(degTargetHotend0()+2high) - { - high=se; - } - block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); - } - high_e_speed=high*axis_steps_per_unit[E_AXIS]/(1000000.0); //so it is independent of the esteps/mm. before - - float g=AUTOTEMP_MIN+high_e_speed*AUTOTEMP_FACTOR; - float t=constrain(AUTOTEMP_MIN,g,AUTOTEMP_MAX); - setTargetHotend0(t); - SERIAL_ECHO_START; - SERIAL_ECHOPAIR("highe",high_e_speed); - SERIAL_ECHOPAIR(" t",t); - SERIAL_ECHOLN(""); -} -#endif - -void check_axes_activity() { - unsigned char x_active = 0; - unsigned char y_active = 0; - unsigned char z_active = 0; - unsigned char e_active = 0; - block_t *block; - - if(block_buffer_tail != block_buffer_head) { - char block_index = block_buffer_tail; - while(block_index != block_buffer_head) { - block = &block_buffer[block_index]; - if(block->steps_x != 0) x_active++; - if(block->steps_y != 0) y_active++; - if(block->steps_z != 0) z_active++; - if(block->steps_e != 0) e_active++; - block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); - } - } - if((DISABLE_X) && (x_active == 0)) disable_x(); - if((DISABLE_Y) && (y_active == 0)) disable_y(); - if((DISABLE_Z) && (z_active == 0)) disable_z(); - if((DISABLE_E) && (e_active == 0)) disable_e(); -} - -// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in -// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration -// calculation the caller must also provide the physical length of the line in millimeters. -void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate) -{ - // Calculate the buffer head after we push this byte - int next_buffer_head = (block_buffer_head + 1) & (BLOCK_BUFFER_SIZE - 1); - - // If the buffer is full: good! That means we are well ahead of the robot. - // Rest here until there is room in the buffer. - while(block_buffer_tail == next_buffer_head) { - manage_heater(); - manage_inactivity(1); - LCD_STATUS; - } - - // The target position of the tool 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 - long target[4]; - target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); - target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); - target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); - target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); - - // Prepare to set up new block - block_t *block = &block_buffer[block_buffer_head]; - - // Mark block as not busy (Not executed by the stepper interrupt) - block->busy = false; - - // Number of steps for each axis - block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); - block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); - block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); - block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); - 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 - if (block->step_event_count <=dropsegments) { - return; - }; - - //enable active axes - if(block->steps_x != 0) enable_x(); - if(block->steps_y != 0) enable_y(); - if(block->steps_z != 0) enable_z(); - if(block->steps_e != 0) enable_e(); - - float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; - float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; - float delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; - float delta_e_mm = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS]; - block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm) + square(delta_e_mm)); - - unsigned long microseconds; - - if (block->steps_e == 0) { - if(feed_ratemillimeters/feed_rate)*1000000); - - // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill - // reduces/removes corner blobs as the machine won't come to a full stop. - int blockcount=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); - - if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) { - if (microsecondsspeed_z = delta_z_mm * multiplier; - block->speed_x = delta_x_mm * multiplier; - block->speed_y = delta_y_mm * multiplier; - block->speed_e = delta_e_mm * multiplier; - - - // Limit speed per axis - float speed_factor = 1; //factor <=1 do decrease speed - if(abs(block->speed_x) > max_feedrate[X_AXIS]) { - speed_factor = max_feedrate[X_AXIS] / abs(block->speed_x); - //if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; /is not need here because auf the init above - } - if(abs(block->speed_y) > max_feedrate[Y_AXIS]){ - float tmp_speed_factor = max_feedrate[Y_AXIS] / abs(block->speed_y); - if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; - } - if(abs(block->speed_z) > max_feedrate[Z_AXIS]){ - float tmp_speed_factor = max_feedrate[Z_AXIS] / abs(block->speed_z); - if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; - } - if(abs(block->speed_e) > max_feedrate[E_AXIS]){ - float tmp_speed_factor = max_feedrate[E_AXIS] / abs(block->speed_e); - if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; - } - multiplier = multiplier * speed_factor; - block->speed_z = delta_z_mm * multiplier; - block->speed_x = delta_x_mm * multiplier; - block->speed_y = delta_y_mm * multiplier; - block->speed_e = delta_e_mm * multiplier; - block->nominal_speed = block->millimeters * multiplier; - block->nominal_rate = ceil(block->step_event_count * multiplier / 60); - - if(block->nominal_rate < 120) - block->nominal_rate = 120; - block->entry_speed = safe_speed(block); - - // Compute the acceleration rate for the trapezoid generator. - float travel_per_step = block->millimeters/block->step_event_count; - if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) { - block->acceleration_st = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2 - } - else { - block->acceleration_st = ceil( (acceleration)/travel_per_step); // convert to: acceleration steps/sec^2 - float tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count; - // Limit acceleration per axis - if((tmp_acceleration * block->steps_x) > axis_steps_per_sqr_second[X_AXIS]) { - block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; - tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count; - } - if((tmp_acceleration * block->steps_y) > axis_steps_per_sqr_second[Y_AXIS]) { - block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; - tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count; - } - if((tmp_acceleration * block->steps_e) > axis_steps_per_sqr_second[E_AXIS]) { - block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; - tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count; - } - if((tmp_acceleration * block->steps_z) > axis_steps_per_sqr_second[Z_AXIS]) { - block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; - tmp_acceleration = (float)block->acceleration_st / (float)block->step_event_count; - } - } - block->acceleration = block->acceleration_st * travel_per_step; - block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); - - #ifdef ADVANCE - // Calculate advance rate - 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) * - (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536; - block->advance = advance; - if(acc_dist == 0) { - block->advance_rate = 0; - } - else { - block->advance_rate = advance / (float)acc_dist; - } - } - #endif // ADVANCE - - // compute a preliminary conservative acceleration trapezoid - float safespeed = safe_speed(block); - calculate_trapezoid_for_block(block, safespeed, safespeed); - - // Compute direction bits for this block - block->direction_bits = 0; - if (target[X_AXIS] < position[X_AXIS]) { - block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<. +*/ + +/* 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 + +#include "Marlin.h" +#include "Configuration.h" +#include "pins.h" +#include "fastio.h" +#include "planner.h" +#include "stepper.h" +#include "temperature.h" +#include "ultralcd.h" + +//=========================================================================== +//=============================public variables ============================ +//=========================================================================== + +unsigned long minsegmenttime; +float max_feedrate[4]; // set the max speeds +float axis_steps_per_unit[4]; +unsigned long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software +float minimumfeedrate; +float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX +float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX +float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. +float max_z_jerk; +float mintravelfeedrate; +unsigned long axis_steps_per_sqr_second[NUM_AXIS]; + +// The current position of the tool in absolute steps +long position[4]; //rescaled from extern when axis_steps_per_unit are changed by gcode +static float previous_speed[4]; // Speed of previous path line segment +static float previous_nominal_speed; // Nominal speed of previous path line segment + +#ifdef AUTOTEMP +float high_e_speed=0; +#endif + + +//=========================================================================== +//=============================private variables ============================ +//=========================================================================== +static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions +static volatile unsigned char block_buffer_head; // Index of the next block to be pushed +static volatile unsigned char block_buffer_tail; // Index of the block to process now + +// Used for the frequency limit +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 y_segment_time[3]={0,0,0}; + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static int8_t next_block_index(int8_t block_index) { + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static int8_t prev_block_index(int8_t block_index) { + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + +//=========================================================================== +//=============================functions ============================ +//=========================================================================== + +// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the +// given acceleration: +inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) { + if (acceleration!=0) { + return((target_rate*target_rate-initial_rate*initial_rate)/ + (2.0*acceleration)); + } + else { + return 0.0; // acceleration was 0, set acceleration distance to 0 + } +} + +// 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) + +inline float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) { + if (acceleration!=0) { + return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ + (4.0*acceleration) ); + } + else { + return 0.0; // acceleration was 0, set intersection distance to 0 + } +} + +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. + +void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) { + unsigned long initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) + unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) + + // Limit minimal step rate (Otherwise the timer will overflow.) + if(initial_rate <120) {initial_rate=120; } + if(final_rate < 120) {final_rate=120; } + + long acceleration = block->acceleration_st; + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration)); + + // Calculate the size of Plateau of Nominal Rate. + 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 + // 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. + if (plateau_steps < 0) { + accelerate_steps = ceil( + 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 = min(accelerate_steps,block->step_event_count); + plateau_steps = 0; + } + + #ifdef ADVANCE + long initial_advance = block->advance*entry_factor*entry_factor; + long final_advance = block->advance*exit_factor*exit_factor; + #endif // ADVANCE + + // block->accelerate_until = accelerate_steps; + // block->decelerate_after = accelerate_steps+plateau_steps; + + 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. + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps+plateau_steps; + block->initial_rate = initial_rate; + block->final_rate = final_rate; + #ifdef ADVANCE + block->initial_advance = initial_advance; + block->final_advance = final_advance; + #endif //ADVANCE + } + CRITICAL_SECTION_END; +} + +// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the +// acceleration within the allotted distance. +inline float max_allowable_speed(float acceleration, float target_velocity, float 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. +// This method will calculate the junction jerk as the euclidean distance between the nominal +// velocities of the respective blocks. +//inline float junction_jerk(block_t *before, block_t *after) { +// return sqrt( +// pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2)); +//} + + +// 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) { + if(!current) { return; } + + if (next) { + // 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 + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (current->entry_speed != current->max_entry_speed) { + + // 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. + if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { + current->entry_speed = min( current->max_entry_speed, + max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); + } else { + current->entry_speed = current->max_entry_speed; + } + current->recalculate_flag = true; + + } + } // Skip last block. Already initialized and set for recalculation. +} + +// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This +// implements the reverse pass. +void planner_reverse_pass() { + char block_index = block_buffer_head; + if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) { + block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1); + block_t *block[3] = { NULL, NULL, NULL }; + while(block_index != block_buffer_tail) { + block_index = prev_block_index(block_index); + block[2]= block[1]; + block[1]= block[0]; + block[0] = &block_buffer[block_index]; + planner_reverse_pass_kernel(block[0], block[1], block[2]); + } + } +} + +// 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) { + if(!previous) { return; } + + // 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 + // speeds have already been reset, maximized, and reverse planned by reverse planner. + // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. + if (!previous->nominal_length_flag) { + if (previous->entry_speed < current->entry_speed) { + double entry_speed = min( current->entry_speed, + max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); + + // Check for junction speed change + if (current->entry_speed != entry_speed) { + current->entry_speed = entry_speed; + current->recalculate_flag = true; + } + } + } +} + +// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This +// implements the forward pass. +void planner_forward_pass() { + char block_index = block_buffer_tail; + block_t *block[3] = { NULL, NULL, NULL }; + + while(block_index != block_buffer_head) { + block[0] = block[1]; + block[1] = block[2]; + block[2] = &block_buffer[block_index]; + planner_forward_pass_kernel(block[0],block[1],block[2]); + block_index = next_block_index(block_index); + } + planner_forward_pass_kernel(block[1], block[2], NULL); +} + +// Recalculates the trapezoid speed profiles for all blocks in the plan according to the +// entry_factor for each junction. Must be called by planner_recalculate() after +// updating the blocks. +void planner_recalculate_trapezoids() { + int8_t block_index = block_buffer_tail; + block_t *current; + block_t *next = NULL; + + while(block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + if (current) { + // Recalculate if current block entry or exit junction speed has changed. + if (current->recalculate_flag || next->recalculate_flag) { + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, + next->entry_speed/current->nominal_speed); + current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + } + } + block_index = next_block_index( block_index ); + } + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + if(next != NULL) { + calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, + MINIMUM_PLANNER_SPEED/next->nominal_speed); + next->recalculate_flag = false; + } +} + +// 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_t.entry_factor) +// so that: +// a. The junction jerk is within the set limit +// b. No speed reduction within one block requires faster deceleration than the one, true constant +// acceleration. +// 2. Go over every block in chronological order and dial down junction speed reduction values if +// 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 +// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than +// the set limit. Finally it will: +// +// 3. Recalculate trapezoids for all blocks. + +void planner_recalculate() { + planner_reverse_pass(); + planner_forward_pass(); + planner_recalculate_trapezoids(); +} + +void plan_init() { + block_buffer_head = 0; + block_buffer_tail = 0; + memset(position, 0, sizeof(position)); // clear position + previous_speed[0] = 0.0; + previous_speed[1] = 0.0; + previous_speed[2] = 0.0; + previous_speed[3] = 0.0; + previous_nominal_speed = 0.0; +} + + +void plan_discard_current_block() { + if (block_buffer_head != block_buffer_tail) { + block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1); + } +} + +block_t *plan_get_current_block() { + if (block_buffer_head == block_buffer_tail) { + return(NULL); + } + block_t *block = &block_buffer[block_buffer_tail]; + block->busy = true; + return(block); +} + +#ifdef AUTOTEMP +void getHighESpeed() +{ + if(degTargetHotend0()+2high) + { + high=se; + } + block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); + } + high_e_speed=high*axis_steps_per_unit[E_AXIS]/(1000000.0); //so it is independent of the esteps/mm. before + + float g=AUTOTEMP_MIN+high_e_speed*AUTOTEMP_FACTOR; + float t=constrain(AUTOTEMP_MIN,g,AUTOTEMP_MAX); + setTargetHotend0(t); + SERIAL_ECHO_START; + SERIAL_ECHOPAIR("highe",high_e_speed); + SERIAL_ECHOPAIR(" t",t); + SERIAL_ECHOLN(""); +} +#endif + +void check_axes_activity() { + unsigned char x_active = 0; + unsigned char y_active = 0; + unsigned char z_active = 0; + unsigned char e_active = 0; + block_t *block; + + if(block_buffer_tail != block_buffer_head) { + char block_index = block_buffer_tail; + while(block_index != block_buffer_head) { + block = &block_buffer[block_index]; + if(block->steps_x != 0) x_active++; + if(block->steps_y != 0) y_active++; + if(block->steps_z != 0) z_active++; + if(block->steps_e != 0) e_active++; + block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); + } + } + if((DISABLE_X) && (x_active == 0)) disable_x(); + if((DISABLE_Y) && (y_active == 0)) disable_y(); + if((DISABLE_Z) && (z_active == 0)) disable_z(); + if((DISABLE_E) && (e_active == 0)) disable_e(); +} + + +float junction_deviation = 0.1; +// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in +// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration +// calculation the caller must also provide the physical length of the line in millimeters. +void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate) +{ + // Calculate the buffer head after we push this byte + int next_buffer_head = next_block_index(block_buffer_head); + + // If the buffer is full: good! That means we are well ahead of the robot. + // Rest here until there is room in the buffer. + while(block_buffer_tail == next_buffer_head) { + manage_heater(); + manage_inactivity(1); + LCD_STATUS; + } + + // The target position of the tool 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 + long target[4]; + target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); + target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); + target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); + target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); + + // Prepare to set up new block + block_t *block = &block_buffer[block_buffer_head]; + + // Mark block as not busy (Not executed by the stepper interrupt) + block->busy = false; + + // Number of steps for each axis + block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); + block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); + block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); + block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); + 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 + if (block->step_event_count <=dropsegments) { return; }; + + // Compute direction bits for this block + block->direction_bits = 0; + if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<steps_x != 0) enable_x(); + if(block->steps_y != 0) enable_y(); + if(block->steps_z != 0) enable_z(); + if(block->steps_e != 0) enable_e(); + + float delta_mm[4]; + 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[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; + delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS]; + block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + + square(delta_mm[Z_AXIS]) + square(delta_mm[E_AXIS])); + 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; + + 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 + + // segment time im micro seconds + long segment_time = lround(1000000.0/inverse_second); + + + if (block->steps_e == 0) { + if(feed_rate0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) { + if (segment_time max_feedrate[i]) + speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + } + +// Max segement time in us. +#ifdef XY_FREQUENCY_LIMIT +#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) + + // Check and limit the xy direction change frequency + unsigned char direction_change = block->direction_bits ^ old_direction_bits; + old_direction_bits = block->direction_bits; + + if((direction_change & (1< max_feedrate[i]) + speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + /* + if(speed_factor < 0.1) { + Serial.print("speed factor : "); Serial.println(speed_factor); + Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]); + } + */ + } + for(unsigned char i=0; i < 4; i++) { + current_speed[i] *= speed_factor; + } + block->nominal_speed *= speed_factor; + block->nominal_rate *= speed_factor; + } + + // Compute and limit the acceleration rate for the trapezoid generator. + float steps_per_mm = block->step_event_count/block->millimeters; + if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) { + block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + } + else { + block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + // Limit acceleration per axis + if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) + block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; + if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) + block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; + if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) + block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; + if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) + block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; + } + block->acceleration = block->acceleration_st / steps_per_mm; + block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); + +#if 0 // Use old jerk for now + // Compute path unit vector + double unit_vec[3]; + + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_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. + if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { + // 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. + double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - 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(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + } + } + } +#endif + // Start with a safe speed + float vmax_junction = max_xy_jerk/2; + if(abs(current_speed[Z_AXIS]) > max_z_jerk/2) + vmax_junction = max_z_jerk/2; + vmax_junction = min(vmax_junction, block->nominal_speed); + + if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { + float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { + vmax_junction = block->nominal_speed; + } + if (jerk > max_xy_jerk) { + vmax_junction *= (max_xy_jerk/jerk); + } + if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { + vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])); + } + } + 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(-block->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 + memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + previous_nominal_speed = block->nominal_speed; + + #ifdef ADVANCE + // Calculate advance rate + 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) * + (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536; + block->advance = advance; + if(acc_dist == 0) { + block->advance_rate = 0; + } + else { + block->advance_rate = advance / (float)acc_dist; + } + } + #endif // ADVANCE + + + + + calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, + MINIMUM_PLANNER_SPEED/block->nominal_speed); + + // Move buffer head + block_buffer_head = next_buffer_head; + + // Update position + memcpy(position, target, sizeof(target)); // position[] = target[] + + planner_recalculate(); + #ifdef AUTOTEMP + getHighESpeed(); + #endif + st_wake_up(); +} + +void plan_set_position(const float &x, const float &y, const float &z, const float &e) +{ + position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); + position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); + position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); + position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); + previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. + previous_speed[0] = 0.0; + previous_speed[1] = 0.0; + previous_speed[2] = 0.0; + previous_speed[3] = 0.0; +} + diff --git a/Marlin/planner.h b/Marlin/planner.h index c5bc5b8aaf..be1587d6b8 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -1,95 +1,98 @@ -/* - planner.h - buffers movement commands and manages the acceleration profile plan - 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 . -*/ - -// 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 - -#include "Configuration.h" - -// 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. -typedef struct { - // Fields used by the bresenham algorithm for tracing the line - long steps_x, steps_y, steps_z, steps_e; // Step count along each axis - long step_event_count; // The number of step events required to complete this block - volatile long accelerate_until; // The index of the step event on which to stop acceleration - volatile long decelerate_after; // The index of the step event on which to start decelerating - volatile long acceleration_rate; // The acceleration rate used for acceleration calculation - unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - #ifdef ADVANCE - long advance_rate; - volatile long initial_advance; - volatile long final_advance; - float advance; - #endif - - // Fields used by the motion planner to manage acceleration - float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis - float nominal_speed; // The nominal speed for this block in mm/min - float millimeters; // The total travel of this block in mm - float entry_speed; - float acceleration; // acceleration mm/sec^2 - - // Settings for the trapezoid generator - long nominal_rate; // The nominal step rate for this block in step_events/sec - volatile long initial_rate; // The jerk-adjusted step rate at start of block - volatile long final_rate; // The minimal rate at exit - long acceleration_st; // acceleration steps/sec^2 - volatile char busy; -} block_t; - -// Initialize the motion plan subsystem -void plan_init(); - -// 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. -void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate); - -// Set position. Used for G92 instructions. -void plan_set_position(const float &x, const float &y, const float &z, const float &e); - - -// Called when the current block is no longer needed. Discards the block and makes the memory -// availible for new blocks. -void plan_discard_current_block(); - -// Gets the current block. Returns NULL if buffer empty -block_t *plan_get_current_block(); - -void check_axes_activity(); - -extern unsigned long minsegmenttime; -extern float max_feedrate[4]; // set the max speeds -extern float axis_steps_per_unit[4]; -extern long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software -extern float minimumfeedrate; -extern float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX -extern float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX -extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. -extern float max_z_jerk; -extern float mintravelfeedrate; -extern unsigned long axis_steps_per_sqr_second[NUM_AXIS]; -#ifdef AUTOTEMP -extern float high_e_speed; -#endif -#endif +/* + planner.h - buffers movement commands and manages the acceleration profile plan + 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 . +*/ + +// 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 + +#include "Configuration.h" + +// 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. +typedef struct { + // Fields used by the bresenham algorithm for tracing the line + long steps_x, steps_y, steps_z, steps_e; // Step count along each axis + long step_event_count; // The number of step events required to complete this block + long accelerate_until; // The index of the step event on which to stop acceleration + long decelerate_after; // The index of the step event on which to start decelerating + long acceleration_rate; // The acceleration rate used for acceleration calculation + unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + #ifdef ADVANCE +// long advance_rate; +// volatile long initial_advance; +// volatile long final_advance; +// float advance; + #endif + + // Fields used by the motion planner to manage acceleration +// float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis + float nominal_speed; // The nominal speed for this block in mm/min + float entry_speed; // Entry speed at previous-current junction in mm/min + float max_entry_speed; // Maximum allowable junction entry speed in mm/min + float millimeters; // The total travel of this block in mm + float acceleration; // acceleration mm/sec^2 + unsigned char recalculate_flag; // Planner flag to recalculate trapezoids on entry junction + unsigned char nominal_length_flag; // Planner flag for nominal speed always reached + + // Settings for the trapezoid generator + unsigned long nominal_rate; // The nominal step rate for this block in step_events/sec + unsigned long initial_rate; // The jerk-adjusted step rate at start of block + unsigned long final_rate; // The minimal rate at exit + unsigned long acceleration_st; // acceleration steps/sec^2 + volatile char busy; +} block_t; + +// Initialize the motion plan subsystem +void plan_init(); + +// 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. +void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate); + +// Set position. Used for G92 instructions. +void plan_set_position(const float &x, const float &y, const float &z, const float &e); + + +// Called when the current block is no longer needed. Discards the block and makes the memory +// availible for new blocks. +void plan_discard_current_block(); + +// Gets the current block. Returns NULL if buffer empty +block_t *plan_get_current_block(); + +void check_axes_activity(); + +extern unsigned long minsegmenttime; +extern float max_feedrate[4]; // set the max speeds +extern float axis_steps_per_unit[4]; +extern unsigned long max_acceleration_units_per_sq_second[4]; // Use M201 to override by software +extern float minimumfeedrate; +extern float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX +extern float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX +extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. +extern float max_z_jerk; +extern float mintravelfeedrate; +extern unsigned long axis_steps_per_sqr_second[NUM_AXIS]; +#ifdef AUTOTEMP +extern float high_e_speed; +#endif +#endif diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 23066ef320..2e232201bf 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -1,664 +1,669 @@ -/* - stepper.c - stepper motor driver: executes motion plans using stepper motors - 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 . -*/ - -/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith - and Philipp Tiefenbacher. */ - -#include "stepper.h" -#include "Configuration.h" -#include "Marlin.h" -#include "planner.h" -#include "pins.h" -#include "fastio.h" -#include "temperature.h" -#include "ultralcd.h" - -#include "speed_lookuptable.h" - - - -//=========================================================================== -//=============================public variables ============================ -//=========================================================================== -block_t *current_block; // A pointer to the block currently being traced - - - -//=========================================================================== -//=============================private variables ============================ -//=========================================================================== -//static makes it inpossible to be called from outside of this file by extern.! - -// Variables used by The Stepper Driver Interrupt -static unsigned char out_bits; // The next stepping-bits to be output -static long counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z, - counter_e; -static unsigned long step_events_completed; // The number of step events executed in the current block -#ifdef ADVANCE - static long advance_rate, advance, final_advance = 0; - static short old_advance = 0; - static short e_steps; -#endif -static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. -static long acceleration_time, deceleration_time; -//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; -static unsigned short acc_step_rate; // needed for deccelaration start point -static char step_loops; - -volatile long endstops_trigsteps[3]={0,0,0}; -volatile long endstops_stepsTotal,endstops_stepsDone; -static volatile bool endstops_hit=false; - -// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer. -// for debugging purposes only, should be disabled by default -#ifdef DEBUG_STEPS - volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0}; - volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1}; -#endif - -//=========================================================================== -//=============================functions ============================ -//=========================================================================== - - -// intRes = intIn1 * intIn2 >> 16 -// uses: -// r26 to store 0 -// r27 to store the byte 1 of the 24 bit result -#define MultiU16X8toH16(intRes, charIn1, intIn2) \ -asm volatile ( \ -"clr r26 \n\t" \ -"mul %A1, %B2 \n\t" \ -"movw %A0, r0 \n\t" \ -"mul %A1, %A2 \n\t" \ -"add %A0, r1 \n\t" \ -"adc %B0, r26 \n\t" \ -"lsr r0 \n\t" \ -"adc %A0, r26 \n\t" \ -"adc %B0, r26 \n\t" \ -"clr r1 \n\t" \ -: \ -"=&r" (intRes) \ -: \ -"d" (charIn1), \ -"d" (intIn2) \ -: \ -"r26" \ -) - -// intRes = longIn1 * longIn2 >> 24 -// uses: -// r26 to store 0 -// r27 to store the byte 1 of the 48bit result -#define MultiU24X24toH16(intRes, longIn1, longIn2) \ -asm volatile ( \ -"clr r26 \n\t" \ -"mul %A1, %B2 \n\t" \ -"mov r27, r1 \n\t" \ -"mul %B1, %C2 \n\t" \ -"movw %A0, r0 \n\t" \ -"mul %C1, %C2 \n\t" \ -"add %B0, r0 \n\t" \ -"mul %C1, %B2 \n\t" \ -"add %A0, r0 \n\t" \ -"adc %B0, r1 \n\t" \ -"mul %A1, %C2 \n\t" \ -"add r27, r0 \n\t" \ -"adc %A0, r1 \n\t" \ -"adc %B0, r26 \n\t" \ -"mul %B1, %B2 \n\t" \ -"add r27, r0 \n\t" \ -"adc %A0, r1 \n\t" \ -"adc %B0, r26 \n\t" \ -"mul %C1, %A2 \n\t" \ -"add r27, r0 \n\t" \ -"adc %A0, r1 \n\t" \ -"adc %B0, r26 \n\t" \ -"mul %B1, %A2 \n\t" \ -"add r27, r1 \n\t" \ -"adc %A0, r26 \n\t" \ -"adc %B0, r26 \n\t" \ -"lsr r27 \n\t" \ -"adc %A0, r26 \n\t" \ -"adc %B0, r26 \n\t" \ -"clr r1 \n\t" \ -: \ -"=&r" (intRes) \ -: \ -"d" (longIn1), \ -"d" (longIn2) \ -: \ -"r26" , "r27" \ -) - -// Some useful constants - -#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<step_event_count; - endstops_stepsDone=stepstaken; - endstops_trigsteps[0]=current_block->steps_x; - endstops_trigsteps[1]=current_block->steps_y; - endstops_trigsteps[2]=current_block->steps_z; - - endstops_hit=true; -} - -void checkHitEndstops() -{ - if( !endstops_hit) - return; - float endstops_triggerpos[3]={0,0,0}; - float ratiodone=endstops_stepsDone/float(endstops_stepsTotal); //ratio of current_block thas was performed - - endstops_triggerpos[0]=current_position[0]-(endstops_trigsteps[0]*ratiodone)/float(axis_steps_per_unit[0]); - endstops_triggerpos[1]=current_position[1]-(endstops_trigsteps[1]*ratiodone)/float(axis_steps_per_unit[1]); - endstops_triggerpos[2]=current_position[2]-(endstops_trigsteps[2]*ratiodone)/float(axis_steps_per_unit[2]); - SERIAL_ECHO_START; - SERIAL_ECHOPGM("endstops hit: "); - SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]); - SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]); - SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]); - SERIAL_ECHOLN(""); - endstops_hit=false; -} - -void endstops_hit_on_purpose() -{ - endstops_hit=false; -} - -// __________________________ -// /| |\ _________________ ^ -// / | | \ /| |\ | -// / | | \ / | | \ s -// / | | | | | \ p -// / | | | | | \ e -// +-----+------------------------+---+--+---------------+----+ e -// | BLOCK 1 | BLOCK 2 | d -// -// time -----> -// -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates -// first block->accelerate_until step_events_completed, then keeps going at constant speed until -// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. -// The slope of acceleration is calculated with the leib ramp alghorithm. - -void st_wake_up() { - // TCNT1 = 0; - ENABLE_STEPPER_DRIVER_INTERRUPT(); -} - -inline unsigned short calc_timer(unsigned short step_rate) { - unsigned short timer; - if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; - - if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times - step_rate = step_rate >> 2; - step_loops = 4; - } - else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times - step_rate = step_rate >> 1; - step_loops = 2; - } - else { - step_loops = 1; - } - - if(step_rate < 32) step_rate = 32; - step_rate -= 32; // Correct for minimal speed - if(step_rate >= (8*256)){ // higher step rate - unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0]; - unsigned char tmp_step_rate = (step_rate & 0x00ff); - unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2); - MultiU16X8toH16(timer, tmp_step_rate, gain); - timer = (unsigned short)pgm_read_word_near(table_address) - timer; - } - else { // lower step rates - unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0]; - table_address += ((step_rate)>>1) & 0xfffc; - timer = (unsigned short)pgm_read_word_near(table_address); - timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3); - } - if(timer < 100) timer = 100; - return timer; -} - -// Initializes the trapezoid generator from the current block. Called whenever a new -// block begins. -inline void trapezoid_generator_reset() { - #ifdef ADVANCE - advance = current_block->initial_advance; - final_advance = current_block->final_advance; - #endif - deceleration_time = 0; - // advance_rate = current_block->advance_rate; - // step_rate to timer interval - acc_step_rate = current_block->initial_rate; - acceleration_time = calc_timer(acc_step_rate); - OCR1A = acceleration_time; -} - -// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse. -// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. -ISR(TIMER1_COMPA_vect) -{ - if(busy){ - SERIAL_ERROR_START - SERIAL_ERROR(*(unsigned short *)OCR1A); - SERIAL_ERRORLNPGM(" ISR overtaking itself."); - return; - } // The busy-flag is used to avoid reentering this interrupt - - busy = true; - sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) - - // If there is no current block, attempt to pop one from the buffer - if (current_block == NULL) { - // Anything in the buffer? - current_block = plan_get_current_block(); - if (current_block != NULL) { - trapezoid_generator_reset(); - counter_x = -(current_block->step_event_count >> 1); - counter_y = counter_x; - counter_z = counter_x; - counter_e = counter_x; - step_events_completed = 0; - #ifdef ADVANCE - e_steps = 0; - #endif - } - else { -// DISABLE_STEPPER_DRIVER_INTERRUPT(); - } - } - - if (current_block != NULL) { - // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt - out_bits = current_block->direction_bits; - - #ifdef ADVANCE - // Calculate E early. - counter_e += current_block->steps_e; - if (counter_e > 0) { - counter_e -= current_block->step_event_count; - if ((out_bits & (1<> 16) - old_advance); - CRITICAL_SECTION_END; - old_advance = advance >> 16; - #endif //ADVANCE - - // Set direction en check limit switches - if ((out_bits & (1< -1 - if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) { - endstops_triggered(step_events_completed); - step_events_completed = current_block->step_event_count; - } - #endif - } - else { // +direction - WRITE(X_DIR_PIN,!INVERT_X_DIR); - #ifdef DEBUG_STEPS - count_direction[X_AXIS]=1; - #endif - #if X_MAX_PIN > -1 - if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x >0)){ - endstops_triggered(step_events_completed); - step_events_completed = current_block->step_event_count; - } - #endif - } - - if ((out_bits & (1< -1 - if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) { - endstops_triggered(step_events_completed); - step_events_completed = current_block->step_event_count; - } - #endif - } - else { // +direction - WRITE(Y_DIR_PIN,!INVERT_Y_DIR); - #ifdef DEBUG_STEPS - count_direction[Y_AXIS]=1; - #endif - #if Y_MAX_PIN > -1 - if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){ - endstops_triggered(step_events_completed); - step_events_completed = current_block->step_event_count; - } - #endif - } - - if ((out_bits & (1< -1 - if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) { - endstops_triggered(step_events_completed); - step_events_completed = current_block->step_event_count; - } - #endif - } - else { // +direction - WRITE(Z_DIR_PIN,!INVERT_Z_DIR); - #ifdef DEBUG_STEPS - count_direction[Z_AXIS]=1; - #endif - #if Z_MAX_PIN > -1 - if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z >0)){ - endstops_triggered(step_events_completed); - step_events_completed = current_block->step_event_count; - } - #endif - } - - #ifndef ADVANCE - if ((out_bits & (1<steps_x; - if (counter_x > 0) { - WRITE(X_STEP_PIN, HIGH); - counter_x -= current_block->step_event_count; - WRITE(X_STEP_PIN, LOW); - #ifdef DEBUG_STEPS - count_position[X_AXIS]+=count_direction[X_AXIS]; - #endif - } - - counter_y += current_block->steps_y; - if (counter_y > 0) { - WRITE(Y_STEP_PIN, HIGH); - counter_y -= current_block->step_event_count; - WRITE(Y_STEP_PIN, LOW); - #ifdef DEBUG_STEPS - count_position[Y_AXIS]+=count_direction[Y_AXIS]; - #endif - } - - counter_z += current_block->steps_z; - if (counter_z > 0) { - WRITE(Z_STEP_PIN, HIGH); - counter_z -= current_block->step_event_count; - WRITE(Z_STEP_PIN, LOW); - #ifdef DEBUG_STEPS - count_position[Z_AXIS]+=count_direction[Z_AXIS]; - #endif - } - - #ifndef ADVANCE - counter_e += current_block->steps_e; - if (counter_e > 0) { - WRITE(E_STEP_PIN, HIGH); - counter_e -= current_block->step_event_count; - WRITE(E_STEP_PIN, LOW); - } - #endif //!ADVANCE - step_events_completed += 1; - if(step_events_completed >= current_block->step_event_count) break; - } - // Calculare new timer value - unsigned short timer; - unsigned short step_rate; - if (step_events_completed <= current_block->accelerate_until) { - MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); - acc_step_rate += current_block->initial_rate; - - // upper limit - if(acc_step_rate > current_block->nominal_rate) - acc_step_rate = current_block->nominal_rate; - - // step_rate to timer interval - timer = calc_timer(acc_step_rate); - #ifdef ADVANCE - advance += advance_rate; - #endif - acceleration_time += timer; - OCR1A = timer; - } - else if (step_events_completed > current_block->decelerate_after) { - MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); - - if(step_rate > acc_step_rate) { // Check step_rate stays positive - step_rate = current_block->final_rate; - } - else { - step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. - } - - // lower limit - if(step_rate < current_block->final_rate) - step_rate = current_block->final_rate; - - // step_rate to timer interval - timer = calc_timer(step_rate); - #ifdef ADVANCE - advance -= advance_rate; - if(advance < final_advance) - advance = final_advance; - #endif //ADVANCE - deceleration_time += timer; - OCR1A = timer; - } - // If current block is finished, reset pointer - if (step_events_completed >= current_block->step_event_count) { - current_block = NULL; - plan_discard_current_block(); - } - } - cli(); // disable interrupts - busy=false; -} - -#ifdef ADVANCE - unsigned char old_OCR0A; - // Timer interrupt for E. e_steps is set in the main routine; - // Timer 0 is shared with millies - ISR(TIMER0_COMPA_vect) - { - // Critical section needed because Timer 1 interrupt has higher priority. - // The pin set functions are placed on trategic position to comply with the stepper driver timing. - WRITE(E_STEP_PIN, LOW); - // Set E direction (Depends on E direction + advance) - if (e_steps < 0) { - WRITE(E_DIR_PIN,INVERT_E_DIR); - e_steps++; - WRITE(E_STEP_PIN, HIGH); - } - if (e_steps > 0) { - WRITE(E_DIR_PIN,!INVERT_E_DIR); - e_steps--; - WRITE(E_STEP_PIN, HIGH); - } - old_OCR0A += 25; // 10kHz interrupt - OCR0A = old_OCR0A; - } -#endif // ADVANCE - -void st_init() -{ - //Initialize Dir Pins - #if X_DIR_PIN > -1 - SET_OUTPUT(X_DIR_PIN); - #endif - #if Y_DIR_PIN > -1 - SET_OUTPUT(Y_DIR_PIN); - #endif - #if Z_DIR_PIN > -1 - SET_OUTPUT(Z_DIR_PIN); - #endif - #if E_DIR_PIN > -1 - SET_OUTPUT(E_DIR_PIN); - #endif - - //Initialize Enable Pins - steppers default to disabled. - - #if (X_ENABLE_PIN > -1) - SET_OUTPUT(X_ENABLE_PIN); - if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH); - #endif - #if (Y_ENABLE_PIN > -1) - SET_OUTPUT(Y_ENABLE_PIN); - if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH); - #endif - #if (Z_ENABLE_PIN > -1) - SET_OUTPUT(Z_ENABLE_PIN); - if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH); - #endif - #if (E_ENABLE_PIN > -1) - SET_OUTPUT(E_ENABLE_PIN); - if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH); - #endif - - //endstops and pullups - #ifdef ENDSTOPPULLUPS - #if X_MIN_PIN > -1 - SET_INPUT(X_MIN_PIN); - WRITE(X_MIN_PIN,HIGH); - #endif - #if X_MAX_PIN > -1 - SET_INPUT(X_MAX_PIN); - WRITE(X_MAX_PIN,HIGH); - #endif - #if Y_MIN_PIN > -1 - SET_INPUT(Y_MIN_PIN); - WRITE(Y_MIN_PIN,HIGH); - #endif - #if Y_MAX_PIN > -1 - SET_INPUT(Y_MAX_PIN); - WRITE(Y_MAX_PIN,HIGH); - #endif - #if Z_MIN_PIN > -1 - SET_INPUT(Z_MIN_PIN); - WRITE(Z_MIN_PIN,HIGH); - #endif - #if Z_MAX_PIN > -1 - SET_INPUT(Z_MAX_PIN); - WRITE(Z_MAX_PIN,HIGH); - #endif - #else //ENDSTOPPULLUPS - #if X_MIN_PIN > -1 - SET_INPUT(X_MIN_PIN); - #endif - #if X_MAX_PIN > -1 - SET_INPUT(X_MAX_PIN); - #endif - #if Y_MIN_PIN > -1 - SET_INPUT(Y_MIN_PIN); - #endif - #if Y_MAX_PIN > -1 - SET_INPUT(Y_MAX_PIN); - #endif - #if Z_MIN_PIN > -1 - SET_INPUT(Z_MIN_PIN); - #endif - #if Z_MAX_PIN > -1 - SET_INPUT(Z_MAX_PIN); - #endif - #endif //ENDSTOPPULLUPS - - - //Initialize Step Pins - #if (X_STEP_PIN > -1) - SET_OUTPUT(X_STEP_PIN); - #endif - #if (Y_STEP_PIN > -1) - SET_OUTPUT(Y_STEP_PIN); - #endif - #if (Z_STEP_PIN > -1) - SET_OUTPUT(Z_STEP_PIN); - #endif - #if (E_STEP_PIN > -1) - SET_OUTPUT(E_STEP_PIN); - #endif - - // waveform generation = 0100 = CTC - TCCR1B &= ~(1<. +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include "stepper.h" +#include "Configuration.h" +#include "Marlin.h" +#include "planner.h" +#include "pins.h" +#include "fastio.h" +#include "temperature.h" +#include "ultralcd.h" + +#include "speed_lookuptable.h" + + + +//=========================================================================== +//=============================public variables ============================ +//=========================================================================== +block_t *current_block; // A pointer to the block currently being traced + + + +//=========================================================================== +//=============================private variables ============================ +//=========================================================================== +//static makes it inpossible to be called from outside of this file by extern.! + +// Variables used by The Stepper Driver Interrupt +static unsigned char out_bits; // The next stepping-bits to be output +static long counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z, + counter_e; +static unsigned long step_events_completed; // The number of step events executed in the current block +#ifdef ADVANCE + static long advance_rate, advance, final_advance = 0; + static short old_advance = 0; + static short e_steps; +#endif +static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. +static long acceleration_time, deceleration_time; +//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; +static unsigned short acc_step_rate; // needed for deccelaration start point +static char step_loops; + +volatile long endstops_trigsteps[3]={0,0,0}; +volatile long endstops_stepsTotal,endstops_stepsDone; +static volatile bool endstops_hit=false; + +// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer. +// for debugging purposes only, should be disabled by default +#ifdef DEBUG_STEPS + volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0}; + volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1}; +#endif + +//=========================================================================== +//=============================functions ============================ +//=========================================================================== + + +// intRes = intIn1 * intIn2 >> 16 +// uses: +// r26 to store 0 +// r27 to store the byte 1 of the 24 bit result +#define MultiU16X8toH16(intRes, charIn1, intIn2) \ +asm volatile ( \ +"clr r26 \n\t" \ +"mul %A1, %B2 \n\t" \ +"movw %A0, r0 \n\t" \ +"mul %A1, %A2 \n\t" \ +"add %A0, r1 \n\t" \ +"adc %B0, r26 \n\t" \ +"lsr r0 \n\t" \ +"adc %A0, r26 \n\t" \ +"adc %B0, r26 \n\t" \ +"clr r1 \n\t" \ +: \ +"=&r" (intRes) \ +: \ +"d" (charIn1), \ +"d" (intIn2) \ +: \ +"r26" \ +) + +// intRes = longIn1 * longIn2 >> 24 +// uses: +// r26 to store 0 +// r27 to store the byte 1 of the 48bit result +#define MultiU24X24toH16(intRes, longIn1, longIn2) \ +asm volatile ( \ +"clr r26 \n\t" \ +"mul %A1, %B2 \n\t" \ +"mov r27, r1 \n\t" \ +"mul %B1, %C2 \n\t" \ +"movw %A0, r0 \n\t" \ +"mul %C1, %C2 \n\t" \ +"add %B0, r0 \n\t" \ +"mul %C1, %B2 \n\t" \ +"add %A0, r0 \n\t" \ +"adc %B0, r1 \n\t" \ +"mul %A1, %C2 \n\t" \ +"add r27, r0 \n\t" \ +"adc %A0, r1 \n\t" \ +"adc %B0, r26 \n\t" \ +"mul %B1, %B2 \n\t" \ +"add r27, r0 \n\t" \ +"adc %A0, r1 \n\t" \ +"adc %B0, r26 \n\t" \ +"mul %C1, %A2 \n\t" \ +"add r27, r0 \n\t" \ +"adc %A0, r1 \n\t" \ +"adc %B0, r26 \n\t" \ +"mul %B1, %A2 \n\t" \ +"add r27, r1 \n\t" \ +"adc %A0, r26 \n\t" \ +"adc %B0, r26 \n\t" \ +"lsr r27 \n\t" \ +"adc %A0, r26 \n\t" \ +"adc %B0, r26 \n\t" \ +"clr r1 \n\t" \ +: \ +"=&r" (intRes) \ +: \ +"d" (longIn1), \ +"d" (longIn2) \ +: \ +"r26" , "r27" \ +) + +// Some useful constants + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<step_event_count; + endstops_stepsDone=stepstaken; + endstops_trigsteps[0]=current_block->steps_x; + endstops_trigsteps[1]=current_block->steps_y; + endstops_trigsteps[2]=current_block->steps_z; + + endstops_hit=true; +} + +void checkHitEndstops() +{ + if( !endstops_hit) + return; + float endstops_triggerpos[3]={0,0,0}; + float ratiodone=endstops_stepsDone/float(endstops_stepsTotal); //ratio of current_block thas was performed + + endstops_triggerpos[0]=current_position[0]-(endstops_trigsteps[0]*ratiodone)/float(axis_steps_per_unit[0]); + endstops_triggerpos[1]=current_position[1]-(endstops_trigsteps[1]*ratiodone)/float(axis_steps_per_unit[1]); + endstops_triggerpos[2]=current_position[2]-(endstops_trigsteps[2]*ratiodone)/float(axis_steps_per_unit[2]); + SERIAL_ECHO_START; + SERIAL_ECHOPGM("endstops hit: "); + SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]); + SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]); + SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]); + SERIAL_ECHOLN(""); + endstops_hit=false; +} + +void endstops_hit_on_purpose() +{ + endstops_hit=false; +} + +// __________________________ +// /| |\ _________________ ^ +// / | | \ /| |\ | +// / | | \ / | | \ s +// / | | | | | \ p +// / | | | | | \ e +// +-----+------------------------+---+--+---------------+----+ e +// | BLOCK 1 | BLOCK 2 | d +// +// time -----> +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates +// first block->accelerate_until step_events_completed, then keeps going at constant speed until +// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. +// The slope of acceleration is calculated with the leib ramp alghorithm. + +void st_wake_up() { + // TCNT1 = 0; + if(busy == false) + ENABLE_STEPPER_DRIVER_INTERRUPT(); +} + +inline unsigned short calc_timer(unsigned short step_rate) { + unsigned short timer; + if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; + + if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times + step_rate = (step_rate >> 2)&0x3fff; + step_loops = 4; + } + else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times + step_rate = (step_rate >> 1)&0x7fff; + step_loops = 2; + } + else { + step_loops = 1; + } + + if(step_rate < 32) step_rate = 32; + step_rate -= 32; // Correct for minimal speed + if(step_rate >= (8*256)){ // higher step rate + unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0]; + unsigned char tmp_step_rate = (step_rate & 0x00ff); + unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2); + MultiU16X8toH16(timer, tmp_step_rate, gain); + timer = (unsigned short)pgm_read_word_near(table_address) - timer; + } + else { // lower step rates + unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0]; + table_address += ((step_rate)>>1) & 0xfffc; + timer = (unsigned short)pgm_read_word_near(table_address); + timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3); + } + if(timer < 100) { timer = 100; Serial.print("Steprate to high : "); Serial.println(step_rate); }//(20kHz this should never happen) + return timer; +} + +// Initializes the trapezoid generator from the current block. Called whenever a new +// block begins. +inline void trapezoid_generator_reset() { + #ifdef ADVANCE + advance = current_block->initial_advance; + final_advance = current_block->final_advance; + #endif + deceleration_time = 0; + // step_rate to timer interval + acc_step_rate = current_block->initial_rate; + acceleration_time = calc_timer(acc_step_rate); + OCR1A = acceleration_time; +} + +// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse. +// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. +ISR(TIMER1_COMPA_vect) +{ + if(busy){ + SERIAL_ERROR_START + SERIAL_ERROR(*(unsigned short *)OCR1A); + SERIAL_ERRORLNPGM(" ISR overtaking itself."); + return; + } // The busy-flag is used to avoid reentering this interrupt + + busy = true; + sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) + + // If there is no current block, attempt to pop one from the buffer + if (current_block == NULL) { + // Anything in the buffer? + current_block = plan_get_current_block(); + if (current_block != NULL) { + trapezoid_generator_reset(); + counter_x = -(current_block->step_event_count >> 1); + counter_y = counter_x; + counter_z = counter_x; + counter_e = counter_x; + step_events_completed = 0; + #ifdef ADVANCE + e_steps = 0; + #endif + } + else { +// DISABLE_STEPPER_DRIVER_INTERRUPT(); + } + } + + if (current_block != NULL) { + // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt + out_bits = current_block->direction_bits; + + #ifdef ADVANCE + // Calculate E early. + counter_e += current_block->steps_e; + if (counter_e > 0) { + counter_e -= current_block->step_event_count; + if ((out_bits & (1<> 16) - old_advance); + CRITICAL_SECTION_END; + old_advance = advance >> 16; + #endif //ADVANCE + + // Set direction en check limit switches + if ((out_bits & (1< -1 + if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) { + // endstops_triggered(step_events_completed); + step_events_completed = current_block->step_event_count; + } + #endif + } + else { // +direction + WRITE(X_DIR_PIN,!INVERT_X_DIR); + #ifdef DEBUG_STEPS + count_direction[X_AXIS]=1; + #endif + #if X_MAX_PIN > -1 + if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x >0)){ + // endstops_triggered(step_events_completed); + step_events_completed = current_block->step_event_count; + } + #endif + } + + if ((out_bits & (1< -1 + if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) { + // endstops_triggered(step_events_completed); + step_events_completed = current_block->step_event_count; + } + #endif + } + else { // +direction + WRITE(Y_DIR_PIN,!INVERT_Y_DIR); + #ifdef DEBUG_STEPS + count_direction[Y_AXIS]=1; + #endif + #if Y_MAX_PIN > -1 + if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){ + // endstops_triggered(step_events_completed); + step_events_completed = current_block->step_event_count; + } + #endif + } + + if ((out_bits & (1< -1 + if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) { + endstops_triggered(step_events_completed); + step_events_completed = current_block->step_event_count; + } + #endif + } + else { // +direction + WRITE(Z_DIR_PIN,!INVERT_Z_DIR); + #ifdef DEBUG_STEPS + count_direction[Z_AXIS]=1; + #endif + #if Z_MAX_PIN > -1 + if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z >0)){ + // endstops_triggered(step_events_completed); + step_events_completed = current_block->step_event_count; + } + #endif + } + + #ifndef ADVANCE + if ((out_bits & (1<steps_x; + if (counter_x > 0) { + WRITE(X_STEP_PIN, HIGH); + counter_x -= current_block->step_event_count; + WRITE(X_STEP_PIN, LOW); + #ifdef DEBUG_STEPS + count_position[X_AXIS]+=count_direction[X_AXIS]; + #endif + } + + counter_y += current_block->steps_y; + if (counter_y > 0) { + WRITE(Y_STEP_PIN, HIGH); + counter_y -= current_block->step_event_count; + WRITE(Y_STEP_PIN, LOW); + #ifdef DEBUG_STEPS + count_position[Y_AXIS]+=count_direction[Y_AXIS]; + #endif + } + + counter_z += current_block->steps_z; + if (counter_z > 0) { + WRITE(Z_STEP_PIN, HIGH); + counter_z -= current_block->step_event_count; + WRITE(Z_STEP_PIN, LOW); + #ifdef DEBUG_STEPS + count_position[Z_AXIS]+=count_direction[Z_AXIS]; + #endif + } + + #ifndef ADVANCE + counter_e += current_block->steps_e; + if (counter_e > 0) { + WRITE(E_STEP_PIN, HIGH); + counter_e -= current_block->step_event_count; + WRITE(E_STEP_PIN, LOW); + } + #endif //!ADVANCE + step_events_completed += 1; + if(step_events_completed >= current_block->step_event_count) break; + } + // Calculare new timer value + unsigned short timer; + unsigned short step_rate; + if (step_events_completed <= current_block->accelerate_until) { + MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); + acc_step_rate += current_block->initial_rate; + + // upper limit + if(acc_step_rate > current_block->nominal_rate) + acc_step_rate = current_block->nominal_rate; + + // step_rate to timer interval + timer = calc_timer(acc_step_rate); + #ifdef ADVANCE + advance += advance_rate; + #endif + acceleration_time += timer; + OCR1A = timer; + } + else if (step_events_completed > current_block->decelerate_after) { + MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); + + if(step_rate > acc_step_rate) { // Check step_rate stays positive + step_rate = current_block->final_rate; + } + else { + step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. + } + + // lower limit + if(step_rate < current_block->final_rate) + step_rate = current_block->final_rate; + + // step_rate to timer interval + timer = calc_timer(step_rate); + #ifdef ADVANCE + advance -= advance_rate; + if(advance < final_advance) + advance = final_advance; + #endif //ADVANCE + deceleration_time += timer; + OCR1A = timer; + } + else { + timer = calc_timer(current_block->nominal_rate); + OCR1A = timer; + } + + // If current block is finished, reset pointer + if (step_events_completed >= current_block->step_event_count) { + current_block = NULL; + plan_discard_current_block(); + } + } + cli(); // disable interrupts + busy=false; +} + +#ifdef ADVANCE + unsigned char old_OCR0A; + // Timer interrupt for E. e_steps is set in the main routine; + // Timer 0 is shared with millies + ISR(TIMER0_COMPA_vect) + { + // Critical section needed because Timer 1 interrupt has higher priority. + // The pin set functions are placed on trategic position to comply with the stepper driver timing. + WRITE(E_STEP_PIN, LOW); + // Set E direction (Depends on E direction + advance) + if (e_steps < 0) { + WRITE(E_DIR_PIN,INVERT_E_DIR); + e_steps++; + WRITE(E_STEP_PIN, HIGH); + } + if (e_steps > 0) { + WRITE(E_DIR_PIN,!INVERT_E_DIR); + e_steps--; + WRITE(E_STEP_PIN, HIGH); + } + old_OCR0A += 25; // 10kHz interrupt + OCR0A = old_OCR0A; + } +#endif // ADVANCE + +void st_init() +{ + //Initialize Dir Pins + #if X_DIR_PIN > -1 + SET_OUTPUT(X_DIR_PIN); + #endif + #if Y_DIR_PIN > -1 + SET_OUTPUT(Y_DIR_PIN); + #endif + #if Z_DIR_PIN > -1 + SET_OUTPUT(Z_DIR_PIN); + #endif + #if E_DIR_PIN > -1 + SET_OUTPUT(E_DIR_PIN); + #endif + + //Initialize Enable Pins - steppers default to disabled. + + #if (X_ENABLE_PIN > -1) + SET_OUTPUT(X_ENABLE_PIN); + if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH); + #endif + #if (Y_ENABLE_PIN > -1) + SET_OUTPUT(Y_ENABLE_PIN); + if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH); + #endif + #if (Z_ENABLE_PIN > -1) + SET_OUTPUT(Z_ENABLE_PIN); + if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH); + #endif + #if (E_ENABLE_PIN > -1) + SET_OUTPUT(E_ENABLE_PIN); + if(!E_ENABLE_ON) WRITE(E_ENABLE_PIN,HIGH); + #endif + + //endstops and pullups + #ifdef ENDSTOPPULLUPS + #if X_MIN_PIN > -1 + SET_INPUT(X_MIN_PIN); + WRITE(X_MIN_PIN,HIGH); + #endif + #if X_MAX_PIN > -1 + SET_INPUT(X_MAX_PIN); + WRITE(X_MAX_PIN,HIGH); + #endif + #if Y_MIN_PIN > -1 + SET_INPUT(Y_MIN_PIN); + WRITE(Y_MIN_PIN,HIGH); + #endif + #if Y_MAX_PIN > -1 + SET_INPUT(Y_MAX_PIN); + WRITE(Y_MAX_PIN,HIGH); + #endif + #if Z_MIN_PIN > -1 + SET_INPUT(Z_MIN_PIN); + WRITE(Z_MIN_PIN,HIGH); + #endif + #if Z_MAX_PIN > -1 + SET_INPUT(Z_MAX_PIN); + WRITE(Z_MAX_PIN,HIGH); + #endif + #else //ENDSTOPPULLUPS + #if X_MIN_PIN > -1 + SET_INPUT(X_MIN_PIN); + #endif + #if X_MAX_PIN > -1 + SET_INPUT(X_MAX_PIN); + #endif + #if Y_MIN_PIN > -1 + SET_INPUT(Y_MIN_PIN); + #endif + #if Y_MAX_PIN > -1 + SET_INPUT(Y_MAX_PIN); + #endif + #if Z_MIN_PIN > -1 + SET_INPUT(Z_MIN_PIN); + #endif + #if Z_MAX_PIN > -1 + SET_INPUT(Z_MAX_PIN); + #endif + #endif //ENDSTOPPULLUPS + + + //Initialize Step Pins + #if (X_STEP_PIN > -1) + SET_OUTPUT(X_STEP_PIN); + #endif + #if (Y_STEP_PIN > -1) + SET_OUTPUT(Y_STEP_PIN); + #endif + #if (Z_STEP_PIN > -1) + SET_OUTPUT(Z_STEP_PIN); + #endif + #if (E_STEP_PIN > -1) + SET_OUTPUT(E_STEP_PIN); + #endif + + // waveform generation = 0100 = CTC + TCCR1B &= ~(1<. - */ - -/* - This firmware is a mashup between Sprinter and grbl. - (https://github.com/kliment/Sprinter) - (https://github.com/simen/grbl/tree) - - It has preliminary support for Matthew Roberts advance algorithm - http://reprap.org/pipermail/reprap-dev/2011-May/003323.html - - This firmware is optimized for gen6 electronics. - */ - -#include "fastio.h" -#include "Configuration.h" -#include "pins.h" -#include "Marlin.h" -#include "ultralcd.h" -#include "temperature.h" -#include "watchdog.h" - -//=========================================================================== -//=============================public variables============================ -//=========================================================================== -int target_raw[3] = {0, 0, 0}; -int current_raw[3] = {0, 0, 0}; - -#ifdef PIDTEMP - - // probably used external - float HeaterPower; - float pid_setpoint = 0.0; - - - float Kp=DEFAULT_Kp; - float Ki=DEFAULT_Ki; - float Kd=DEFAULT_Kd; - float Kc=DEFAULT_Kc; -#endif //PIDTEMP - - -//=========================================================================== -//=============================private variables============================ -//=========================================================================== -static bool temp_meas_ready = false; - -static unsigned long previous_millis_heater, previous_millis_bed_heater; - -#ifdef PIDTEMP - //static cannot be external: - static float temp_iState = 0; - static float temp_dState = 0; - static float pTerm; - static float iTerm; - static float dTerm; - //int output; - static float pid_error; - static float temp_iState_min; - static float temp_iState_max; - static float pid_input; - static float pid_output; - static bool pid_reset; - -#endif //PIDTEMP - -#ifdef WATCHPERIOD - static int watch_raw[3] = {-1000,-1000,-1000}; - static unsigned long watchmillis = 0; -#endif //WATCHPERIOD - -#ifdef HEATER_0_MINTEMP - static int minttemp_0 = temp2analog(HEATER_0_MINTEMP); -#endif //MINTEMP -#ifdef HEATER_0_MAXTEMP - static int maxttemp_0 = temp2analog(HEATER_0_MAXTEMP); -#endif //MAXTEMP - -#ifdef HEATER_1_MINTEMP - static int minttemp_1 = temp2analog(HEATER_1_MINTEMP); -#endif //MINTEMP -#ifdef HEATER_1_MAXTEMP - static int maxttemp_1 = temp2analog(HEATER_1_MAXTEMP); -#endif //MAXTEMP - -#ifdef BED_MINTEMP - static int bed_minttemp = temp2analog(BED_MINTEMP); -#endif //BED_MINTEMP -#ifdef BED_MAXTEMP - static int bed_maxttemp = temp2analog(BED_MAXTEMP); -#endif //BED_MAXTEMP - -//=========================================================================== -//=============================functions ============================ -//=========================================================================== - -void updatePID() -{ -#ifdef PIDTEMP - temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki; -#endif -} - -void manage_heater() -{ - #ifdef USE_WATCHDOG - wd_reset(); - #endif - - float pid_input; - float pid_output; - if(temp_meas_ready != true) //better readability - return; - - CRITICAL_SECTION_START; - temp_meas_ready = false; - CRITICAL_SECTION_END; - - #ifdef PIDTEMP - pid_input = analog2temp(current_raw[TEMPSENSOR_HOTEND_0]); - - #ifndef PID_OPENLOOP - pid_error = pid_setpoint - pid_input; - if(pid_error > 10){ - pid_output = PID_MAX; - pid_reset = true; - } - else if(pid_error < -10) { - pid_output = 0; - pid_reset = true; - } - else { - if(pid_reset == true) { - temp_iState = 0.0; - pid_reset = false; - } - pTerm = Kp * pid_error; - temp_iState += pid_error; - temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max); - iTerm = Ki * temp_iState; - //K1 defined in Configuration.h in the PID settings - #define K2 (1.0-K1) - dTerm = (Kd * (pid_input - temp_dState))*K2 + (K1 * dTerm); - temp_dState = pid_input; - #ifdef PID_ADD_EXTRUSION_RATE - pTerm+=Kc*current_block->speed_e; //additional heating if extrusion speed is high - #endif - pid_output = constrain(pTerm + iTerm - dTerm, 0, PID_MAX); - - } - #endif //PID_OPENLOOP - #ifdef PID_DEBUG - //SERIAL_ECHOLN(" PIDDEBUG Input "<= target_raw[0]) - { - WRITE(HEATER_0_PIN,LOW); - } - else - { - WRITE(HEATER_0_PIN,HIGH); - } - #endif - - if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL) - return; - previous_millis_bed_heater = millis(); - - #if TEMP_1_PIN > -1 - if(current_raw[TEMPSENSOR_BED] >= target_raw[TEMPSENSOR_BED]) - { - WRITE(HEATER_1_PIN,LOW); - } - else - { - WRITE(HEATER_1_PIN,HIGH); - } - #endif -} - -// Takes hot end temperature value as input and returns corresponding raw value. -// For a thermistor, it uses the RepRap thermistor temp table. -// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value. -// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware. -int temp2analog(int celsius) { - #ifdef HEATER_0_USES_THERMISTOR - int raw = 0; - byte i; - - for (i=1; i raw) - { - celsius = heater_0_temptable[i-1][1] + - (raw - heater_0_temptable[i-1][0]) * - (float)(heater_0_temptable[i][1] - heater_0_temptable[i-1][1]) / - (float)(heater_0_temptable[i][0] - heater_0_temptable[i-1][0]); - - break; - } - } - - // Overflow: Set to last value in the table - if (i == NUMTEMPS_HEATER_0) celsius = heater_0_temptable[i-1][1]; - - return celsius; - #elif defined HEATER_0_USES_AD595 - return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR; - #endif -} - -// Derived from RepRap FiveD extruder::getTemperature() -// For bed temperature measurement. -float analog2tempBed(int raw) { - #ifdef BED_USES_THERMISTOR - int celsius = 0; - byte i; - - raw = (1023 * OVERSAMPLENR) - raw; - - for (i=1; i raw) - { - celsius = bedtemptable[i-1][1] + - (raw - bedtemptable[i-1][0]) * - (bedtemptable[i][1] - bedtemptable[i-1][1]) / - (bedtemptable[i][0] - bedtemptable[i-1][0]); - - break; - } - } - - // Overflow: Set to last value in the table - if (i == BNUMTEMPS) celsius = bedtemptable[i-1][1]; - - return celsius; - - #elif defined BED_USES_AD595 - return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR; - #endif -} - -void tp_init() -{ - #if (HEATER_0_PIN > -1) - SET_OUTPUT(HEATER_0_PIN); - #endif - #if (HEATER_1_PIN > -1) - SET_OUTPUT(HEATER_1_PIN); - #endif - #if (HEATER_2_PIN > -1) - SET_OUTPUT(HEATER_2_PIN); - #endif - - #ifdef PIDTEMP - temp_iState_min = 0.0; - temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki; - #endif //PIDTEMP - - // Set analog inputs - ADCSRA = 1< -1 - target_raw[0]=0; - #if HEATER_0_PIN > -1 - WRITE(HEATER_0_PIN,LOW); - #endif - #endif - - #if TEMP_1_PIN > -1 - target_raw[1]=0; - #if HEATER_1_PIN > -1 - WRITE(HEATER_1_PIN,LOW); - #endif - #endif - - #if TEMP_2_PIN > -1 - target_raw[2]=0; - #if HEATER_2_PIN > -1 - WRITE(HEATER_2_PIN,LOW); - #endif - #endif -} - -// Timer 0 is shared with millies -ISR(TIMER0_COMPB_vect) -{ - //these variables are only accesible from the ISR, but static, so they don't loose their value - static unsigned char temp_count = 0; - static unsigned long raw_temp_0_value = 0; - static unsigned long raw_temp_1_value = 0; - static unsigned long raw_temp_2_value = 0; - static unsigned char temp_state = 0; - - switch(temp_state) { - case 0: // Prepare TEMP_0 - #if (TEMP_0_PIN > -1) - #if TEMP_0_PIN < 8 - DIDR0 = 1 << TEMP_0_PIN; - #else - DIDR2 = 1<<(TEMP_0_PIN - 8); - ADCSRB = 1< -1) - raw_temp_0_value += ADC; - #endif - temp_state = 2; - break; - case 2: // Prepare TEMP_1 - #if (TEMP_1_PIN > -1) - #if TEMP_1_PIN < 7 - DIDR0 = 1< -1) - raw_temp_1_value += ADC; - #endif - temp_state = 4; - break; - case 4: // Prepare TEMP_2 - #if (TEMP_2_PIN > -1) - #if TEMP_2_PIN < 7 - DIDR0 = 1 << TEMP_2_PIN; - #else - DIDR2 = 1<<(TEMP_2_PIN - 8); - ADCSRB = 1< -1) - raw_temp_2_value += ADC; - #endif - temp_state = 0; - temp_count++; - break; - default: - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temp measurement error!"); - break; - } - - if(temp_count >= 16) // 6 ms * 16 = 96ms. - { - #ifdef HEATER_0_USES_AD595 - current_raw[0] = raw_temp_0_value; - #else - current_raw[0] = 16383 - raw_temp_0_value; - #endif - - #ifdef HEATER_1_USES_AD595 - current_raw[2] = raw_temp_2_value; - #else - current_raw[2] = 16383 - raw_temp_2_value; - #endif - - #ifdef BED_USES_AD595 - current_raw[1] = raw_temp_1_value; - #else - current_raw[1] = 16383 - raw_temp_1_value; - #endif - - temp_meas_ready = true; - temp_count = 0; - raw_temp_0_value = 0; - raw_temp_1_value = 0; - raw_temp_2_value = 0; - #ifdef HEATER_0_MAXTEMP - #if (HEATER_0_PIN > -1) - if(current_raw[TEMPSENSOR_HOTEND_0] >= maxttemp_0) { - target_raw[TEMPSENSOR_HOTEND_0] = 0; - analogWrite(HEATER_0_PIN, 0); - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MAXTEMP triggered !!"); - kill(); - } - #endif - #endif - #ifdef HEATER_1_MAXTEMP - #if (HEATER_1_PIN > -1) - if(current_raw[TEMPSENSOR_HOTEND_1] >= maxttemp_1) { - target_raw[TEMPSENSOR_HOTEND_1] = 0; - if(current_raw[2] >= maxttemp_1) { - analogWrite(HEATER_2_PIN, 0); - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MAXTEMP triggered !!"); - kill() - } - #endif - #endif //MAXTEMP - - #ifdef HEATER_0_MINTEMP - #if (HEATER_0_PIN > -1) - if(current_raw[TEMPSENSOR_HOTEND_0] <= minttemp_0) { - target_raw[TEMPSENSOR_HOTEND_0] = 0; - analogWrite(HEATER_0_PIN, 0); - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MINTEMP triggered !!"); - kill(); - } - #endif - #endif - - #ifdef HEATER_1_MINTEMP - #if (HEATER_2_PIN > -1) - if(current_raw[TEMPSENSOR_HOTEND_1] <= minttemp_1) { - target_raw[TEMPSENSOR_HOTEND_1] = 0; - analogWrite(HEATER_2_PIN, 0); - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MINTEMP triggered !!"); - kill(); - } - #endif - #endif //MAXTEMP - - #ifdef BED_MINTEMP - #if (HEATER_1_PIN > -1) - if(current_raw[1] <= bed_minttemp) { - target_raw[1] = 0; - WRITE(HEATER_1_PIN, 0); - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temperatur heated bed switched off. MINTEMP triggered !!"); - kill(); - } - #endif - #endif - - #ifdef BED_MAXTEMP - #if (HEATER_1_PIN > -1) - if(current_raw[1] >= bed_maxttemp) { - target_raw[1] = 0; - WRITE(HEATER_1_PIN, 0); - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !!"); - kill(); - } - #endif - #endif - } -} - - +/* + temperature.c - temperature control + Part of Marlin + + Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm + + This program 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. + + This program 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 this program. If not, see . + */ + +/* + This firmware is a mashup between Sprinter and grbl. + (https://github.com/kliment/Sprinter) + (https://github.com/simen/grbl/tree) + + It has preliminary support for Matthew Roberts advance algorithm + http://reprap.org/pipermail/reprap-dev/2011-May/003323.html + + This firmware is optimized for gen6 electronics. + */ +#include + +#include "fastio.h" +#include "Configuration.h" +#include "pins.h" +#include "Marlin.h" +#include "ultralcd.h" +#include "temperature.h" +#include "watchdog.h" + +//=========================================================================== +//=============================public variables============================ +//=========================================================================== +int target_raw[3] = {0, 0, 0}; +int current_raw[3] = {0, 0, 0}; + +#ifdef PIDTEMP + + // probably used external + float HeaterPower; + float pid_setpoint = 0.0; + + + float Kp=DEFAULT_Kp; + float Ki=DEFAULT_Ki; + float Kd=DEFAULT_Kd; + #ifdef PID_ADD_EXTRUSION_RATE + float Kc=DEFAULT_Kc; + #endif +#endif //PIDTEMP + + +//=========================================================================== +//=============================private variables============================ +//=========================================================================== +static bool temp_meas_ready = false; + +static unsigned long previous_millis_heater, previous_millis_bed_heater; + +#ifdef PIDTEMP + //static cannot be external: + static float temp_iState = 0; + static float temp_dState = 0; + static float pTerm; + static float iTerm; + static float dTerm; + //int output; + static float pid_error; + static float temp_iState_min; + static float temp_iState_max; + static float pid_input; + static float pid_output; + static bool pid_reset; + +#endif //PIDTEMP + +#ifdef WATCHPERIOD + static int watch_raw[3] = {-1000,-1000,-1000}; + static unsigned long watchmillis = 0; +#endif //WATCHPERIOD + +#ifdef HEATER_0_MINTEMP + static int minttemp_0 = temp2analog(HEATER_0_MINTEMP); +#endif //MINTEMP +#ifdef HEATER_0_MAXTEMP + static int maxttemp_0 = temp2analog(HEATER_0_MAXTEMP); +#endif //MAXTEMP + +#ifdef HEATER_1_MINTEMP + static int minttemp_1 = temp2analog(HEATER_1_MINTEMP); +#endif //MINTEMP +#ifdef HEATER_1_MAXTEMP + static int maxttemp_1 = temp2analog(HEATER_1_MAXTEMP); +#endif //MAXTEMP + +#ifdef BED_MINTEMP + static int bed_minttemp = temp2analog(BED_MINTEMP); +#endif //BED_MINTEMP +#ifdef BED_MAXTEMP + static int bed_maxttemp = temp2analog(BED_MAXTEMP); +#endif //BED_MAXTEMP + +//=========================================================================== +//=============================functions ============================ +//=========================================================================== + +void updatePID() +{ +#ifdef PIDTEMP + temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki; +#endif +} + +void manage_heater() +{ + #ifdef USE_WATCHDOG + wd_reset(); + #endif + + float pid_input; + float pid_output; + if(temp_meas_ready != true) //better readability + return; + + CRITICAL_SECTION_START; + temp_meas_ready = false; + CRITICAL_SECTION_END; + + #ifdef PIDTEMP + pid_input = analog2temp(current_raw[TEMPSENSOR_HOTEND_0]); + + #ifndef PID_OPENLOOP + pid_error = pid_setpoint - pid_input; + if(pid_error > 10){ + pid_output = PID_MAX; + pid_reset = true; + } + else if(pid_error < -10) { + pid_output = 0; + pid_reset = true; + } + else { + if(pid_reset == true) { + temp_iState = 0.0; + pid_reset = false; + } + pTerm = Kp * pid_error; + temp_iState += pid_error; + temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max); + iTerm = Ki * temp_iState; + //K1 defined in Configuration.h in the PID settings + #define K2 (1.0-K1) + dTerm = (Kd * (pid_input - temp_dState))*K2 + (K1 * dTerm); + temp_dState = pid_input; +// #ifdef PID_ADD_EXTRUSION_RATE +// pTerm+=Kc*current_block->speed_e; //additional heating if extrusion speed is high +// #endif + pid_output = constrain(pTerm + iTerm - dTerm, 0, PID_MAX); + + } + #endif //PID_OPENLOOP + #ifdef PID_DEBUG + //SERIAL_ECHOLN(" PIDDEBUG Input "<= target_raw[0]) + { + WRITE(HEATER_0_PIN,LOW); + } + else + { + WRITE(HEATER_0_PIN,HIGH); + } + #endif + + if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL) + return; + previous_millis_bed_heater = millis(); + + #if TEMP_1_PIN > -1 + if(current_raw[TEMPSENSOR_BED] >= target_raw[TEMPSENSOR_BED]) + { + WRITE(HEATER_1_PIN,LOW); + } + else + { + WRITE(HEATER_1_PIN,HIGH); + } + #endif +} + +#define PGM_RD_W(x) (short)pgm_read_word(&x) +// Takes hot end temperature value as input and returns corresponding raw value. +// For a thermistor, it uses the RepRap thermistor temp table. +// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value. +// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware. +int temp2analog(int celsius) { + #ifdef HEATER_0_USES_THERMISTOR + int raw = 0; + byte i; + + for (i=1; i raw) + { + celsius = PGM_RD_W(heater_0_temptable[i-1][1]) + + (raw - PGM_RD_W(heater_0_temptable[i-1][0])) * + (float)(PGM_RD_W(heater_0_temptable[i][1]) - PGM_RD_W(heater_0_temptable[i-1][1])) / + (float)(PGM_RD_W(heater_0_temptable[i][0]) - PGM_RD_W(heater_0_temptable[i-1][0])); + break; + } + } + + // Overflow: Set to last value in the table + if (i == NUMTEMPS_HEATER_0) celsius = PGM_RD_W(heater_0_temptable[i-1][1]); + + return celsius; + #elif defined HEATER_0_USES_AD595 + return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR; + #endif +} + +// Derived from RepRap FiveD extruder::getTemperature() +// For bed temperature measurement. +float analog2tempBed(int raw) { + #ifdef BED_USES_THERMISTOR + int celsius = 0; + byte i; + + raw = (1023 * OVERSAMPLENR) - raw; + + for (i=1; i raw) + { + celsius = PGM_RD_W(bedtemptable[i-1][1]) + + (raw - PGM_RD_W(bedtemptable[i-1][0])) * + (PGM_RD_W(bedtemptable[i][1]) - PGM_RD_W(bedtemptable[i-1][1])) / + (PGM_RD_W(bedtemptable[i][0]) - PGM_RD_W(bedtemptable[i-1][0])); + + break; + } + } + + // Overflow: Set to last value in the table + if (i == BNUMTEMPS) celsius = PGM_RD_W(bedtemptable[i-1][1]); + + return celsius; + + #elif defined BED_USES_AD595 + return raw * ((5.0 * 100.0) / 1024.0) / OVERSAMPLENR; + #endif +} + +void tp_init() +{ + #if (HEATER_0_PIN > -1) + SET_OUTPUT(HEATER_0_PIN); + #endif + #if (HEATER_1_PIN > -1) + SET_OUTPUT(HEATER_1_PIN); + #endif + #if (HEATER_2_PIN > -1) + SET_OUTPUT(HEATER_2_PIN); + #endif + + #ifdef PIDTEMP + temp_iState_min = 0.0; + temp_iState_max = PID_INTEGRAL_DRIVE_MAX / Ki; + #endif //PIDTEMP + + // Set analog inputs + ADCSRA = 1< -1 + target_raw[0]=0; + #if HEATER_0_PIN > -1 + WRITE(HEATER_0_PIN,LOW); + #endif + #endif + + #if TEMP_1_PIN > -1 + target_raw[1]=0; + #if HEATER_1_PIN > -1 + WRITE(HEATER_1_PIN,LOW); + #endif + #endif + + #if TEMP_2_PIN > -1 + target_raw[2]=0; + #if HEATER_2_PIN > -1 + WRITE(HEATER_2_PIN,LOW); + #endif + #endif +} + +// Timer 0 is shared with millies +ISR(TIMER0_COMPB_vect) +{ + //these variables are only accesible from the ISR, but static, so they don't loose their value + static unsigned char temp_count = 0; + static unsigned long raw_temp_0_value = 0; + static unsigned long raw_temp_1_value = 0; + static unsigned long raw_temp_2_value = 0; + static unsigned char temp_state = 0; + + switch(temp_state) { + case 0: // Prepare TEMP_0 + #if (TEMP_0_PIN > -1) + #if TEMP_0_PIN < 8 + DIDR0 = 1 << TEMP_0_PIN; + #else + DIDR2 = 1<<(TEMP_0_PIN - 8); + ADCSRB = 1< -1) + raw_temp_0_value += ADC; + #endif + temp_state = 2; + break; + case 2: // Prepare TEMP_1 + #if (TEMP_1_PIN > -1) + #if TEMP_1_PIN < 7 + DIDR0 = 1< -1) + raw_temp_1_value += ADC; + #endif + temp_state = 4; + break; + case 4: // Prepare TEMP_2 + #if (TEMP_2_PIN > -1) + #if TEMP_2_PIN < 7 + DIDR0 = 1 << TEMP_2_PIN; + #else + DIDR2 = 1<<(TEMP_2_PIN - 8); + ADCSRB = 1< -1) + raw_temp_2_value += ADC; + #endif + temp_state = 0; + temp_count++; + break; + default: + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temp measurement error!"); + break; + } + + if(temp_count >= 16) // 6 ms * 16 = 96ms. + { + #ifdef HEATER_0_USES_AD595 + current_raw[0] = raw_temp_0_value; + #else + current_raw[0] = 16383 - raw_temp_0_value; + #endif + + #ifdef HEATER_1_USES_AD595 + current_raw[2] = raw_temp_2_value; + #else + current_raw[2] = 16383 - raw_temp_2_value; + #endif + + #ifdef BED_USES_AD595 + current_raw[1] = raw_temp_1_value; + #else + current_raw[1] = 16383 - raw_temp_1_value; + #endif + + temp_meas_ready = true; + temp_count = 0; + raw_temp_0_value = 0; + raw_temp_1_value = 0; + raw_temp_2_value = 0; + #ifdef HEATER_0_MAXTEMP + #if (HEATER_0_PIN > -1) + if(current_raw[TEMPSENSOR_HOTEND_0] >= maxttemp_0) { + target_raw[TEMPSENSOR_HOTEND_0] = 0; + analogWrite(HEATER_0_PIN, 0); + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MAXTEMP triggered !!"); + kill(); + } + #endif + #endif + #ifdef HEATER_1_MAXTEMP + #if (HEATER_1_PIN > -1) + if(current_raw[TEMPSENSOR_HOTEND_1] >= maxttemp_1) { + target_raw[TEMPSENSOR_HOTEND_1] = 0; + if(current_raw[2] >= maxttemp_1) { + analogWrite(HEATER_2_PIN, 0); + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MAXTEMP triggered !!"); + kill() + } + #endif + #endif //MAXTEMP + + #ifdef HEATER_0_MINTEMP + #if (HEATER_0_PIN > -1) + if(current_raw[TEMPSENSOR_HOTEND_0] <= minttemp_0) { + target_raw[TEMPSENSOR_HOTEND_0] = 0; + analogWrite(HEATER_0_PIN, 0); + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temperature extruder 0 switched off. MINTEMP triggered !!"); + kill(); + } + #endif + #endif + + #ifdef HEATER_1_MINTEMP + #if (HEATER_2_PIN > -1) + if(current_raw[TEMPSENSOR_HOTEND_1] <= minttemp_1) { + target_raw[TEMPSENSOR_HOTEND_1] = 0; + analogWrite(HEATER_2_PIN, 0); + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temperature extruder 1 switched off. MINTEMP triggered !!"); + kill(); + } + #endif + #endif //MAXTEMP + + #ifdef BED_MINTEMP + #if (HEATER_1_PIN > -1) + if(current_raw[1] <= bed_minttemp) { + target_raw[1] = 0; + WRITE(HEATER_1_PIN, 0); + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temperatur heated bed switched off. MINTEMP triggered !!"); + kill(); + } + #endif + #endif + + #ifdef BED_MAXTEMP + #if (HEATER_1_PIN > -1) + if(current_raw[1] >= bed_maxttemp) { + target_raw[1] = 0; + WRITE(HEATER_1_PIN, 0); + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Temperature heated bed switched off. MAXTEMP triggered !!"); + kill(); + } + #endif + #endif + } +} + + diff --git a/Marlin/thermistortables.h b/Marlin/thermistortables.h index fbd2ef1446..22d9f02487 100644 --- a/Marlin/thermistortables.h +++ b/Marlin/thermistortables.h @@ -1,12 +1,14 @@ #ifndef THERMISTORTABLES_H_ #define THERMISTORTABLES_H_ +#include + #define OVERSAMPLENR 16 #if (THERMISTORHEATER_0 == 1) || (THERMISTORHEATER_1 == 1) || (THERMISTORBED == 1) //100k bed thermistor #define NUMTEMPS_1 61 -const short temptable_1[NUMTEMPS_1][2] = { +const short temptable_1[NUMTEMPS_1][2] PROGMEM = { { 23*OVERSAMPLENR , 300 }, { 25*OVERSAMPLENR , 295 }, { 27*OVERSAMPLENR , 290 }, @@ -72,7 +74,7 @@ const short temptable_1[NUMTEMPS_1][2] = { #endif #if (THERMISTORHEATER_0 == 2) || (THERMISTORHEATER_1 == 2) || (THERMISTORBED == 2) //200k bed thermistor #define NUMTEMPS_2 21 -const short temptable_2[NUMTEMPS_2][2] = { +const short temptable_2[NUMTEMPS_2][2] PROGMEM = { {1*OVERSAMPLENR, 848}, {54*OVERSAMPLENR, 275}, {107*OVERSAMPLENR, 228}, @@ -99,7 +101,7 @@ const short temptable_2[NUMTEMPS_2][2] = { #endif #if (THERMISTORHEATER_0 == 3) || (THERMISTORHEATER_1 == 3) || (THERMISTORBED == 3) //mendel-parts #define NUMTEMPS_3 28 -const short temptable_3[NUMTEMPS_3][2] = { +const short temptable_3[NUMTEMPS_3][2] PROGMEM = { {1*OVERSAMPLENR,864}, {21*OVERSAMPLENR,300}, {25*OVERSAMPLENR,290}, @@ -134,7 +136,7 @@ const short temptable_3[NUMTEMPS_3][2] = { #if (THERMISTORHEATER_0 == 4) || (THERMISTORHEATER_1 == 4) || (THERMISTORBED == 4) //10k thermistor #define NUMTEMPS_4 20 -short temptable_4[NUMTEMPS_4][2] = { +const short temptable_4[NUMTEMPS_4][2] PROGMEM = { {1*OVERSAMPLENR, 430}, {54*OVERSAMPLENR, 137}, {107*OVERSAMPLENR, 107}, @@ -161,7 +163,7 @@ short temptable_4[NUMTEMPS_4][2] = { #if (THERMISTORHEATER_0 == 5) || (THERMISTORHEATER_1 == 5) || (THERMISTORBED == 5) //100k ParCan thermistor (104GT-2) #define NUMTEMPS_5 61 -const short temptable_5[NUMTEMPS_5][2] = { +const short temptable_5[NUMTEMPS_5][2] PROGMEM = { {1*OVERSAMPLENR, 713}, {18*OVERSAMPLENR, 316}, {35*OVERSAMPLENR, 266}, @@ -228,7 +230,7 @@ const short temptable_5[NUMTEMPS_5][2] = { #if (THERMISTORHEATER_0 == 6) || (THERMISTORHEATER_1 == 6) || (THERMISTORBED == 6) // 100k Epcos thermistor #define NUMTEMPS_6 36 -const short temptable_6[NUMTEMPS_6][2] = { +const short temptable_6[NUMTEMPS_6][2] PROGMEM = { {28*OVERSAMPLENR, 250}, {31*OVERSAMPLENR, 245}, {35*OVERSAMPLENR, 240}, @@ -270,7 +272,7 @@ const short temptable_6[NUMTEMPS_6][2] = { #if (THERMISTORHEATER_0 == 7) || (THERMISTORHEATER_1 == 7) || (THERMISTORBED == 7) // 100k Honeywell 135-104LAG-J01 #define NUMTEMPS_7 54 -const short temptable_7[NUMTEMPS_7][2] = { +const short temptable_7[NUMTEMPS_7][2] PROGMEM = { {46*OVERSAMPLENR, 270}, {50*OVERSAMPLENR, 265}, {54*OVERSAMPLENR, 260}, diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h index c836757e1c..6bae43dccb 100644 --- a/Marlin/ultralcd.h +++ b/Marlin/ultralcd.h @@ -1,102 +1,103 @@ -#ifndef __ULTRALCDH -#define __ULTRALCDH -#include "Configuration.h" - -#ifdef ULTRA_LCD - - void lcd_status(); - void lcd_init(); - void lcd_status(const char* message); - void beep(); - void buttons_check(); - - - #define LCD_UPDATE_INTERVAL 100 - #define STATUSTIMEOUT 15000 - - - #include - extern LiquidCrystal lcd; - - - #ifdef NEWPANEL - - - #define EN_C (1< + extern LiquidCrystal lcd; + + + #ifdef NEWPANEL + + + #define EN_C (1<