src/movement/block_handler.c

Go to the documentation of this file.

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

#include "yarf.h"
#include "movement/planner_queue.h"
#include "hardware/steppers.h"
#include "hardware/endstops.h"
#include "hardware/usart.h"
#include "scheduling/realtime_timer.h"
#include "util/math.h"
#include "advance.h"

#include <stdlib.h>
#include <util/delay.h>
#include <util/atomic.h>

#define BLOCK_HANDLER_START_DELAY_TICKS 1L

static bool handler_running;

static realtime_task_t handler_task;


static block_t *b;


static steps_t counter[NUM_AXES]; 


#if ADVANCE_ALGORITHM

static volatile int32_t e_steps;

static int32_t previous_advance;

static uint8_t extruder_tick_state;
#endif

static void
block_progress_init(void)
{
  int axis;
  for(axis = 0; axis < NUM_AXES; ++axis) {
    counter[axis] = -(b->nb_steps/2);
  }
}

static inline void
enable_steppers(void)
{
  if (b->steps[X_AXIS] != 0) {
    stepper_enable_x();
  }
  if (b->steps[Y_AXIS] != 0) {
    stepper_enable_y();
  }
  if (b->steps[Z_AXIS] != 0) {
    stepper_enable_z();
  }
  if (b->steps[E_AXIS] != 0) {
    stepper_enable_e();
  }
}

static inline void
disable_steppers(void)
{
#if !STEPPER_HOLD_X
  stepper_disable_x();
#endif
#if !STEPPER_HOLD_Y
  stepper_disable_y();
#endif
#if !STEPPER_HOLD_Z
  stepper_disable_z();
#endif
#if !STEPPER_HOLD_E && !ADVANCE_ALGORITHM
  stepper_disable_e();
#endif
}

static inline void
block_finished(void)
{
  b = NULL;

  /* Remove the block from the tail of the queue */
  plan_q_discard_tail();
  
  /* Disable the stepper motors that need not be held */
  disable_steppers();
}

static inline void
cancel_block(void)
{
  block_finished();
  rttimer_schedule_ticks(&handler_task,1);
}


#if ADVANCE_ALGORITHM
//TODO: enable/disable interrupt

static inline void
block_handler_extruder_tick(void)
{
  /* Set the timer 1 A top value */
  OCR1A += EXTRUDER_TIMER_TICKS;
  TIFR1 |= _BV(OCF1A);

  /* Check the USART0 receive buffer, to prevent data overrun */
  usart0_try_read_character();

  if (extruder_tick_state == 0) {
    extruder_tick_state = 1;
    stepper_e_step_low();
  } else {
    extruder_tick_state = 0;
      if (e_steps == 0) {
        return;
      }
      
      if (e_steps < 0) {
        stepper_e_dir_neg();
        e_steps += 1;
      } else if (e_steps > 0) {
        stepper_e_dir_pos();
        e_steps -= 1;
      }
    stepper_e_step_high();
  }  
}

ISR(TIMER1_COMPA_vect, ISR_BLOCK)
{
  block_handler_extruder_tick();
}

#endif


static inline void
update_advance(void)
{
#if ADVANCE_ALGORITHM
  b->advance = advance_get_value(b);
#endif
}

static void
block_handler_run(void)
{
  if (b == NULL) {
    /* Get new block to execute from the planning queue */
    b = plan_q_tail();
    if (b == NULL) {
      /* No blocks to be executed */
      handler_running = false;
      return;
    } else {
      block_progress_init();
      enable_steppers();
    }
  }


  /* Make sure all the stepper inputs are low */
  stepper_x_step_low();
  stepper_y_step_low();
  stepper_z_step_low();
#if !ADVANCE_ALGORITHM
  stepper_e_step_low();
#endif

  
  /* Setup the steppers' direction pins and check the endstops */
  uint8_t collisions = 0;
  if (b->steps[X_AXIS] < 0) {
    stepper_x_dir_neg();
    if (endstop_x_min()) {
      collisions |= _BV(X_AXIS_NEG);
    }
  } else if (b->steps[X_AXIS] > 0) {
    stepper_x_dir_pos();
    if (endstop_x_max()) {
      collisions |= _BV(X_AXIS_POS);
    }
  }
  if (b->steps[Y_AXIS] < 0) {
    stepper_y_dir_neg();
    if (endstop_y_min()) {
      collisions |= _BV(Y_AXIS_NEG);
    }
  } else if (b->steps[Y_AXIS] > 0) {
    stepper_y_dir_pos();
    if (endstop_y_max()) {
      collisions |= _BV(Y_AXIS_POS);
    }
  }
  if (b->steps[Z_AXIS] < 0) {
    stepper_z_dir_neg();
    if (endstop_z_min()) {
      collisions |= _BV(Z_AXIS_NEG);
    }
  } else if (b->steps[Z_AXIS] > 0) {
    stepper_z_dir_pos();
    if (endstop_z_max()) {
      collisions |= _BV(Z_AXIS_POS);
    }
  }
#if !ADVANCE_ALGORITHM
  if (b->steps[E_AXIS] < 0) {
    stepper_e_dir_neg();
  } else {
    stepper_e_dir_pos();
  }
#endif

  if (collisions) {
    b->collision_handler(collisions);
    cancel_block();
    return;
  }

  /* Step all the steppers that need to step */
  counter[X_AXIS] += labs(b->steps[X_AXIS]);
  if (counter[X_AXIS] > 0) {
    counter[X_AXIS] -= b->nb_steps;
    stepper_x_step_high();
  }
  counter[Y_AXIS] += labs(b->steps[Y_AXIS]);
  if (counter[Y_AXIS] > 0) {
    counter[Y_AXIS] -= b->nb_steps;
    stepper_y_step_high();
  }
  counter[Z_AXIS] += labs(b->steps[Z_AXIS]);
  if (counter[Z_AXIS] > 0) {
    counter[Z_AXIS] -= b->nb_steps;
    stepper_z_step_high();
  }
#if !ADVANCE_ALGORITHM
  counter[E_AXIS] += labs(b->steps[E_AXIS]);
  if (counter[E_AXIS] > 0) {
    counter[E_AXIS] -= b->nb_steps;
    stepper_e_step_high();
  }
#else
  /* Calculate E steps to take */
  counter[E_AXIS] += labs(b->steps[E_AXIS]);
  if (counter[E_AXIS] > 0) {
    counter[E_AXIS] -= b->nb_steps;
    if (b->steps[E_AXIS] < 0) {
      e_steps -= 1;
    } else {
      e_steps += 1;
    }
  }    
  /* Do advance steps */
  e_steps += (b->advance - previous_advance);
  previous_advance = b->advance;
  block_handler_extruder_tick();
#endif



  /* Reschedule the timer */
  b->nb_steps_completed += 1;
  fxp16u16_t diff = fxp16u16_div_uint16(2*b->timer_ticks, b->acceleration_step);
  if (b->nb_steps_completed < b->acceleration_end) {
    /* We're in the acceleration slope */
    if (b->timer_ticks - b->plateau_timer_ticks > diff) { // overflow detection
      b->timer_ticks -= diff;
    } else {
      b->timer_ticks = b->plateau_timer_ticks;
    }

    b->acceleration_step += 4;
    update_advance();
  } else if (b->nb_steps_completed >= b->deceleration_start) {
    /* We're in the deceleration slope */
    if (b->timer_ticks < b->exit_timer_ticks &&
        b->exit_timer_ticks - b->timer_ticks > diff) { // overflow detection
      b->timer_ticks += diff; 
    } else {
      b->timer_ticks = b->exit_timer_ticks;
    }

    if (b->acceleration_step > 4) {
      b->acceleration_step -= 4;
    } else {
      b->acceleration_step = 1;
    }
    update_advance();
  } else {
    b->timer_ticks = b->plateau_timer_ticks;
  }
  rttimer_schedule_ticks(&handler_task, fxp16u16_to_uint16(b->timer_ticks));


  /* Check if we're done with this block */
  if (b->nb_steps_completed >= b->nb_steps) {
    block_finished();
  }
}



void
block_handler_init(void)
{
  rttimer_init();

  b = NULL;
  handler_running = false;
  handler_task.f = block_handler_run;

#if ADVANCE_ALGORITHM
  e_steps = 0;
  previous_advance = 0;
  extruder_tick_state = 0;
#endif
}

void 
block_handler_start(void)
{
  if (! handler_running) {
    handler_running = true;
    rttimer_schedule_ticks(&handler_task, BLOCK_HANDLER_START_DELAY_TICKS);
  }
}

void
block_handler_pause(void)
{
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if (handler_running) {
      rttimer_cancel();
      handler_running = false;    
    }
  }
}

void
block_handler_stop(void)
{
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if (handler_running) {
      rttimer_cancel();
      if (b != NULL) {
        block_finished();
      }
      handler_running = false;    
    }
  }
}

bool
block_handler_running(void)
{
  return handler_running;
}