planner.c

00001 /*
00002   planner.c - buffers movement commands and manages the acceleration profile plan
00003   Part of Grbl
00004 
00005   Copyright (c) 2009-2011 Simen Svale Skogsrud
00006   Copyright (c) 2011 Sungeun K. Jeon
00007   
00008   Grbl is free software: you can redistribute it and/or modify
00009   it under the terms of the GNU General Public License as published by
00010   the Free Software Foundation, either version 3 of the License, or
00011   (at your option) any later version.
00012 
00013   Grbl is distributed in the hope that it will be useful,
00014   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016   GNU General Public License for more details.
00017 
00018   You should have received a copy of the GNU General Public License
00019   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
00020 */
00021 
00022 /* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
00023 
00024 #include <inttypes.h>
00025 #include <stdbool.h>
00026 #include <math.h>       
00027 #include <stdlib.h>
00028 #include <string.h>
00029 
00030 
00031 #include "planner.h"
00032 #include "stepper.h"
00033 #include "config.h"
00034 #include "global.h"
00035 
00036 // The GRBL configuration (scaling etc)
00037 config_t config;
00038 
00039 #define lround(x) ( (long)floor(x+0.5) )
00040 
00041 // The number of linear motions that can be in the plan at any give time
00042 #define BLOCK_BUFFER_SIZE 16
00043 tTarget startpoint;
00044 
00045 static block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructions
00046 static volatile uint8_t block_buffer_head;       // Index of the next block to be pushed
00047 static volatile uint8_t block_buffer_tail;       // Index of the block to process now
00048 
00049 static int32_t position[NUM_AXES];             // The current position of the tool in absolute steps
00050 static float previous_unit_vec[NUM_AXES];     // Unit vector of previous path line segment
00051 static float previous_nominal_speed;   // Nominal speed of previous path line segment
00052 
00053 static uint8_t acceleration_manager_enabled;   // Acceleration management active?
00054 
00055 
00056 // initial entry point of the planner
00057 // Clear values and set defaults
00058 void plan_init() {
00059   block_buffer_head = 0;
00060   block_buffer_tail = 0;
00061   plan_set_acceleration_manager_enabled(true);
00062   clear_vector(position);
00063   clear_vector_double(previous_unit_vec);
00064   previous_nominal_speed = 0.0;
00065   
00066   memset (&startpoint, 0, sizeof(startpoint));
00067   
00068  // default config:
00069   config.steps_per_mm_x = fabs((float)cfg->xscale/1000.0); // convert xscale from [steps/meter] to [steps/mm]
00070   config.steps_per_mm_y = fabs((float)cfg->yscale/1000.0); 
00071   config.steps_per_mm_z = fabs((float)cfg->zscale/1000.0); 
00072   config.steps_per_mm_e = fabs((float)cfg->escale/1000.0); 
00073   config.maximum_feedrate_x = 60 * cfg->xspeed; // convert speed from [mm/sec] to [mm/min]
00074   config.maximum_feedrate_y = 60 * cfg->yspeed;
00075   config.maximum_feedrate_z = 60 * cfg->zspeed;
00076   config.maximum_feedrate_e = 60 * cfg->espeed;
00077   config.acceleration = cfg->accel; // [mm/sec2]
00078   config.junction_deviation = cfg->tolerance/1000.0; //  convert tolerance from [micron] to [mm]
00079   
00080   config.junction_deviation = 0.05;
00081  //  config.steps_per_mm_x =  config.steps_per_mm_y =  config.steps_per_mm_z =  config.steps_per_mm_e = 200;
00082   // config.acceleration = 200;
00083   //config.maximum_feedrate_x =  config.maximum_feedrate_y =  config.maximum_feedrate_z =  config.maximum_feedrate_e = 60000;
00084   printf("steps_per_mm_x %f...\n", (float)config.steps_per_mm_x);
00085   printf("steps_per_mm_y %f...\n", (float)config.steps_per_mm_y);
00086   printf("steps_per_mm_z %f...\n", (float)config.steps_per_mm_z);
00087   printf("steps_per_mm_e %f...\n", (float)config.steps_per_mm_e);
00088   printf("accel %f...\n", (float)config.acceleration);
00089   printf("Motion: double=%d, float=%d, block=%d\n", sizeof(double), sizeof(float), sizeof(block_t));
00090 
00091 }
00092 
00093 
00094 // Returns the index of the next block in the ring buffer
00095 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
00096 static int8_t next_block_index(int8_t block_index) {
00097   block_index++;
00098   if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
00099   return(block_index);
00100 }
00101 
00102 
00103 // Returns the index of the previous block in the ring buffer
00104 static int8_t prev_block_index(int8_t block_index) {
00105   if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
00106   block_index--;
00107   return(block_index);
00108 }
00109 
00110 
00111 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
00112 // given acceleration:
00113 static float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
00114   return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
00115 }
00116 
00117 
00118 /*                        + <- some maximum rate we don't care about
00119                          /|\
00120                         / | \                    
00121                        /  |  + <- final_rate     
00122                       /   |  |                   
00123      initial_rate -> +----+--+                   
00124                           ^  ^                   
00125                           |  |                   
00126       intersection_distance  distance                                                                           */
00127 // This function gives you the point at which you must start braking (at the rate of -acceleration) if 
00128 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
00129 // a total travel of distance. This can be used to compute the intersection point between acceleration and
00130 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
00131 static float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
00132   return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
00133 }
00134 
00135             
00136 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
00137 // using the acceleration within the allotted distance.
00138 // NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
00139 // in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
00140 // BLOCK_BUFFER_SIZE calls per planner cycle.
00141 static float max_allowable_speed(float acceleration, float target_velocity, float distance) {
00142   return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) );
00143 }
00144 
00145 
00146 // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
00147 static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
00148   if (!current) { return; }  // Cannot operate on nothing.
00149   
00150   if (next) { 
00151     // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
00152     // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and 
00153     // check for maximum allowable speed reductions to ensure maximum possible planned speed.
00154     if (current->entry_speed != current->max_entry_speed) {
00155     
00156       // If nominal length true, max junction speed is guaranteed to be reached. Only compute
00157       // for max allowable speed if block is decelerating and nominal length is false.
00158       if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
00159         current->entry_speed = min( current->max_entry_speed,
00160           max_allowable_speed(-config.acceleration,next->entry_speed,current->millimeters));
00161       } else {
00162         current->entry_speed = current->max_entry_speed;
00163       } 
00164       current->recalculate_flag = true;    
00165     
00166     }
00167   } // Skip last block. Already initialized and set for recalculation.
00168 }
00169 
00170 
00171 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
00172 // implements the reverse pass.
00173 static void planner_reverse_pass() {
00174   auto int8_t block_index = block_buffer_head;
00175   block_t *block[3] = {NULL, NULL, NULL};
00176   while(block_index != block_buffer_tail) {    
00177     block_index = prev_block_index( block_index );
00178     block[2]= block[1];
00179     block[1]= block[0];
00180     block[0] = &block_buffer[block_index];
00181     planner_reverse_pass_kernel(block[0], block[1], block[2]);
00182   }
00183   // Skip buffer tail/first block to prevent over-writing the initial entry speed.
00184 }
00185 
00186 
00187 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
00188 static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
00189   if(!previous) { return; }  // Begin planning after buffer_tail
00190   
00191   // If the previous block is an acceleration block, but it is not long enough to complete the
00192   // full speed change within the block, we need to adjust the entry speed accordingly. Entry
00193   // speeds have already been reset, maximized, and reverse planned by reverse planner.
00194   // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.  
00195   if (!previous->nominal_length_flag) {
00196     if (previous->entry_speed < current->entry_speed) {
00197       float entry_speed = min( current->entry_speed,
00198         max_allowable_speed(-config.acceleration,previous->entry_speed,previous->millimeters) );
00199 
00200       // Check for junction speed change
00201       if (current->entry_speed != entry_speed) {
00202         current->entry_speed = entry_speed;
00203         current->recalculate_flag = true;
00204       }
00205     }    
00206   }
00207 }
00208 
00209 
00210 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
00211 // implements the forward pass.
00212 static void planner_forward_pass() {
00213   int8_t block_index = block_buffer_tail;
00214   block_t *block[3] = {NULL, NULL, NULL};
00215   
00216   while(block_index != block_buffer_head) {
00217     block[0] = block[1];
00218     block[1] = block[2];
00219     block[2] = &block_buffer[block_index];
00220     planner_forward_pass_kernel(block[0],block[1],block[2]);
00221     block_index = next_block_index( block_index );
00222   }
00223   planner_forward_pass_kernel(block[1], block[2], NULL);
00224 }
00225 
00226 
00227 /*                             STEPPER RATE DEFINITION                                              
00228                                      +--------+   <- nominal_rate
00229                                     /          \                                
00230     nominal_rate*entry_factor ->   +            \                               
00231                                    |             + <- nominal_rate*exit_factor  
00232                                    +-------------+                              
00233                                        time -->                                 
00234 */                                                                              
00235 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
00236 // The factors represent a factor of braking and must be in the range 0.0-1.0.
00237 // This converts the planner parameters to the data required by the stepper controller.
00238 // NOTE: Final rates must be computed in terms of their respective blocks.
00239 static void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
00240   
00241   block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
00242   block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
00243   int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2)
00244   int32_t accelerate_steps = 
00245     ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
00246   int32_t decelerate_steps = 
00247     floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute));
00248 
00249   // Calculate the size of Plateau of Nominal Rate. 
00250   int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
00251   
00252   // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
00253   // have to use intersection_distance() to calculate when to abort acceleration and start braking 
00254   // in order to reach the final_rate exactly at the end of this block.
00255   if (plateau_steps < 0) {  
00256     accelerate_steps = ceil(
00257       intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count));
00258     accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
00259     accelerate_steps = min(accelerate_steps,block->step_event_count);
00260     plateau_steps = 0;
00261   }  
00262   
00263   block->accelerate_until = accelerate_steps;
00264   block->decelerate_after = accelerate_steps+plateau_steps;
00265 }     
00266 
00267 /*                            PLANNER SPEED DEFINITION                                              
00268                                      +--------+   <- current->nominal_speed
00269                                     /          \                                
00270          current->entry_speed ->   +            \                               
00271                                    |             + <- next->entry_speed
00272                                    +-------------+                              
00273                                        time -->                                 
00274 */                                                                              
00275 // Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the 
00276 // entry_speed for each junction and the entry_speed of the next junction. Must be called by 
00277 // planner_recalculate() after updating the blocks. Any recalulate flagged junction will
00278 // compute the two adjacent trapezoids to the junction, since the junction speed corresponds 
00279 // to exit speed and entry speed of one another.
00280 static void planner_recalculate_trapezoids() {
00281   int8_t block_index = block_buffer_tail;
00282   block_t *current;
00283   block_t *next = NULL;
00284   
00285   while(block_index != block_buffer_head) {
00286     current = next;
00287     next = &block_buffer[block_index];
00288     if (current) {
00289       // Recalculate if current block entry or exit junction speed has changed.
00290       if (current->recalculate_flag || next->recalculate_flag) {
00291         // NOTE: Entry and exit factors always > 0 by all previous logic operations.     
00292         calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
00293           next->entry_speed/current->nominal_speed);      
00294         current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
00295       }
00296     }
00297     block_index = next_block_index( block_index );
00298   }
00299   // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
00300   calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
00301     MINIMUM_PLANNER_SPEED/next->nominal_speed);
00302   next->recalculate_flag = false;
00303 }
00304 
00305 // Recalculates the motion plan according to the following algorithm:
00306 //
00307 //   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) 
00308 //      so that:
00309 //     a. The junction speed is equal to or less than the maximum junction speed limit
00310 //     b. No speed reduction within one block requires faster deceleration than the one, true constant 
00311 //        acceleration.
00312 //   2. Go over every block in chronological order and dial down junction speed values if 
00313 //     a. The speed increase within one block would require faster acceleration than the one, true 
00314 //        constant acceleration.
00315 //
00316 // When these stages are complete all blocks have an entry speed that will allow all speed changes to 
00317 // be performed using only the one, true constant acceleration, and where no junction speed is greater
00318 // than the max limit. Finally it will:
00319 //
00320 //   3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids
00321 //      with no updated junction speeds will not be recalculated and assumed ok as is.
00322 //
00323 // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
00324 // off errors. Only when planned values are converted to stepper rate parameters, these are integers.
00325 
00326 static void planner_recalculate() {     
00327   planner_reverse_pass();
00328   planner_forward_pass();
00329   planner_recalculate_trapezoids();
00330 }
00331 
00332 void plan_set_acceleration_manager_enabled(uint8_t enabled) {
00333   if ((!!acceleration_manager_enabled) != (!!enabled)) {
00334     st_synchronize();
00335     acceleration_manager_enabled = !!enabled;
00336   }
00337 }
00338 
00339 int plan_is_acceleration_manager_enabled() {
00340   return(acceleration_manager_enabled);
00341 }
00342 
00343 void plan_discard_current_block() {
00344   if (block_buffer_head != block_buffer_tail) {
00345     block_buffer_tail = next_block_index( block_buffer_tail );
00346   }
00347 }
00348 
00349 block_t *plan_get_current_block() {
00350   if (block_buffer_head == block_buffer_tail) { return(NULL); }
00351   return(&block_buffer[block_buffer_tail]);
00352 }
00353 
00354 // Add a new Action movement to the buffer. x, y and z is the signed, absolute target position in 
00355 // millimeters. Feed rate specifies the speed of the motion. 
00356 void plan_buffer_line (tActionRequest *pAction)
00357 {
00358   float x;
00359   float y;
00360   float z;
00361   float feed_rate;
00362   bool e_only = false;
00363   float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis  
00364     
00365   x = pAction->target.x;
00366   y = pAction->target.y;
00367   z = pAction->target.z;
00368   feed_rate = pAction->target.feed_rate;
00369   
00370   // hard clipping. Might implement correct clipping some day...
00371   // JAAP: temporary disabled clipping because it caused parts of the print 
00372   // to "disappear" outside the working area, even though they were still 
00373   // with x/y limits!  This needs fixing!
00374   //if ( 1000*x < cfg->xmin || 1000*x > cfg->xmax ) return;
00375   //if ( 1000*y < cfg->ymin || 1000*y > cfg->ymax ) return;
00376   //if ( 1000*z < cfg->zmin || 1000*z > cfg->zmax ) return;
00377   
00378   
00379   // printf("%f %f %f %f %f\n", x,y,z,(float)feed_rate); 
00380   // Calculate target position in absolute steps
00381   int32_t target[NUM_AXES];
00382   target[X_AXIS] = lround(x*(float)config.steps_per_mm_x);
00383   target[Y_AXIS] = lround(y*(float)config.steps_per_mm_y);
00384   target[Z_AXIS] = lround(z*(float)config.steps_per_mm_z);     
00385   target[E_AXIS] = lround(pAction->target.e*(float)config.steps_per_mm_e);     
00386   
00387   // Calculate the buffer head after we push this byte
00388   int next_buffer_head = next_block_index( block_buffer_head );    
00389   
00390   // If the buffer is full: good! That means we are well ahead of the robot. 
00391   // Rest here until there is room in the buffer.
00392   while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
00393   
00394   // Prepare to set up new block
00395   block_t *block = &block_buffer[block_buffer_head];
00396 
00397   block->action_type = AT_MOVE;
00398   block->power = pAction->param;
00399   
00400   // Compute direction bits for this block
00401   block->direction_bits = 0;
00402   if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
00403   if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
00404   if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
00405   if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_DIRECTION_BIT); }
00406   
00407   // Number of steps for each axis
00408   block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
00409   block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
00410   block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
00411   block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
00412   block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
00413   block->step_event_count = max(block->step_event_count, block->steps_e);
00414 
00415   // Bail if this is a zero-length block
00416   if (block->step_event_count == 0) { return; };
00417   
00418   // Compute path vector in terms of absolute step target and current positions
00419   float delta_mm[NUM_AXES];
00420   delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)config.steps_per_mm_x;
00421   delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)config.steps_per_mm_y;
00422   delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)config.steps_per_mm_z;
00423   delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/(float)config.steps_per_mm_e;
00424   block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + 
00425                             square(delta_mm[Z_AXIS]));
00426   if (block->millimeters == 0)
00427   {
00428     e_only = true;
00429     block->millimeters = fabs(delta_mm[E_AXIS]);
00430   }
00431   float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides    
00432   
00433 //
00434 // Speed limit code from Marlin firmware
00435 //
00436   float microseconds;
00437   //if(feedrate<minimumfeedrate)
00438   //  feedrate=minimumfeedrate;
00439   microseconds = lround((block->millimeters/feed_rate*60.0)*1000000.0);
00440 
00441   // Calculate speed in mm/minute for each axis
00442   float multiplier = 60.0*1000000.0/(float)microseconds;
00443   speed_x = delta_mm[X_AXIS] * multiplier;
00444   speed_y = delta_mm[Y_AXIS] * multiplier;
00445   speed_z = delta_mm[Z_AXIS] * multiplier;
00446   speed_e = delta_mm[E_AXIS] * multiplier;
00447 
00448   // Limit speed per axis
00449   float speed_factor = 1; //factor <=1 do decrease speed
00450   if(fabs(speed_x) > config.maximum_feedrate_x) 
00451   {
00452     speed_factor = (float)config.maximum_feedrate_x / fabs(speed_x);
00453   }
00454   if(fabs(speed_y) > config.maximum_feedrate_y)
00455   {
00456     float tmp_speed_factor = (float)config.maximum_feedrate_y / fabs(speed_y);
00457     if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
00458   }
00459   if(fabs(speed_z) > config.maximum_feedrate_z)
00460   {
00461     float tmp_speed_factor = (float)config.maximum_feedrate_z / fabs(speed_z);
00462     if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
00463   }
00464   if(fabs(speed_e) > config.maximum_feedrate_e)
00465   {
00466     float tmp_speed_factor = (float)config.maximum_feedrate_e / fabs(speed_e);
00467     if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
00468   }
00469 
00470   multiplier = multiplier * speed_factor;
00471   speed_x = delta_mm[X_AXIS] * multiplier;
00472   speed_y = delta_mm[Y_AXIS] * multiplier;
00473   speed_z = delta_mm[Z_AXIS] * multiplier;
00474   speed_e = delta_mm[E_AXIS] * multiplier;
00475   block->nominal_speed = block->millimeters * multiplier;    // mm per min
00476   block->nominal_rate = ceil(block->step_event_count * multiplier);   // steps per minute
00477 
00478   
00479   // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
00480   // average travel per step event changes. For a line along one axis the travel per step event
00481   // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
00482   // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
00483   // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
00484   // specifically for each line to compensate for this phenomenon:
00485   // Convert universal acceleration for direction-dependent stepper rate change parameter
00486   block->rate_delta = ceil( block->step_event_count*inverse_millimeters *  
00487         config.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick)
00488 
00489   // Perform planner-enabled calculations
00490   if (acceleration_manager_enabled  ) 
00491   {  
00492     // Compute path unit vector                            
00493     float unit_vec[NUM_AXES];
00494 
00495     unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
00496     unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
00497     unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
00498   
00499     // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
00500     // Let a circle be tangent to both previous and current path line segments, where the junction 
00501     // deviation is defined as the distance from the junction to the closest edge of the circle, 
00502     // colinear with the circle center. The circular segment joining the two paths represents the 
00503     // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
00504     // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
00505     // path width or max_jerk in the previous grbl version. This approach does not actually deviate 
00506     // from path, but used as a robust way to compute cornering speeds, as it takes into account the
00507     // nonlinearities of both the junction angle and junction velocity.
00508     float vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
00509 
00510     // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
00511     if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
00512       // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
00513       // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
00514       float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
00515                          - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
00516                          - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
00517                            
00518       // Skip and use default max junction speed for 0 degree acute junction.
00519       if (cos_theta < 0.95) {
00520         vmax_junction = min(previous_nominal_speed,block->nominal_speed);
00521         // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
00522         if (cos_theta > -0.95) {
00523           // Compute maximum junction velocity based on maximum acceleration and junction deviation
00524           float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
00525           vmax_junction = min(vmax_junction,
00526             sqrt(config.acceleration*60*60 * config.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
00527         }
00528       }
00529     }
00530     block->max_entry_speed = vmax_junction;
00531     
00532     // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
00533     float v_allowable = max_allowable_speed(-config.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
00534     block->entry_speed = min(vmax_junction, v_allowable);
00535 
00536     // Initialize planner efficiency flags
00537     // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
00538     // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
00539     // the current block and next block junction speeds are guaranteed to always be at their maximum
00540     // junction speeds in deceleration and acceleration, respectively. This is due to how the current
00541     // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
00542     // the reverse and forward planners, the corresponding block junction speed will always be at the
00543     // the maximum junction speed and may always be ignored for any speed reduction checks.
00544     if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
00545     else { block->nominal_length_flag = false; }
00546     block->recalculate_flag = true; // Always calculate trapezoid for new block
00547   
00548     // Update previous path unit_vector and nominal speed
00549     memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
00550     previous_nominal_speed = block->nominal_speed;
00551 
00552   } else {
00553     // Acceleration planner disabled. Set minimum that is required.
00554     //  block->entry_speed = block->nominal_speed;
00555     block->initial_rate = block->nominal_rate;
00556     block->final_rate = block->nominal_rate;
00557     block->accelerate_until = 0;
00558     block->decelerate_after = block->step_event_count;
00559     block->rate_delta = 0;
00560   }
00561  
00562  // check action options 
00563   block->check_endstops = (pAction->ActionType == AT_MOVE_ENDSTOP);
00564   if (  pAction->ActionType == AT_LASER ) 
00565     block->options = OPT_LASER_ON;
00566   else if (  pAction->ActionType == AT_BITMAP ) 
00567     block->options = OPT_BITMAP;
00568   else
00569     block->options = 0;
00570   
00571   // now that the options are set: make this a MOVE action.
00572   pAction->ActionType = AT_MOVE;
00573   
00574   // Move buffer head
00575   block_buffer_head = next_buffer_head;     
00576   // Update position
00577   memcpy(position, target, sizeof(target)); // position[] = target[]
00578 
00579   startpoint = pAction->target;
00580   
00581   if (acceleration_manager_enabled) { planner_recalculate(); }  
00582   st_wake_up();
00583 }
00584 
00585 
00586 // push a wait (dwell) in the motion queue
00587 void plan_buffer_wait (tActionRequest *pAction)
00588 {
00589     
00590   // Calculate the buffer head after we push this block
00591   int next_buffer_head = next_block_index( block_buffer_head );    
00592   
00593   // If the buffer is full: good! That means we are well ahead of the robot. 
00594   // Rest here until there is room in the buffer.
00595   while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
00596   
00597   // Prepare to set up new block
00598   block_t *block = &block_buffer[block_buffer_head];
00599   
00600   //TODO
00601   
00602   block->action_type = pAction->ActionType;
00603   // every 50ms
00604   block->millimeters = 10;
00605   block->nominal_speed = 600;
00606   block->nominal_rate = 20*60;
00607   
00608   block->step_event_count = 1000;
00609   
00610     // Acceleration planner disabled. Set minimum that is required.
00611     block->entry_speed = block->nominal_speed;
00612     
00613     block->initial_rate = block->nominal_rate;
00614     block->final_rate = block->nominal_rate;
00615     block->accelerate_until = 0;
00616     block->decelerate_after = block->step_event_count;
00617     block->rate_delta = 0;
00618     
00619   // Move buffer head
00620   block_buffer_head = next_buffer_head;     
00621 
00622   if (acceleration_manager_enabled) { planner_recalculate(); }  
00623   st_wake_up();    
00624 }
00625 
00626 // Enqueue an action. Either move, laser, endstop or wait. 
00627 void plan_buffer_action(tActionRequest *pAction)
00628 {
00629   switch (pAction->ActionType)
00630   {
00631   case AT_MOVE:
00632   case AT_LASER:
00633   case AT_BITMAP:
00634   case AT_MOVE_ENDSTOP:
00635     plan_buffer_line (pAction);
00636     break;    
00637   case AT_WAIT:
00638     plan_buffer_wait (pAction);
00639     break;
00640   }
00641 }
00642 
00643 // Reset the planner position vector and planner speed
00644 void plan_get_current_position_xyz(float *x, float *y, float *z)
00645 {
00646   *x = position[X_AXIS] / config.steps_per_mm_x;
00647   *y = position[Y_AXIS] / config.steps_per_mm_y;
00648   *z = position[Z_AXIS] / config.steps_per_mm_z;
00649 }
00650 
00651 
00652 // Reset the planner position vector and planner speed
00653 void plan_set_current_position_xyz(float x, float y, float z)
00654 {
00655   tTarget new_pos = startpoint;
00656   new_pos.x = x;
00657   new_pos.y = y;
00658   new_pos.z = z;
00659   plan_set_current_position (&new_pos);
00660 }
00661 
00662 // Set absolute position
00663 void plan_set_current_position(tTarget *new_position) 
00664 {
00665   startpoint = *new_position;
00666   position[X_AXIS] = lround(new_position->x*(float)config.steps_per_mm_x);
00667   position[Y_AXIS] = lround(new_position->y*(float)config.steps_per_mm_y);
00668   position[Z_AXIS] = lround(new_position->z*(float)config.steps_per_mm_z);    
00669   position[E_AXIS] = lround(new_position->e*(float)config.steps_per_mm_e);    
00670   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
00671   clear_vector_double(previous_unit_vec);
00672   printf("Set Position: %d,%d,%d,%d", position[X_AXIS],  position[Y_AXIS],  position[Z_AXIS],  position[E_AXIS]);
00673 }
00674 
00675 // Force the feedrate
00676 //void plan_set_feed_rate (tTarget *new_position) 
00677 //{
00678 // startpoint.feed_rate = new_position->feed_rate;
00679 //}
00680 
00681 // return true if queue is filled
00682 uint8_t plan_queue_full (void)
00683 {
00684   int next_buffer_head = next_block_index( block_buffer_head );    
00685   
00686   if (block_buffer_tail == next_buffer_head)
00687     return 1;
00688   else
00689     return 0;
00690 }
00691 
00692 // Return true if queue is empty
00693 uint8_t plan_queue_empty(void) 
00694 {
00695   if (block_buffer_head == block_buffer_tail)
00696     return 1;
00697   else
00698     return 0;
00699 }
00700 
00701 // Return nr of items in the queue
00702 uint8_t plan_queue_items(void) 
00703 {
00704  // BLOCK_BUFFER_SIZE;
00705   int len =  block_buffer_head - block_buffer_tail;
00706   if ( len < 0 ) len = -len;
00707   return len;
00708 }
00709 
00710