src/movement/planner_lookahead.c

Go to the documentation of this file.

/*
 * planner_lookahead.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf 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.
 *
 * Yarf 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 Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "planner_lookahead.h"

#include "yarf.h"
#include "planner.h"
#include "planner_queue.h"
#include "util/math.h"

#include <stdint.h>
#include <string.h>


#if PLANNER_LOOKAHEAD

static struct {
  float direction[NUM_AXES]; 
  float plateau_speed; 
} previous_block_info;





static speed_t 
max_allowable_speed(speed_t target_speed, float distance_mm)
{
  return sqrt(lsquare(target_speed)+2*ACCELERATION*distance_mm);
}



static void 
reverse_pass_kernel(block_t *current, block_t *next)
{
  if (! current) { return; }
  
  if (next) { 
    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and 
    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
    if (current->entry_speed < current->max_entry_speed) {
      if (current->plateau_speed_always_reachable || current->max_entry_speed < next->entry_speed) {
        current->entry_speed = current->max_entry_speed;
      } else {
        current->entry_speed = MIN(current->max_entry_speed, max_allowable_speed(next->entry_speed,current->distance_mm));
      }
      current->recalculate = true;    
    }
  } // Skip last block. Already initialized and set for recalculation.
}


static void
reverse_pass(void)
{
  uint8_t block_index = plan_q_head_index();
  block_t *block[2] = {NULL, NULL};
  while(block_index != plan_q_tail_index()) {
    block_index = plan_q_prev_index(block_index);
    block[1] = block[0];
    block[0] = plan_q_get(block_index);
    reverse_pass_kernel(block[0], block[1]);
  }
}


static void
forward_pass_kernel(block_t *previous, block_t *current)
{
  if (! previous) { return; }  // Begin planning after buffer_tail
  
  // If the previous block is an acceleration block, but it is not long enough to complete the
  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
  // speeds have already been reset, maximized, and reverse planned by reverse planner.
  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.  
  if (! previous->plateau_speed_always_reachable) {
    if (previous->entry_speed < current->entry_speed) {
      speed_t max_obtainable_speed = max_allowable_speed(previous->entry_speed,previous->distance_mm);

      // Check for junction speed change
      if (max_obtainable_speed < current->entry_speed) {
        current->entry_speed = max_obtainable_speed;
        current->recalculate = true;
      }
    }
  }
}




static void 
forward_pass(void)
{
  uint8_t block_index = plan_q_tail_index();
  block_t *block[2] = {NULL, NULL};
  while(block_index != plan_q_head_index()) {
    block[0] = block[1];
    block[1] = plan_q_get(block_index);
    forward_pass_kernel(block[0],block[1]);
    block_index = plan_q_next_index(block_index);
  }
}

                                                                          
static void
recalculate_trapezoids(void)
{
  uint8_t block_index = plan_q_tail_index();
  block_t *current;
  block_t *next = NULL;
  
  while(block_index != plan_q_head_index()) {
    current = next;
    next = plan_q_get(block_index);
    if (current) {
      // Recalculate if current block entry or exit junction speed has changed.
      if (current->recalculate || next->recalculate) {
        plan_calculate_trapezoid(current, current->entry_speed, next->entry_speed);
        current->recalculate = false;
      }
    }
    block_index = plan_q_next_index(block_index);
  }
  // Last/newest block in buffer. Exit speed is set with START_FEED_RATE. Always recalculated.
  plan_calculate_trapezoid(next, next->entry_speed, MIN(next->plateau_speed, START_FEED_RATE));
  next->recalculate = false;
}


static inline void 
recalculate_motion_plan(void) {     
  reverse_pass();
  forward_pass();
  recalculate_trapezoids();
}


void
plan_lookahead_init()
{
  previous_block_info.plateau_speed = 0;
}


void
plan_lookahead()
{
  block_t *b = plan_q_get(plan_q_prev_index(plan_q_head_index()));
  // 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.
  const speed_t start_feed_rate = MIN(b->plateau_speed, START_FEED_RATE);
  speed_t max_junction_speed = start_feed_rate; // Set default conservative max junction speed

  // Skip first block
  if (plan_q_size() >= 2) {
    // Make sure the junction speed will not be higher than the previous plateau speed.
    max_junction_speed = MIN(max_junction_speed, previous_block_info.plateau_speed);
    // Compute cosine of angle between previous and current path.
    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
    float cos_theta = -(previous_block_info.direction[X_AXIS] * b->direction[X_AXIS] +
                        previous_block_info.direction[Y_AXIS] * b->direction[Y_AXIS] +
                        previous_block_info.direction[Z_AXIS] * b->direction[Z_AXIS] +
                        previous_block_info.direction[E_AXIS] * b->direction[E_AXIS]) ;

    // Skip and use default max junction speed for 0 degree acute junction.
    if (cos_theta < 0.95) { 
      // Skip and avoid divide by zero for straight junctions at 180 degrees.
      if (cos_theta > -0.95) { // -0.95 < cos_theta < 0.95
        // Compute maximum junction velocity based on maximum acceleration and junction deviation
        float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
        max_junction_speed = MIN(max_junction_speed, sqrt(ACCELERATION * JUNCTION_DEVIATION * sin_theta_d2/(1.0-sin_theta_d2)) );
      } // else { very unsharp corner }
    } // else { very sharp corner } 
  }
  b->max_entry_speed = max_junction_speed;

  // Calculate the maximum allowable speed to still be able to decelerate to START_FEED_RATE over
  // the length of the block.
  speed_t max_allowable_speed_ = max_allowable_speed(start_feed_rate,b->distance_mm);
  b->entry_speed = MIN(max_junction_speed, max_allowable_speed_);

  // Initialize planner efficiency flags
  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  // If a block can decelerate from nominal speed to minimum speed within the length of the block, then
  // the current block and next block junction speeds are guaranteed to always be at their maximum
  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  // the reverse and forward planners, the corresponding block junction speed will always be at the
  // the maximum junction speed and may always be ignored for any speed reduction checks.
  b->plateau_speed_always_reachable = (b->plateau_speed <= max_allowable_speed_);
  b->recalculate = true; // Always calculate trapezoid for new block
  
  // Update previous path unit_vector and nominal speed
  memcpy(previous_block_info.direction, b->direction, sizeof(b->direction));
  previous_block_info.plateau_speed = b->plateau_speed;

  recalculate_motion_plan();
}

#endif //PLANNER_LOOKAHEAD