src/movement/planner.c

Go to the documentation of this file.

/*
 * planner.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.h"

#include "yarf.h"
#include "block.h"
#include "planner_queue.h"
#include "planner_lookahead.h"
#include "block_handler.h"
#include "advance.h"
#include "scheduling/realtime_timer.h"
#include "util/math.h"
#include "util/fixed_point.h"

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <util/atomic.h>

#define HOME_AXIS_CALIBRATION_MM_X  5

#define HOME_AXIS_CALIBRATION_MM_Y  5

#define HOME_AXIS_CALIBRATION_MM_Z  1

#define HOME_AXIS_CALIBRATION_SPEED_X 300

#define HOME_AXIS_CALIBRATION_SPEED_Y 300

#define HOME_AXIS_CALIBRATION_SPEED_Z 50


static uint8_t axis_homed;


static steps_t virtual_position[NUM_AXES];


static unsigned int next_id;



static inline int32_t
intersection_steps(speed_t entry_speed, speed_t exit_speed, float distance_mm, long nb_steps)
{
  return lround(nb_steps * ( ((exit_speed+entry_speed)*(exit_speed-entry_speed))/(4*ACCELERATION*distance_mm) + 0.5 ));
}


static inline fxp16u16_t
ticks_float_to_fxp16u16_safe(float ticks_fl)
{
  if (ticks_fl < fxp16u16_to_float(FXP_16U16_MAX/2)) {
    return fxp16u16_from_float(ticks_fl);
  } else {
    return FXP_16U16_MAX/2;
  }
}



static inline void
initialize_block(block_t *b, steps_t steps_x, steps_t steps_y, steps_t steps_z, steps_t steps_e, speed_t plateau_speed, void (*collision_handler)(uint8_t))
{
  /* Set the block's id */
  b->id = next_id++;

  /* Set the collision handler */
  b->collision_handler = collision_handler;

  /* The number of steps in each direction and the total number of steps */
  b->steps[X_AXIS] = steps_x;
  b->steps[Y_AXIS] = steps_y;
  b->steps[Z_AXIS] = steps_z;
  b->steps[E_AXIS] = steps_e;
  b->nb_steps = MAX(labs(b->steps[X_AXIS]), MAX(labs(b->steps[Y_AXIS]),
                  MAX(labs(b->steps[Z_AXIS]), labs(b->steps[E_AXIS]))));
  b->deceleration_start = b->nb_steps;
  b->nb_steps_completed = 0;

  /* The distance to travel in each direction in mm */
  float distance_mm[NUM_AXES];
  distance_mm[X_AXIS] = steps_x*X_MM_PER_STEP;
  distance_mm[Y_AXIS] = steps_y*Y_MM_PER_STEP;
  distance_mm[Z_AXIS] = steps_z*Z_MM_PER_STEP;
  distance_mm[E_AXIS] = steps_e*E_MM_PER_STEP;

  /* The straight-line distance (must not be zero) */
  float distance_mm_line = sqrt(
    fsquare(distance_mm[X_AXIS])+ 
    fsquare(distance_mm[Y_AXIS])+ 
    fsquare(distance_mm[Z_AXIS])+ 
    fsquare(distance_mm[E_AXIS]));
  b->distance_mm = distance_mm_line;
  b->mm_ticks_per_step_min = ((float)(distance_mm_line*RTTIMER_TICKS_PER_MIN))/b->nb_steps; // mm per step multiplied by RTTIMER_TICKS_PER_MIN

  /* The 4D direction unit vector of this move */
  float distance_mm_line_inverse = 1.0/distance_mm_line;
  b->direction[X_AXIS] = distance_mm[X_AXIS]*distance_mm_line_inverse;
  b->direction[Y_AXIS] = distance_mm[Y_AXIS]*distance_mm_line_inverse;
  b->direction[Z_AXIS] = distance_mm[Z_AXIS]*distance_mm_line_inverse;
  b->direction[E_AXIS] = distance_mm[E_AXIS]*distance_mm_line_inverse;

  /* Limit the feed rate according to the max feed rate of each axis */
  if (distance_mm[X_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(X_MAX_SPEED/b->direction[X_AXIS])));
  if (distance_mm[Y_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(Y_MAX_SPEED/b->direction[Y_AXIS])));
  if (distance_mm[Z_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(Z_MAX_SPEED/b->direction[Z_AXIS])));
  if (distance_mm[E_AXIS] > 0) plateau_speed = MIN(plateau_speed, labs(lround(E_MAX_SPEED/b->direction[E_AXIS])));
  b->plateau_speed = plateau_speed;
  speed_t start_feed_rate = MIN(plateau_speed, START_FEED_RATE);

  /* Set the number of ticks in between steps at the plateau */
  b->plateau_timer_ticks = ticks_float_to_fxp16u16_safe(b->mm_ticks_per_step_min/plateau_speed);


  /* Calculate a conservative acceleration/deceleration trapezoid */
  plan_calculate_trapezoid(b, start_feed_rate, start_feed_rate);

#if ADVANCE_ALGORITHM
  if ((steps_e == 0) || (steps_x == 0 && steps_y == 0 && steps_z == 0)) {
    b->use_advance = false;
  } else {
    b->use_advance = true;
  }
  b->advance_multiplier = advance_get_multiplier(b);
  b->advance = advance_get_value(b);
#endif
}


static void 
linear_movement_rel(steps_t steps_x, steps_t steps_y, steps_t steps_z, steps_t steps_e, speed_t plateau_speed, void (*collision_handler)(uint8_t))
{
  if (steps_x == 0 && steps_y == 0 && steps_z == 0 && steps_e == 0) {
    /* This is a null-move */
    return;
  }
  if (plateau_speed <= 0) {
    /* Invalid feed rate */
    return;
  }

  
  block_t *b = plan_q_head();
  while (b == NULL) {
    /* Wait until space is available in the planning queue */
    b = plan_q_head();
  } 
  
  // Fill in the variables required for the block handler
  initialize_block(b, steps_x, steps_y, steps_z, steps_e, plateau_speed, collision_handler);
  // Move planning queue head
  plan_q_shift_head();

#if PLANNER_LOOKAHEAD
  plan_lookahead();
#endif

  plan_set_virtual_position_rel(steps_x,steps_y, steps_z,steps_e);
}

static void
collision(uint8_t collisions)
{
  block_handler_stop();
  plan_q_clear();
  // TODO: properly signal hardware fault
  puts("!! // collision on axis ");
  if (collisions & _BV(X_AXIS_POS)) {
    puts("X+ ");
  }
  if (collisions & _BV(X_AXIS_NEG)) {
    puts("X- ");
  }
  if (collisions & _BV(Y_AXIS_POS)) {
    puts("Y+ ");
  }
  if (collisions & _BV(Y_AXIS_NEG)) {
    puts("Y- ");
  }
  if (collisions & _BV(Z_AXIS_POS)) {
    puts("Z+ ");
  }
  if (collisions & _BV(Z_AXIS_NEG)) {
    puts("Z- ");
  }
  puts("while moving!\n");
}

static void
homing_collision(uint8_t collisions)
{
  return;
}



void
plan_init(void)
{
  plan_q_init();
#if PLANNER_LOOKAHEAD
  plan_lookahead_init();
#endif
  axis_homed = 0;
  virtual_position[E_AXIS] = 0;
  next_id = 0;
}

steps_t
plan_get_virtual_position(unsigned char axis)
{
  return virtual_position[axis];
}

void
plan_set_virtual_position_rel(steps_t x, steps_t y, steps_t z, steps_t e)
{
  virtual_position[X_AXIS] += x;
  virtual_position[Y_AXIS] += y;
  virtual_position[Z_AXIS] += z;
  virtual_position[E_AXIS] += e;
}

void
plan_set_virtual_position_abs(steps_t x, steps_t y, steps_t z, steps_t e)
{
  virtual_position[X_AXIS] = x;
  virtual_position[Y_AXIS] = y;
  virtual_position[Z_AXIS] = z;
  virtual_position[E_AXIS] = e;
}


void
plan_calculate_trapezoid(block_t *b, speed_t entry_speed, speed_t exit_speed)
{
  /* Calculate the acceleration variables */
  float common_multiplier = ((float)(b->nb_steps * 2))/(ACCELERATION * b->distance_mm);
  uint16_t entry_speed_step = (uint16_t)lround(lsquare(entry_speed) * common_multiplier) + 1;
  uint16_t exit_speed_step = (uint16_t)lround(lsquare(exit_speed) * common_multiplier) + 1;
  uint16_t plateau_reached_step = (uint16_t)lround(lsquare(b->plateau_speed) * common_multiplier) + 1;

  int32_t nb_acceleration_steps = (plateau_reached_step - entry_speed_step)/4;
  int32_t nb_deceleration_steps = (plateau_reached_step - exit_speed_step)/4;
  int32_t nb_plateau_steps = b->nb_steps - nb_acceleration_steps - nb_deceleration_steps;

  if (nb_plateau_steps < 0) {  
    nb_acceleration_steps = BOUNDS(0,intersection_steps(entry_speed,exit_speed,b->distance_mm,b->nb_steps),b->nb_steps);
    nb_plateau_steps = 0;
  }
  uint32_t deceleration_start = (uint32_t)(nb_acceleration_steps + nb_plateau_steps);
  fxp16u16_t entry_timer_ticks = ticks_float_to_fxp16u16_safe(b->mm_ticks_per_step_min/entry_speed); //rttimer ticks per step
  fxp16u16_t exit_timer_ticks = ticks_float_to_fxp16u16_safe(b->mm_ticks_per_step_min/exit_speed); //rttimer ticks per step


  /* Fill in the block acceleration variables if the block hasn't started yet */
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if (b->nb_steps_completed < deceleration_start && 
        b->nb_steps_completed < b->deceleration_start) {
      b->deceleration_start = deceleration_start;
      b->exit_timer_ticks = exit_timer_ticks; 
    }
    if (b->nb_steps_completed == 0) {
      b->acceleration_end = nb_acceleration_steps;
      b->timer_ticks = entry_timer_ticks;
      b->acceleration_step = entry_speed_step;
    }
  }
}

void 
plan_linear_movement_rel(steps_t steps_x, steps_t steps_y, steps_t steps_z, steps_t steps_e, speed_t plateau_speed)
{
  if (axis_homed != (_BV(X_AXIS)|_BV(Y_AXIS)|_BV(Z_AXIS))) {
    /* Home all axes before we make a linear movement */
    plan_home_axes(!(axis_homed & _BV(X_AXIS)),!(axis_homed & _BV(Y_AXIS)),!(axis_homed & _BV(Z_AXIS)));
  }

  if ((steps_x < -virtual_position[X_AXIS]) ||
      (steps_y < -virtual_position[Y_AXIS]) ||
      (steps_z < -virtual_position[Z_AXIS]) ||
      (steps_x > X_LENGTH*X_STEPS_PER_MM - virtual_position[X_AXIS]) ||
      (steps_y > Y_LENGTH*Y_STEPS_PER_MM - virtual_position[Y_AXIS]) ||
      (steps_z > Z_LENGTH*Z_STEPS_PER_MM - virtual_position[Z_AXIS])) {
    /* This is a request to move out of bounds */
    //TODO: signal error
    return;
  }

  linear_movement_rel(steps_x, steps_y, steps_z, steps_e, plateau_speed, collision);
}


void 
plan_linear_movement_abs(steps_t dest_x, steps_t dest_y, steps_t dest_z, steps_t dest_e, speed_t plateau_speed)
{
  plan_linear_movement_rel(
    dest_x - virtual_position[X_AXIS],
    dest_y - virtual_position[Y_AXIS],
    dest_z - virtual_position[Z_AXIS],
    dest_e - virtual_position[E_AXIS],
    plateau_speed
  );
}



void 
plan_home_axes(bool x, bool y, bool z)
{
  /* First move all axes to the origin fast, then move away from the origin fast
   * and finally return to the origin slowly for increased accuracy. */
  if (x) {
    linear_movement_rel((-2 * X_LENGTH * X_STEPS_PER_MM), 0, 0, 0, X_MAX_SPEED/2, homing_collision);
    linear_movement_rel((HOME_AXIS_CALIBRATION_MM_X * X_STEPS_PER_MM), 0, 0, 0, X_MAX_SPEED, collision);
    linear_movement_rel((-2 * HOME_AXIS_CALIBRATION_MM_X * X_STEPS_PER_MM), 0, 0, 0, HOME_AXIS_CALIBRATION_SPEED_X, homing_collision);
    virtual_position[X_AXIS] = 0;
    axis_homed |= _BV(X_AXIS);
  }
  if (y) {
    linear_movement_rel(0, (-2 * Y_LENGTH * Y_STEPS_PER_MM), 0, 0, Y_MAX_SPEED/2, homing_collision);
    linear_movement_rel(0, (HOME_AXIS_CALIBRATION_MM_Y * Y_STEPS_PER_MM), 0, 0, Y_MAX_SPEED, collision);
    linear_movement_rel(0, (-2 * HOME_AXIS_CALIBRATION_MM_Y * Y_STEPS_PER_MM), 0, 0, HOME_AXIS_CALIBRATION_SPEED_Y, homing_collision);
    virtual_position[Y_AXIS] = 0;
    axis_homed |= _BV(Y_AXIS);
  }
  if (z) {
    linear_movement_rel(0, 0, (-2 * Z_LENGTH * Z_STEPS_PER_MM), 0, Z_MAX_SPEED/2, homing_collision);
    linear_movement_rel(0, 0, (HOME_AXIS_CALIBRATION_MM_Z * Z_STEPS_PER_MM), 0, Z_MAX_SPEED, collision);
    linear_movement_rel(0, 0, (-2 * HOME_AXIS_CALIBRATION_MM_Z * Z_STEPS_PER_MM), 0, HOME_AXIS_CALIBRATION_SPEED_Z, homing_collision);
    virtual_position[Z_AXIS] = 0;
    axis_homed |= _BV(Z_AXIS);
  }
}