From 29cb4b2b5dba81d5049487e5c6b3d4ae460f84c7 Mon Sep 17 00:00:00 2001 From: bubnikv Date: Thu, 1 Sep 2016 13:09:56 +0200 Subject: [PATCH] Removed the non-working pressure advance feature. Improved accuracy of diagonal moves by oversampling the path discretization. Accelerated the planner by rewriting time critical routines from floating point to fixed point arithmetics. --- Firmware/Marlin_main.cpp | 18 +- Firmware/planner.cpp | 267 +++++++++++------- Firmware/planner.h | 20 +- Firmware/stepper.cpp | 162 ++--------- Firmware/temperature.cpp | 1 + Firmware/ultralcd.cpp | 1 - .../ultralcd_implementation_hitachi_HD44780.h | 30 +- 7 files changed, 223 insertions(+), 276 deletions(-) diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index c68457efc..146a2b88b 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -1038,22 +1038,15 @@ void setup() // is being written into the EEPROM, so the update procedure will be triggered only once. lang_selected = eeprom_read_byte((uint8_t*)EEPROM_LANG); if (lang_selected >= LANG_NUM){ - lcd_mylang(); + lcd_mylang(); } if (eeprom_read_byte((uint8_t*)EEPROM_BABYSTEP_Z_SET) == 0x0ff) { // Reset the babystepping values, so the printer will not move the Z axis up when the babystepping is enabled. - // eeprom_update_byte((uint8_t*)EEPROM_BABYSTEP_X, 0x0ff); - // eeprom_update_byte((uint8_t*)EEPROM_BABYSTEP_Y, 0x0ff); - eeprom_update_byte((uint8_t*)EEPROM_BABYSTEP_Z, 0x0ff); - // Get the selected laugnage index before display update. - lang_selected = eeprom_read_byte((uint8_t*)EEPROM_LANG); - if (lang_selected >= LANG_NUM) - lang_selected = LANG_ID_DEFAULT; // Czech language + eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0); // Show the message. lcd_show_fullscreen_message_and_wait_P(MSG_BABYSTEP_Z_NOT_SET); lcd_update_enable(true); - // lcd_implementation_clear(); } // Store the currently running firmware into an eeprom, @@ -2961,8 +2954,9 @@ void process_commands() bool result = sample_mesh_and_store_reference(); // if (result) babystep_apply(); } else { - // Reset the baby step value. + // Reset the baby step value and the baby step applied flag. eeprom_write_byte((unsigned char*)EEPROM_BABYSTEP_Z_SET, 0xFF); + eeprom_update_word((uint16_t*)EEPROM_BABYSTEP_Z, 0); // Complete XYZ calibration. BedSkewOffsetDetectionResultType result = find_bed_offset_and_skew(verbosity_level); uint8_t point_too_far_mask = 0; @@ -4820,11 +4814,7 @@ void prepare_move() // Do not use feedmultiply for E or Z only moves if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) { -#ifdef MESH_BED_LEVELING - mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); -#else plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); -#endif } else { #ifdef MESH_BED_LEVELING diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 62855f189..d70f75155 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -110,6 +110,11 @@ block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion volatile unsigned char block_buffer_head; // Index of the next block to be pushed volatile unsigned char block_buffer_tail; // Index of the block to process now +#ifdef PLANNER_DIAGNOSTICS +// Diagnostic function: Minimum number of planned moves since the last +static uint8_t g_cntr_planner_queue_min = 0; +#endif /* PLANNER_DIAGNOSTICS */ + //=========================================================================== //=============================private variables ============================ //=========================================================================== @@ -171,59 +176,86 @@ FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, f } } -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +#define MINIMAL_STEP_RATE 120 -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) +// 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) +{ + // These two lines are the only floating point calculations performed in this routine. + uint32_t initial_rate = ceil(entry_speed * block->speed_factor); // (step/min) + uint32_t final_rate = ceil(exit_speed * block->speed_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; - } + if (initial_rate < MINIMAL_STEP_RATE) + initial_rate = MINIMAL_STEP_RATE; + if (initial_rate > block->nominal_rate) + initial_rate = block->nominal_rate; + if (final_rate < MINIMAL_STEP_RATE) + final_rate = MINIMAL_STEP_RATE; + if (final_rate > block->nominal_rate) + final_rate = block->nominal_rate; - long acceleration = block->acceleration_st; - int32_t accelerate_steps = - ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration)); - int32_t decelerate_steps = - floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration)); - - // Calculate the size of Plateau of Nominal Rate. - int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + uint32_t acceleration = block->acceleration_st; + if (acceleration == 0) + // Don't allow zero acceleration. + acceleration = 1; + // estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) + // (target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration)); + uint32_t initial_rate_sqr = initial_rate*initial_rate; + //FIXME assert that this result fits a 64bit unsigned int. + uint32_t nominal_rate_sqr = block->nominal_rate*block->nominal_rate; + uint32_t final_rate_sqr = final_rate*final_rate; + uint32_t acceleration_x2 = acceleration << 1; + // ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration)); + uint32_t accelerate_steps = (nominal_rate_sqr - initial_rate_sqr + acceleration_x2 - 1) / acceleration_x2; + // floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration)); + uint32_t decelerate_steps = (nominal_rate_sqr - final_rate_sqr) / acceleration_x2; + uint32_t accel_decel_steps = accelerate_steps + decelerate_steps; + // Size of Plateau of Nominal Rate. + uint32_t plateau_steps = 0; // 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(initial_rate, final_rate, acceleration, block->step_event_count)); - accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off - accelerate_steps = min((uint32_t)accelerate_steps,block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero) - plateau_steps = 0; + if (accel_decel_steps < block->step_event_count) { + plateau_steps = block->step_event_count - accel_decel_steps; + } else { + uint32_t acceleration_x4 = acceleration << 2; + // Avoid negative numbers + if (final_rate_sqr >= initial_rate_sqr) { + // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count)); + // intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) + // (2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration); +// accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; + accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; + if (block->step_event_count & 1) + accelerate_steps += acceleration_x2; + accelerate_steps += block->step_event_count >> 1; + accelerate_steps /= acceleration_x4; + if (accelerate_steps > block->step_event_count) + accelerate_steps = block->step_event_count; + } else { +// decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; + decelerate_steps = initial_rate_sqr - final_rate_sqr; + if (block->step_event_count & 1) + decelerate_steps += acceleration_x2; + decelerate_steps += block->step_event_count >> 1; + decelerate_steps /= acceleration_x4; + if (decelerate_steps > block->step_event_count) + decelerate_steps = block->step_event_count; + accelerate_steps = block->step_event_count - decelerate_steps; + } } -#ifdef ADVANCE - volatile long initial_advance = block->advance*entry_factor*entry_factor; - volatile 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) { // 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 entry speed, when you must be able to reach target_velocity using the // decceleration within the allotted distance. @@ -249,6 +281,27 @@ FORCE_INLINE float max_allowable_entry_speed(float decceleration, float target_v // the set limit. Finally it will: // // 3. Recalculate trapezoids for all blocks. +// +//FIXME This routine is called 15x every time a new line is added to the planner, +// therefore it is a bottle neck and it shall be rewritten into a Fixed Point arithmetics, +// if the CPU is found lacking computational power. +// +// Following sources may be used to optimize the 8-bit AVR code: +// http://www.mikrocontroller.net/articles/AVR_Arithmetik +// http://darcy.rsgc.on.ca/ACES/ICE4M/FixedPoint/avrfix.pdf +// +// https://github.com/gcc-mirror/gcc/blob/master/libgcc/config/avr/lib1funcs-fixed.S +// https://gcc.gnu.org/onlinedocs/gcc/Fixed-Point.html +// https://gcc.gnu.org/onlinedocs/gccint/Fixed-point-fractional-library-routines.html +// +// https://ucexperiment.wordpress.com/2015/04/04/arduino-s15-16-fixed-point-math-routines/ +// https://mekonik.wordpress.com/2009/03/18/arduino-avr-gcc-multiplication/ +// https://github.com/rekka/avrmultiplication +// +// https://people.ece.cornell.edu/land/courses/ece4760/Math/Floating_point/ +// https://courses.cit.cornell.edu/ee476/Math/ +// https://courses.cit.cornell.edu/ee476/Math/GCC644/fixedPt/multASM.S +// void planner_recalculate(const float &safe_final_speed) { // Reverse pass @@ -291,8 +344,12 @@ void planner_recalculate(const float &safe_final_speed) // segment and the maximum acceleration allowed for this segment. // If nominal length true, max junction speed is guaranteed to be reached even if decelerating to a jerk-from-zero velocity. // Only compute for max allowable speed if block is decelerating and nominal length is false. + // entry_speed is uint16_t, 24 bits would be sufficient for block->acceleration and block->millimiteres, if scaled to um. + // therefore an optimized assembly 24bit x 24bit -> 32bit multiply would be more than sufficient + // together with an assembly 32bit->16bit sqrt function. current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || current->max_entry_speed <= next->entry_speed) ? current->max_entry_speed : + // min(current->max_entry_speed, sqrt(next->entry_speed*next->entry_speed+2*current->acceleration*current->millimeters)); min(current->max_entry_speed, max_allowable_entry_speed(-current->acceleration,next->entry_speed,current->millimeters)); current->flag |= BLOCK_FLAG_RECALCULATE; } @@ -325,7 +382,7 @@ void planner_recalculate(const float &safe_final_speed) // Recalculate if current block entry or exit junction speed has changed. if ((prev->flag | current->flag) & BLOCK_FLAG_RECALCULATE) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. - calculate_trapezoid_for_block(prev, prev->entry_speed/prev->nominal_speed, current->entry_speed/prev->nominal_speed); + calculate_trapezoid_for_block(prev, prev->entry_speed, current->entry_speed); // Reset current only to ensure next trapezoid is computed. prev->flag &= ~BLOCK_FLAG_RECALCULATE; } @@ -338,7 +395,7 @@ void planner_recalculate(const float &safe_final_speed) // Last/newest block in buffer. Exit speed is set with safe_final_speed. Always recalculated. current = block_buffer + prev_block_index(block_buffer_head); - calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, safe_final_speed/current->nominal_speed); + calculate_trapezoid_for_block(current, current->entry_speed, safe_final_speed); current->flag &= ~BLOCK_FLAG_RECALCULATE; // SERIAL_ECHOLNPGM("planner_recalculate - 4"); @@ -471,6 +528,15 @@ void planner_abort_soft() } */ +#ifdef PLANNER_DIAGNOSTICS +static inline void planner_update_queue_min_counter() +{ + uint8_t new_counter = moves_planned(); + if (new_counter < g_cntr_planner_queue_min) + g_cntr_planner_queue_min = new_counter; +} +#endif /* PLANNER_DIAGNOSTICS */ + void planner_abort_hard() { // Abort the stepper routine and flush the planner queue. @@ -527,11 +593,18 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate manage_inactivity(false); lcd_update(); } while (block_buffer_tail == next_buffer_head); - if (waiting_inside_plan_buffer_line_print_aborted) + if (waiting_inside_plan_buffer_line_print_aborted) { // Inside the lcd_update() routine the print has been aborted. // Cancel the print, do not plan the current line this routine is waiting on. +#ifdef PLANNER_DIAGNOSTICS + planner_update_queue_min_counter(); +#endif /* PLANNER_DIAGNOSTICS */ return; + } } +#ifdef PLANNER_DIAGNOSTICS + planner_update_queue_min_counter(); +#endif /* PLANNER_DIAGNOSTICS */ #ifdef ENABLE_AUTO_BED_LEVELING apply_rotation_xyz(plan_bed_level_matrix, x, y, z); @@ -637,14 +710,20 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi #endif block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); - block->steps_e *= volumetric_multiplier[active_extruder]; - block->steps_e *= extrudemultiply; - block->steps_e /= 100; + if (volumetric_multiplier[active_extruder] != 1.f) + block->steps_e *= volumetric_multiplier[active_extruder]; + if (extrudemultiply != 100) { + block->steps_e *= extrudemultiply; + block->steps_e /= 100; + } 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) { +#ifdef PLANNER_DIAGNOSTICS + planner_update_queue_min_counter(); +#endif /* PLANNER_DIAGNOSTICS */ return; } @@ -869,6 +948,8 @@ Having the real displacement of the head, we can calculate the total movement le } // Compute and limit the acceleration rate for the trapezoid generator. + // block->step_event_count ... event count of the fastest axis + // block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement. float steps_per_mm = block->step_event_count/block->millimeters; if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) { @@ -888,49 +969,27 @@ Having the real displacement of the head, we can calculate the total movement le 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]; } + // Acceleration of the segment, in mm/sec^2 block->acceleration = block->acceleration_st / steps_per_mm; + +#if 1 + // Oversample diagonal movements by a power of 2 up to 8x + // to achieve more accurate diagonal movements. + uint8_t bresenham_oversample = 1; + for (uint8_t i = 0; i < 3; ++ i) { + if (block->nominal_rate >= 5000) // 5kHz + break; + block->nominal_rate << 1; + bresenham_oversample << 1; + block->step_event_count << 1; + } + if (bresenham_oversample > 1) + // Lower the acceleration steps/sec^2 to account for the oversampling. + block->acceleration_st = (block->acceleration_st + (bresenham_oversample >> 1)) / bresenham_oversample; +#endif + block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); -#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. // Safe speed is the speed, from which the machine may halt to stop immediately. float safe_speed = block->nominal_speed; @@ -1047,34 +1106,9 @@ Having the real displacement of the head, we can calculate the total movement le previous_nominal_speed = block->nominal_speed; previous_safe_speed = safe_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) * - (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256; - block->advance = advance; - if(acc_dist == 0) { - block->advance_rate = 0; - } - else { - block->advance_rate = advance / (float)acc_dist; - } - } - /* - SERIAL_ECHO_START; - SERIAL_ECHOPGM("advance :"); - SERIAL_ECHO(block->advance/256.0); - SERIAL_ECHOPGM("advance rate :"); - SERIAL_ECHOLN(block->advance_rate/256.0); - */ -#endif // ADVANCE - - calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed); + // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated. + block->speed_factor = block->nominal_rate / block->nominal_speed; + calculate_trapezoid_for_block(block, block->entry_speed, safe_speed); // Move the buffer head. From now the block may be picked up by the stepper interrupt controller. block_buffer_head = next_buffer_head; @@ -1092,6 +1126,9 @@ Having the real displacement of the head, we can calculate the total movement le // SERIAL_ECHO(int(moves_planned())); // SERIAL_ECHOLNPGM(""); +#ifdef PLANNER_DIAGNOSTICS + planner_update_queue_min_counter(); +#endif /* PLANNER_DIAGNOSTIC */ st_wake_up(); } @@ -1172,3 +1209,15 @@ void reset_acceleration_rates() axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; } } + +#ifdef PLANNER_DIAGNOSTICS +uint8_t planner_queue_min() +{ + return g_cntr_planner_queue_min; +} + +void planner_queue_min_reset() +{ + g_cntr_planner_queue_min = moves_planned(); +} +#endif /* PLANNER_DIAGNOSTICS */ \ No newline at end of file diff --git a/Firmware/planner.h b/Firmware/planner.h index 58619beac..a13aa1bd2 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -55,12 +55,6 @@ typedef struct { // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. 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 - #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/sec for each axis @@ -82,12 +76,18 @@ typedef struct { // Settings for the trapezoid generator (runs inside an interrupt handler). // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. + //FIXME nominal_rate, initial_rate and final_rate are limited to uint16_t by MultiU24X24toH16 in the stepper interrupt anyway! 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 + //FIXME does it have to be unsigned long? Probably uint8_t would be just fine. unsigned long fan_speed; volatile char busy; + + + // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster. + float speed_factor; } block_t; #ifdef ENABLE_AUTO_BED_LEVELING @@ -196,3 +196,11 @@ void set_extrude_min_temp(float temp); void reset_acceleration_rates(); #endif + +// #define PLANNER_DIAGNOSTICS +#ifdef PLANNER_DIAGNOSTICS +// Diagnostic functions to display planner buffer underflow on the display. +extern uint8_t planner_queue_min(); +// Diagnostic function: Reset the minimum planner segments. +extern void planner_queue_min_reset(); +#endif /* PLANNER_DIAGNOSTICS */ diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 6bfea2a61..783299b97 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -47,22 +47,17 @@ block_t *current_block; // A pointer to the block currently being traced // 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; -volatile 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 long old_advance = 0; - static long e_steps[3]; -#endif -static long acceleration_time, deceleration_time; +static int32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z, + counter_e; +volatile static uint32_t step_events_completed; // The number of step events executed in the current block +static int32_t 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; -static unsigned short OCR1A_nominal; -static unsigned short step_loops_nominal; +static uint16_t acc_step_rate; // needed for deccelaration start point +static uint8_t step_loops; +static uint16_t OCR1A_nominal; +static uint8_t step_loops_nominal; volatile long endstops_trigsteps[3]={0,0,0}; volatile long endstops_stepsTotal,endstops_stepsDone; @@ -306,13 +301,6 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { // Initializes the trapezoid generator from the current block. Called whenever a new // block begins. FORCE_INLINE void trapezoid_generator_reset() { - #ifdef ADVANCE - advance = current_block->initial_advance; - final_advance = current_block->final_advance; - // Do E steps + advance steps - e_steps[current_block->active_extruder] += ((advance >>8) - old_advance); - old_advance = advance >>8; - #endif deceleration_time = 0; // step_rate to timer interval OCR1A_nominal = calc_timer(current_block->nominal_rate); @@ -359,10 +347,6 @@ ISR(TIMER1_COMPA_vect) return; } #endif - -// #ifdef ADVANCE -// e_steps[current_block->active_extruder] = 0; -// #endif } else { OCR1A=2000; // 1kHz. @@ -531,37 +515,20 @@ ISR(TIMER1_COMPA_vect) } #endif - #ifndef ADVANCE - if ((out_bits & (1<steps_e; - if (counter_e > 0) { - counter_e -= current_block->step_event_count; - if ((out_bits & (1<active_extruder]--; - } - else { - e_steps[current_block->active_extruder]++; - } - } - #endif //ADVANCE - counter_x += current_block->steps_x; if (counter_x > 0) { WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); @@ -604,7 +571,6 @@ ISR(TIMER1_COMPA_vect) #endif } - #ifndef ADVANCE counter_e += current_block->steps_e; if (counter_e > 0) { WRITE_E_STEP(!INVERT_E_STEP_PIN); @@ -612,7 +578,6 @@ ISR(TIMER1_COMPA_vect) count_position[E_AXIS]+=count_direction[E_AXIS]; WRITE_E_STEP(INVERT_E_STEP_PIN); } - #endif //!ADVANCE step_events_completed += 1; if(step_events_completed >= current_block->step_event_count) break; } @@ -620,7 +585,7 @@ ISR(TIMER1_COMPA_vect) unsigned short timer; unsigned short step_rate; if (step_events_completed <= (unsigned long int)current_block->accelerate_until) { - + // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); acc_step_rate += current_block->initial_rate; @@ -632,16 +597,6 @@ ISR(TIMER1_COMPA_vect) timer = calc_timer(acc_step_rate); OCR1A = timer; acceleration_time += timer; - #ifdef ADVANCE - for(int8_t i=0; i < step_loops; i++) { - advance += advance_rate; - } - //if(advance > current_block->advance) advance = current_block->advance; - // Do E steps + advance steps - e_steps[current_block->active_extruder] += ((advance >>8) - old_advance); - old_advance = advance >>8; - - #endif // ADVANCE } else if (step_events_completed > (unsigned long int)current_block->decelerate_after) { MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); @@ -661,15 +616,6 @@ ISR(TIMER1_COMPA_vect) timer = calc_timer(step_rate); OCR1A = timer; deceleration_time += timer; - #ifdef ADVANCE - for(int8_t i=0; i < step_loops; i++) { - advance -= advance_rate; - } - if(advance < final_advance) advance = final_advance; - // Do E steps + advance steps - e_steps[current_block->active_extruder] += ((advance >>8) - old_advance); - old_advance = advance >>8; - #endif //ADVANCE } else { OCR1A = OCR1A_nominal; @@ -685,63 +631,6 @@ ISR(TIMER1_COMPA_vect) } } -#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) - { - old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz) - OCR0A = old_OCR0A; - // Set E direction (Depends on E direction + advance) - for(unsigned char i=0; i<4;i++) { - if (e_steps[0] != 0) { - WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); - if (e_steps[0] < 0) { - WRITE(E0_DIR_PIN, INVERT_E0_DIR); - e_steps[0]++; - WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); - } - else if (e_steps[0] > 0) { - WRITE(E0_DIR_PIN, !INVERT_E0_DIR); - e_steps[0]--; - WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); - } - } - #if EXTRUDERS > 1 - if (e_steps[1] != 0) { - WRITE(E1_STEP_PIN, INVERT_E_STEP_PIN); - if (e_steps[1] < 0) { - WRITE(E1_DIR_PIN, INVERT_E1_DIR); - e_steps[1]++; - WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN); - } - else if (e_steps[1] > 0) { - WRITE(E1_DIR_PIN, !INVERT_E1_DIR); - e_steps[1]--; - WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN); - } - } - #endif - #if EXTRUDERS > 2 - if (e_steps[2] != 0) { - WRITE(E2_STEP_PIN, INVERT_E_STEP_PIN); - if (e_steps[2] < 0) { - WRITE(E2_DIR_PIN, INVERT_E2_DIR); - e_steps[2]++; - WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN); - } - else if (e_steps[2] > 0) { - WRITE(E2_DIR_PIN, !INVERT_E2_DIR); - e_steps[2]--; - WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN); - } - } - #endif - } - } -#endif // ADVANCE - void st_init() { digipot_init(); //Initialize Digipot Motor Current @@ -930,17 +819,6 @@ void st_init() TCNT1 = 0; ENABLE_STEPPER_DRIVER_INTERRUPT(); - #ifdef ADVANCE - #if defined(TCCR0A) && defined(WGM01) - TCCR0A &= ~(1<> 1)) { + lcd.print('!'); + } else { + lcd.print((char)(queue / 10) + '0'); + queue %= 10; + } + lcd.print((char)queue + '0'); + planner_queue_min_reset(); + } +#endif //Print SD status lcd.setCursor(0, 2);