diff --git a/Makefile b/Makefile index f0e7f41..78c1032 100644 --- a/Makefile +++ b/Makefile @@ -84,7 +84,7 @@ main.elf: $(OBJECTS) grbl.hex: main.elf rm -f grbl.hex avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex - avr-size -C --mcu=$(DEVICE) main.elf + avr-size --format=berkeley main.elf # If you have an EEPROM section, you must also create a hex file for the # EEPROM and add it to the "flash" target. diff --git a/config.h b/config.h index c1c450e..41d6a38 100644 --- a/config.h +++ b/config.h @@ -2,8 +2,8 @@ config.h - compile time configuration Part of Grbl - Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2011-2013 Sungeun K. Jeon + 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 @@ -19,80 +19,24 @@ along with Grbl. If not, see . */ -#ifndef config_h -#define config_h +// This file contains compile-time configurations for Grbl's internal system. For the most part, +// users will not need to directly modify these, but they are here for specific needs, i.e. +// performance tuning or adjusting to non-typical machines. // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them. +#ifndef config_h +#define config_h + // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h #define DEFAULTS_ZEN_TOOLWORKS_7x7 // Serial baud rate -#define BAUD_RATE 9600 +#define BAUD_RATE 115200 -// Define pin-assignments -// NOTE: All step bit and direction pins must be on the same port. -#define STEPPING_DDR DDRD -#define STEPPING_PORT PORTD -#define X_STEP_BIT 2 // Uno Digital Pin 2 -#define Y_STEP_BIT 3 // Uno Digital Pin 3 -#define Z_STEP_BIT 4 // Uno Digital Pin 4 -#define X_DIRECTION_BIT 5 // Uno Digital Pin 5 -#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 -#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 -#define STEP_MASK ((1< #include "config.h" #include "defaults.h" +#include "pin_map.h" #define false 0 #define true 1 diff --git a/pin_map.h b/pin_map.h new file mode 100644 index 0000000..0167cc9 --- /dev/null +++ b/pin_map.h @@ -0,0 +1,181 @@ +/* + pin_map.h - Pin mapping configuration file + Part of Grbl + + Copyright (c) 2013 Sungeun K. Jeon + + 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 pin_map.h file serves as a central pin mapping settings file for different processor + types, i.e. AVR 328p or AVR Mega 2560. Grbl officially supports the Arduino Uno, but the + other supplied pin mappings are supplied by users, so your results may vary. */ + +#ifndef pin_map_h +#define pin_map_h + +#ifdef PIN_MAP_ARDUINO_UNO // AVR 328p, Officially supported by Grbl. + + // Serial port pins + #define SERIAL_RX USART_RX_vect + #define SERIAL_UDRE USART_UDRE_vect + + // NOTE: All step bit and direction pins must be on the same port. + #define STEPPING_DDR DDRD + #define STEPPING_PORT PORTD + #define X_STEP_BIT 2 // Uno Digital Pin 2 + #define Y_STEP_BIT 3 // Uno Digital Pin 3 + #define Z_STEP_BIT 4 // Uno Digital Pin 4 + #define X_DIRECTION_BIT 5 // Uno Digital Pin 5 + #define Y_DIRECTION_BIT 6 // Uno Digital Pin 6 + #define Z_DIRECTION_BIT 7 // Uno Digital Pin 7 + #define STEP_MASK ((1<entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining; + } else { // Block is accelerating or cruising + partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining); + partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr); + } + + // Update only the relevant planner block information so the planner can plan correctly. + partial_block->millimeters = millimeters_remaining; + partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated. + } +} + + + + /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ - | + <- next->entry_speed + | + <- next->entry_speed (aka exit speed) +-------------+ time --> @@ -112,7 +153,7 @@ static uint8_t prev_block_index(uint8_t block_index) in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. - The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED. Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory @@ -123,69 +164,178 @@ static uint8_t prev_block_index(uint8_t block_index) as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions. + + Index mapping: + - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner + never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED. + - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible + as long as the segment buffer is not empty. + - next_buffer_head: Points to next planner buffer block after the last block. Should always be empty. + - block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since + the stepper can be executing the first block and if the planner changes its conditions, this will cause + a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm, + the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but + this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block + is decelerating, the planner conditions can change such that the block can new accelerate mid-block. + + !!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too. + + !!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the + block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation + of this function to accept two different conditions to operate on. + + - block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be + improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned + block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry + speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it + can still be changed, meaning the forward pass calculations must start from here and influence the following block + entry speed. + + !!! Need to check if this is the start of the non-optimal or the end of the optimal block. */ static void planner_recalculate() -{ - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - uint8_t block_index = block_buffer_head; - plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer +{ + // Query stepper module for safe planner block index to recalculate to, which corresponds to the end + // of the step segment buffer. + uint8_t block_buffer_safe = st_get_prep_block_index(); + // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module + // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although, + // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the + // planner at all times. - // Ping the stepper algorithm to check if we can alter the parameters of the currently executing - // block. If not, skip it and work on the next block. - // TODO: Need to look into if there are conditions where this fails. - uint8_t block_buffer_safe = next_block_index( block_buffer_tail ); - - // TODO: Need to recompute buffer tail millimeters based on how much is completed. - - if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on + /* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1 + or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only + when the planner queue has been entirely queued into the segment buffer and there are no more blocks + in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the + planner should be able to treat a newly added block during this time as an empty planner buffer since + we can't touch the segment buffer. + + - The segment buffer is atomic to the planner buffer, because the main program computes these seperately. + Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't + effect the safe pointer. - block_buffer_planned = block_buffer_safe; -// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + - If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may + be executing. This is the last block in the planner queue, so it has been planned to decelerate to + zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming, + from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to + do anything, since the trapezoid calculations called by the stepper module should complete the block plan. + + - In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two + beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it + can be in any one of these states. As the stepper module executes the planner block, the buffer tail, + and hence the safe pointer, can push forward through the planner blocks and overcome the planned + pointer at any time. + - Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only + allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass + ignores this block. The safe pointer is the same way, where the entry speed does not change, but + the velocity profile within it does. + + - The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything + operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any + rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer + at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not + changed by adding a new block or something else. + + - The planner recalculate function should always reset the planned pointer at the proper break points + or when it encounters the safe block pointer, but will only do so when there are more than one block + in the buffer. In the case of single blocks, the planned pointer should always be set to the first + write-able block in the buffer, aka safe block. + + - When does this not work? There might be an issue when the planned pointer moves from the tail to the + next head as a new block is being added and planned. Otherwise, the planned pointer should remain + static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks, + or both simultaneously. Need to make sure that this case is covered. + */ + + + // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one. + // NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up + // the remainder of the planner buffer. In this case, a new planner block will be treated as a single block. + if (block_buffer_head == block_buffer_safe) { // Also catches head = tail + + // Just set block_buffer_planned pointer. + block_buffer_planned = block_buffer_head; + printString("z"); + + // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For + // a single added block and recalculate after a feed hold, we don't need to compute this, since we already + // know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock + // rate, and we have to calculate the new velocity profile from it. + // plan_update_partial_block(block_index,0.0); + } else { - - // TODO: need to account for the two block condition better. If the currently executing block - // is not safe, do we wait until its done? Can we treat the buffer head differently? - - // Calculate trapezoid for the last/newest block. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, - MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters); -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); + + // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for + // all junctions before proceeding. + // Initialize planner buffer pointers and indexing. + uint8_t block_index = block_buffer_head; + plan_block_t *current = &block_buffer[block_index]; + + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); - // Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease - // planning when: (1) the last optimal planned pointer is reached. (2) the safe block - // pointer is reached, whereby the planned pointer is updated. + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached. + // (2) the safe block pointer is reached, whereby the planned pointer is updated. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block + // will be recomputed within the plan. So, we need to update it if it is partially completed. float entry_speed_sqr; plan_block_t *next; block_index = prev_block_index(block_index); - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; + + if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so. - // Exit loop and update planned pointer when the tail/safe block is reached. - if (block_index == block_buffer_safe) { - block_buffer_planned = block_buffer_safe; - break; - } + // Only two plannable blocks in buffer. Compute previous block based on + // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero. + // !!! Need to make the current entry speed calculation after this. + plan_update_partial_block(block_index, 0.0); + block_buffer_planned = block_index; +printString("y"); + + } else { - // Crudely maximize deceleration curve from the end of the non-optimally planned buffer to - // the optimal plan pointer. Forward pass will adjust and finish optimizing the plan. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; + // Three or more plan-able + while (block_index != block_buffer_planned) { + + next = current; + current = &block_buffer[block_index]; + + // Increment block index early to check if the safe block is before the current block. If encountered, + // this is an exit condition as we can't go further than this block in the reverse pass. + block_index = prev_block_index(block_index); + if (block_index == block_buffer_safe) { + // Check if the safe block is partially completed. If so, update it before its exit speed + // (=current->entry speed) is over-written. + // TODO: The update breaks with feedrate overrides, because the replanning process no longer has + // the previous nominal speed to update this block with. There will need to be something along the + // lines of a nominal speed change check and send the correct value to this function. + plan_update_partial_block(block_index,current->entry_speed_sqr); +printString("x"); + // Set planned pointer at safe block and for loop exit after following computation is done. + block_buffer_planned = block_index; + } + + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } } } - block_index = prev_block_index(block_index); - } + + } // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - block_index = block_buffer_planned; // Begin at buffer planned pointer - next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = next_block_index(block_buffer_planned); while (block_index != next_buffer_head) { current = next; next = &block_buffer[block_index]; @@ -194,22 +344,22 @@ static void planner_recalculate() // pointer forward, since everything before this is all optimal. In other words, nothing // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { - block_buffer_planned = block_index; entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this. + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. } } // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. The optimally planned pointer is updated. + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. if (next->entry_speed_sqr == next->max_entry_speed_sqr) { - block_buffer_planned = block_index; + block_buffer_planned = block_index; // Set optimal plan pointer } - // Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned - // pointer to the end of the buffer, except the last block. -// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); block_index = next_block_index( block_index ); } @@ -218,19 +368,24 @@ static void planner_recalculate() } +void plan_reset_buffer() +{ + block_buffer_planned = block_buffer_tail; +} + void plan_init() { - block_buffer_head = 0; - block_buffer_tail = block_buffer_head; - next_buffer_head = next_block_index(block_buffer_head); - block_buffer_planned = block_buffer_head; + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // next_block_index(block_buffer_head) + plan_reset_buffer(); memset(&pl, 0, sizeof(pl)); // Clear planner struct } void plan_discard_current_block() { - if (block_buffer_head != block_buffer_tail) { + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. block_buffer_tail = next_block_index( block_buffer_tail ); } } @@ -238,7 +393,10 @@ void plan_discard_current_block() plan_block_t *plan_get_current_block() { - if (block_buffer_head == block_buffer_tail) { return(NULL); } + if (block_buffer_head == block_buffer_tail) { // Buffer empty + plan_reset_buffer(); + return(NULL); + } return(&block_buffer[block_buffer_tail]); } @@ -289,6 +447,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later // Compute and store initial move distance data. + // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea + // to try to keep these types of things completely separate from the planner for portability. int32_t target_steps[N_AXIS]; float unit_vec[N_AXIS], delta_mm; uint8_t idx; @@ -313,7 +473,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() - // Bail if this is a zero-length block + // Bail if this is a zero-length block. Highly unlikely to occur. if (block->step_event_count == 0) { return; } // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) @@ -346,41 +506,59 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) } } - /* 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. - NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - is exactly the same. Instead of motioning all the way to junction point, the machine will - just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - a continuous mode path, but ARM-based microcontrollers most certainly do. - */ - // TODO: Acceleration need to be limited by the minimum of the two junctions. - // TODO: Need to setup a method to handle zero junction speeds when starting from rest. + + // TODO: Need to check this method handling zero junction speeds when starting from rest. if (block_buffer_head == block_buffer_tail) { - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + // !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break + // feedrate overrides later, as you can override this single block and it maybe moving already at a given rate. + // Better to do it here and make it clean. + // !!! Shouldn't need this for anything other than a single block. + block->entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + } else { + /* + 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. + + NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + is exactly the same. Instead of motioning all the way to junction point, the machine will + just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + a continuous mode path, but ARM-based microcontrollers most certainly do. + + NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + changed dynamically during operation nor can the line segment geometry. This must be kept in + memory in the event of a feedrate override changing the nominal speeds of blocks, which can + change the overall maximum entry speed conditions of all blocks. + + */ // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2); + + // TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions. + block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } - // Store block nominal speed and rate + // Store block nominal speed block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 -// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) -// -// // Compute and store acceleration and distance traveled per step event. -// block->rate_delta = ceil(block->acceleration* -// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) -// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step) - + + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + // TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later. + block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); + // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; @@ -390,11 +568,17 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) planner_recalculate(); - // Update buffer head and next buffer head indices. - // NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan - // calculations to help prevent overwriting scenarios with adding a new block to a low buffer. + // Update buffer head and next buffer head indices. Advance only after new plan has been computed. block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); + + + +int32_t blength = block_buffer_head - block_buffer_tail; +if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } +printInteger(blength); + + } @@ -408,6 +592,8 @@ void plan_sync_position() } + + /* STEPPER VELOCITY PROFILE DEFINITION less than nominal rate-> + +--------+ <- nominal_rate /|\ @@ -419,31 +605,35 @@ void plan_sync_position() | | | | decelerate distance decelerate distance - Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be - described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block - reaches the nominal speed of the block and cruises for a period of time. A triangle occurs - when the nominal speed is not reached within the block. Some other special cases exist, - such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that - has no deceleration period when the next block resumes acceleration. + Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper + algorithm. The planner computes the entry and exit speeds of each block, but does not bother to + determine the details of the velocity profiles within them, as they aren't needed for computing + an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles + are computed ad hoc. + + Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The + trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of + time. A triangle occurs when the nominal speed is not reached within the block. Both of these + velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, + as they can be influenced by the conditions of neighboring blocks. The following function determines the type of velocity profile and stores the minimum required - information for the stepper algorithm to execute the calculated profiles. In this case, only - the new initial rate and n_steps until deceleration are computed, since the stepper algorithm - already handles acceleration and cruising and just needs to know when to start decelerating. + information for the stepper algorithm to execute the calculated profiles. Since the stepper + algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate + is reached, we only need to know when to begin deceleration to the end of the block. Hence, only + the distance from the end of the block to begin a deceleration ramp are computed. */ -int32_t calculate_trapezoid_for_block(uint8_t block_index) +float plan_calculate_velocity_profile(uint8_t block_index) { plan_block_t *current_block = &block_buffer[block_index]; // Determine current block exit speed - float exit_speed_sqr; - uint8_t next_index = next_block_index(block_index); - plan_block_t *next_block = plan_get_block_by_index(next_index); - if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed. - else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block + float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed. + plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index)); + if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block // First determine intersection distance (in steps) from the exit point for a triangular profile. - // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) ) + // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) ); // Check if this is a pure acceleration block by a intersection distance less than zero. Also @@ -452,18 +642,17 @@ int32_t calculate_trapezoid_for_block(uint8_t block_index) float decelerate_distance; // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile. // Value is never negative. Nominal speed is always greater than or equal to the exit speed. - // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) ) + // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration) decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration); // The lesser of the two triangle and trapezoid distances always defines the velocity profile. if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; } // Finally, check if this is a pure deceleration block. - if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; } - - return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters)); + if (decelerate_distance > current_block->millimeters) { return(0.0); } + else { return( (current_block->millimeters-decelerate_distance) ); } } - return(0); + return( current_block->millimeters ); // No deceleration in velocity profile. } @@ -481,7 +670,7 @@ void plan_cycle_reinitialize(int32_t step_events_remaining) // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. block->entry_speed_sqr = 0.0; - block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; + block->max_entry_speed_sqr = 0.0; block_buffer_planned = block_buffer_tail; planner_recalculate(); } diff --git a/planner.h b/planner.h index e371bb0..083d182 100644 --- a/planner.h +++ b/planner.h @@ -33,20 +33,20 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line + // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly. uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) int32_t steps[N_AXIS]; // Step count along each axis - int32_t step_event_count; // The number of step events required to complete this block + int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. // Fields used by the motion planner to manage acceleration + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2 - float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2 - float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2 - float acceleration; // Axes-limit adjusted line acceleration in mm/min^2 - float millimeters; // The total travel for this block to be executed in mm + float acceleration; // Axis-limit adjusted line acceleration in mm/min^2 + float millimeters; // The remaining distance for this block to be executed in mm - // Settings for the trapezoid generator -// int32_t decelerate_after; // The index of the step event on which to start decelerating - } plan_block_t; // Initialize the motion plan subsystem @@ -66,7 +66,9 @@ plan_block_t *plan_get_current_block(); plan_block_t *plan_get_block_by_index(uint8_t block_index); -int32_t calculate_trapezoid_for_block(uint8_t block_index); +float plan_calculate_velocity_profile(uint8_t block_index); + +// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating); // Reset the planner position vector (in steps) void plan_sync_position(); diff --git a/serial.c b/serial.c index 69fa717..d3325c7 100644 --- a/serial.c +++ b/serial.c @@ -91,11 +91,7 @@ void serial_write(uint8_t data) { } // Data Register Empty Interrupt handler -#ifdef __AVR_ATmega644P__ -ISR(USART0_UDRE_vect) -#else -ISR(USART_UDRE_vect) -#endif +ISR(SERIAL_UDRE) { // Temporary tx_buffer_tail (to optimize for volatile) uint8_t tail = tx_buffer_tail; @@ -144,11 +140,7 @@ uint8_t serial_read() } } -#ifdef __AVR_ATmega644P__ -ISR(USART0_RX_vect) -#else -ISR(USART_RX_vect) -#endif +ISR(SERIAL_RX) { uint8_t data = UDR0; uint8_t next_head; diff --git a/stepper.c b/stepper.c index 05ddd9e..6509bed 100644 --- a/stepper.c +++ b/stepper.c @@ -42,7 +42,7 @@ #define ST_DECEL 2 #define ST_DECEL_EOB 3 -#define SEGMENT_BUFFER_SIZE 10 +#define SEGMENT_BUFFER_SIZE 6 // Stepper state variable. Contains running data and trapezoid variables. typedef struct { @@ -50,11 +50,11 @@ typedef struct { int32_t counter_x, // Counter variables for the bresenham line tracer counter_y, counter_z; - uint8_t segment_steps_remaining; // Steps remaining in line motion + uint8_t segment_steps_remaining; // Steps remaining in line segment motion // Used by inverse time algorithm to track step rate - int32_t counter_d; // Inverse time distance traveled since last step event - uint32_t delta_d; // Inverse time distance traveled per interrupt tick + int32_t counter_d; // Inverse time distance traveled since last step event + uint32_t delta_d; // Inverse time distance traveled per interrupt tick uint32_t d_per_tick; // Used by the stepper driver interrupt @@ -68,9 +68,11 @@ typedef struct { } stepper_t; static stepper_t st; -// Stores stepper buffer common data. Can change planner mid-block in special conditions. +// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner +// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs. +// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions. typedef struct { - int32_t step_events_remaining; // Tracks step event count for the executing planner block + int32_t step_events_remaining; // Tracks step event count for the executing planner block uint32_t d_next; // Scaled distance to next step uint32_t initial_rate; // Initialized step rate at re/start of a planner block uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute @@ -80,16 +82,17 @@ typedef struct { } st_data_t; static st_data_t segment_data[SEGMENT_BUFFER_SIZE]; -// Primary stepper motion buffer +// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked +// out incrementally from the first block in the planner buffer. These step segments typedef struct { - uint8_t n_step; - uint8_t st_data_index; - uint8_t flag; + uint8_t n_step; // Number of step events to be executed for this segment + uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment. + uint8_t flag; // Stepper algorithm execution flag to notify special conditions. } st_segment_t; static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; static volatile uint8_t segment_buffer_tail; -static uint8_t segment_buffer_head; +static volatile uint8_t segment_buffer_head; static uint8_t segment_next_head; static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. @@ -97,11 +100,16 @@ static plan_block_t *pl_current_block; // A pointer to the planner block curren static st_segment_t *st_current_segment; static st_data_t *st_current_data; +// Pointers for the step segment being prepped from the planner buffer. Accessed only by the +// main program. Pointers may be planning segments or planner blocks ahead of what being executed. static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer static uint8_t pl_prep_index; static st_data_t *st_prep_data; static uint8_t st_data_prep_index; +static uint8_t pl_partial_block_flag; + + // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. @@ -115,7 +123,7 @@ static uint8_t next_block_index(uint8_t block_index) static uint8_t next_block_pl_index(uint8_t block_index) { block_index++; - if (block_index == 18) { block_index = 0; } + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } return(block_index); } @@ -255,22 +263,20 @@ ISR(TIMER2_COMPA_vect) // Initialize inverse time and step rate counter data st.counter_d = st_current_data->d_next; // d_next always greater than delta_d. + if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } + else { st.d_per_tick = st.delta_d; } // During feed hold, do not update rate or ramp type. Keep decelerating. // if (sys.state == STATE_CYCLE) { st.delta_d = st_current_data->initial_rate; -// if (st.delta_d == st_current_data->nominal_rate) { -// st.ramp_type = RAMP_NOOP_CRUISE; - st.ramp_type = RAMP_ACCEL; st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid -// } + if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; } + else { st.ramp_type = RAMP_ACCEL; } // } - if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; } - else { st.d_per_tick = st.delta_d; } } - // Acceleration and cruise handled by ramping. Just check for deceleration. + // Acceleration and cruise handled by ramping. Just check if deceleration needs to begin. if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) { if (st.ramp_type == RAMP_NOOP_CRUISE) { st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid @@ -292,6 +298,8 @@ ISR(TIMER2_COMPA_vect) } // Adjust inverse time counter for ac/de-celerations + // NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally + // efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development. if (st.ramp_type) { st.ramp_count--; // Tick acceleration ramp counter if (st.ramp_count == 0) { // Adjust step rate when its time @@ -396,14 +404,20 @@ ISR(TIMER0_OVF_vect) void st_reset() { memset(&st, 0, sizeof(st)); - pl_current_block = NULL; - pl_prep_block = NULL; - pl_prep_index = 0; - st_data_prep_index = 0; + st.load_flag = LOAD_BLOCK; busy = false; + + pl_current_block = NULL; // Planner block pointer used by stepper algorithm + pl_prep_block = NULL; // Planner block pointer used by segment buffer + pl_prep_index = 0; // Planner buffer indices are also reset to zero. + st_data_prep_index = 0; + segment_buffer_tail = 0; + segment_buffer_head = 0; // empty = tail segment_next_head = 1; + + pl_partial_block_flag = false; } @@ -485,7 +499,7 @@ void st_cycle_reinitialize() } -/* Preps stepper buffer. Called from main program. +/* Prepares step segment buffer. Continuously called from main program. NOTE: There doesn't seem to be a great way to figure out how many steps occur within a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a @@ -510,41 +524,75 @@ void st_cycle_reinitialize() been doing here limit this phase issue by truncating some of the ramp timing for certain events like deceleration initialization and end of block. */ + +// !!! Need to make sure when a single partially completed block can be re-computed here with +// new deceleration point and the segment manager begins accelerating again immediately. void st_prep_buffer() { while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. // Determine if we need to load a new planner block. if (pl_prep_block == NULL) { - pl_prep_block = plan_get_block_by_index(pl_prep_index); - if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out. + pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block + if (pl_prep_block == NULL) { return; } // No planner blocks. Exit. - // Prepare commonly shared planner block data for the ensuing step buffer moves - st_data_prep_index = next_block_index(st_data_prep_index); - st_prep_data = &segment_data[st_data_prep_index]; - - // Initialize Bresenham variables - st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + // Check if the planner has re-computed this block mid-execution. If so, push the old segment block + // data Otherwise, prepare a new segment block data. + if (pl_partial_block_flag) { + + // Prepare new shared segment block data and copy the relevant last segment block data. + st_data_t *last_st_prep_data; + last_st_prep_data = &segment_data[st_data_prep_index]; + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining; + st_prep_data->rate_delta = last_st_prep_data->rate_delta; + st_prep_data->d_next = last_st_prep_data->d_next; + st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides. + + st_prep_data->mm_per_step = last_st_prep_data->mm_per_step; - // Convert new block to stepper variables. - // NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must - // be maintained as these execute. - // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, - // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. - st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // This data doesn't change. Could be performed in the planner, but fits nicely here. - // Although, acceleration can change for S-curves. So keep it here. - st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* - ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) - // This definitely doesn't change, but could be precalculated in a way to help some of the - // math in this handler, i.e. millimeters per step event data. - st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) - st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; - - // Calculate trapezoid data from planner. - st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index); + pl_partial_block_flag = false; // Reset flag + + // TODO: If the planner updates this block, particularly from a deceleration to an acceleration, + // we must reload the initial rate data, such that the velocity profile is re-constructed correctly. + // The stepper algorithm must be flagged to adjust the acceleration counters. + + } else { + + // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since + // the planner buffer can dynamically change the velocity profile data as blocks are added. + st_data_prep_index = next_block_index(st_data_prep_index); + st_prep_data = &segment_data[st_data_prep_index]; + + // Initialize Bresenham variables + st_prep_data->step_events_remaining = pl_prep_block->step_event_count; + + // Convert planner block velocity profile data to stepper rate and step distance data. + st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + st_prep_data->rate_delta = ceil(pl_prep_block->acceleration* + ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step) + + // TODO: Check if we really need to store this. + st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count; + + } + + // Convert planner entry speed to stepper initial rate. + st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // TODO: Nominal rate changes with feedrate override. + // st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) + + // Calculate the planner block velocity profile type and determine deceleration point. + float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index); + if (mm_decelerate_after == pl_prep_block->millimeters) { + st_prep_data->decelerate_after = st_prep_data->step_events_remaining; + } else { + st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step ); + } } @@ -564,77 +612,108 @@ void st_prep_buffer() - From deceleration to acceleration, i.e. common with jogging when new blocks are added. */ - st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head]; - st_prep_segment->st_data_index = st_data_prep_index; + st_segment_t *new_segment = &segment_buffer[segment_buffer_head]; + new_segment->st_data_index = st_data_prep_index; // TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'. - st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step ); -// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? -// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // The basic equation is: s = u*t + 0.5*a*t^2 + // For the most part, we can store the acceleration portion in the st_data buffer and all + // we would need to do is track the current approximate speed per loop with: v = u + a*t + // Each loop would require 3 multiplication and 2 additions, since most of the variables + // are constants and would get compiled out. + +//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm. +// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step); +// if (st_prep_data->decelerate_after > 0) { +// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// } else { +// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step); +// } + + new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step ); +// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions? +// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow. + // Check if n_step exceeds steps remaining in planner block. If so, truncate. - if (st_prep_segment->n_step > st_prep_data->step_events_remaining) { - st_prep_segment->n_step = st_prep_data->step_events_remaining; + if (new_segment->n_step > st_prep_data->step_events_remaining) { + new_segment->n_step = st_prep_data->step_events_remaining; + + // Don't need to compute last velocity, since it will be refreshed with a new block. } // Check if n_step exceeds decelerate point in block. Need to perform this so that the // ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should // be OK since it is likely moving at a fast rate already. if (st_prep_data->decelerate_after > 0) { - if (st_prep_segment->n_step > st_prep_data->decelerate_after) { - st_prep_segment->n_step = st_prep_data->decelerate_after; - } - } - -// float distance, exit_speed_sqr; -// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero -// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) { -// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance; -// } else { // Acceleration or cruising ramp -// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) { -// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance; -// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; } -// } else { -// exit_speed_sqr = pl_prep_block->nominal_speed_sqr; + if (new_segment->n_step > st_prep_data->decelerate_after) { + new_segment->n_step = st_prep_data->decelerate_after; + } +// !!! Doesn't work. Remove if not using. +// if (last_velocity < last_nominal_v) { +// // !!! Doesn't work since distance changes and gets truncated. +// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp. +// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising. // } -// } - - // Update planner block variables. -// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr); -// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable. -// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block. -// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter. +// } else { // In deceleration ramp +// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); + } // Update stepper block variables. - st_prep_data->step_events_remaining -= st_prep_segment->n_step; + st_prep_data->step_events_remaining -= new_segment->n_step; if ( st_prep_data->step_events_remaining == 0 ) { // Move planner pointer to next block if (st_prep_data->decelerate_after == 0) { - st_prep_segment->flag = ST_DECEL_EOB; + new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too. } else { - st_prep_segment->flag = ST_END_OF_BLOCK; + new_segment->flag = ST_END_OF_BLOCK; } pl_prep_index = next_block_pl_index(pl_prep_index); pl_prep_block = NULL; -printString("EOB"); } else { + // Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags. if (st_prep_data->decelerate_after == 0) { - st_prep_segment->flag = ST_DECEL; + new_segment->flag = ST_DECEL; } else { - st_prep_segment->flag = ST_NOOP; + new_segment->flag = ST_NOOP; } -printString("x"); + st_prep_data->decelerate_after -= new_segment->n_step; } - st_prep_data->decelerate_after -= st_prep_segment->n_step; + - // New step block completed. Increment step buffer indices. + // New step segment completed. Increment segment buffer indices. segment_buffer_head = segment_next_head; segment_next_head = next_block_index(segment_buffer_head); -printInteger((long)st_prep_segment->n_step); -printString(" "); -printInteger((long)st_prep_data->decelerate_after); -printString(" "); -printInteger((long)st_prep_data->step_events_remaining); } } + +uint8_t st_get_prep_block_index() +{ +// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this? + return(pl_prep_index); +} + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating) +{ + // if called, can we assume that this always changes and needs to be updated? if so, then + // we can perform all of the segment buffer setup tasks here to make sure the next time + // the segments are loaded, the st_data buffer is updated correctly. + // !!! Make sure that this is always pointing to the correct st_prep_data block. + + // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated + // correctly, much in the same fashion as the deceleration counters. Need to think about this + // make sure this is right, but i'm pretty sure it is. + + // TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit. + if (pl_prep_block != NULL) { + *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step; + if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } + else { *is_decelerating = true; } + + // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes. + pl_partial_block_flag = true; + pl_prep_block = NULL; + } + return; +} diff --git a/stepper.h b/stepper.h index 74ab717..8e2a6e0 100644 --- a/stepper.h +++ b/stepper.h @@ -47,4 +47,8 @@ void st_feed_hold(); void st_prep_buffer(); +uint8_t st_get_prep_block_index(); + +void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating); + #endif