Marlin/stepper.cpp

   1 /*
   2   stepper.c - stepper motor driver: executes motion plans using stepper motors
   3   Part of Grbl
   4
   5   Copyright (c) 2009-2011 Simen Svale Skogsrud
   6
   7   Grbl is free software: you can redistribute it and/or modify
   8   it under the terms of the GNU General Public License as published by
   9   the Free Software Foundation, either version 3 of the License, or
  10   (at your option) any later version.
  11
  12   Grbl is distributed in the hope that it will be useful,
  13   but WITHOUT ANY WARRANTY; without even the implied warranty of
  14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15   GNU General Public License for more details.
  16
  17   You should have received a copy of the GNU General Public License
  18   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
  19 */
  20
  21 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
  22    and Philipp Tiefenbacher. */
  23
  24 #include "Marlin.h"
  25 #include "stepper.h"
  26 #include "planner.h"
  27 #include "temperature.h"
  28 #include "ultralcd.h"
  29 #include "language.h"
  30 #include "speed_lookuptable.h"
  31
  32
  33
  34 //===========================================================================
  35 //=============================public variables  ============================
  36 //===========================================================================
  37 block_t *current_block;  // A pointer to the block currently being traced
  38
  39
  40 //===========================================================================
  41 //=============================private variables ============================
  42 //===========================================================================
  43 //static makes it inpossible to be called from outside of this file by extern.!
  44
  45 // Variables used by The Stepper Driver Interrupt
  46 static unsigned char out_bits;        // The next stepping-bits to be output
  47 static long counter_x,       // Counter variables for the bresenham line tracer
  48             counter_y,
  49             counter_z,
  50             counter_e;
  51 volatile static unsigned long step_events_completed; // The number of step events executed in the current block
  52 #ifdef ADVANCE
  53   static long advance_rate, advance, final_advance = 0;
  54   static long old_advance = 0;
  55 #endif
  56 static long e_steps[3];
  57 static long acceleration_time, deceleration_time;
  58 //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
  59 static unsigned short acc_step_rate; // needed for deccelaration start point
  60 static char step_loops;
  61 static unsigned short OCR1A_nominal;
  62
  63 volatile long endstops_trigsteps[3]={0,0,0};
  64 volatile long endstops_stepsTotal,endstops_stepsDone;
  65 static volatile bool endstop_x_hit=false;
  66 static volatile bool endstop_y_hit=false;
  67 static volatile bool endstop_z_hit=false;
  68
  69 static bool old_x_min_endstop=false;
  70 static bool old_x_max_endstop=false;
  71 static bool old_y_min_endstop=false;
  72 static bool old_y_max_endstop=false;
  73 static bool old_z_min_endstop=false;
  74 static bool old_z_max_endstop=false;
  75
  76 static bool check_endstops = true;
  77
  78 volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
  79 volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
  80
  81 //===========================================================================
  82 //=============================functions         ============================
  83 //===========================================================================
  84
  85 #define CHECK_ENDSTOPS  if(check_endstops)
  86
  87 // intRes = intIn1 * intIn2 >> 16
  88 // uses:
  89 // r26 to store 0
  90 // r27 to store the byte 1 of the 24 bit result
  91 #define MultiU16X8toH16(intRes, charIn1, intIn2) \
  92 asm volatile ( \
  93 "clr r26 \n\t" \
  94 "mul %A1, %B2 \n\t" \
  95 "movw %A0, r0 \n\t" \
  96 "mul %A1, %A2 \n\t" \
  97 "add %A0, r1 \n\t" \
  98 "adc %B0, r26 \n\t" \
  99 "lsr r0 \n\t" \
 100 "adc %A0, r26 \n\t" \
 101 "adc %B0, r26 \n\t" \
 102 "clr r1 \n\t" \
 103 : \
 104 "=&r" (intRes) \
 105 : \
 106 "d" (charIn1), \
 107 "d" (intIn2) \
 108 : \
 109 "r26" \
 110 )
 111
 112 // intRes = longIn1 * longIn2 >> 24
 113 // uses:
 114 // r26 to store 0
 115 // r27 to store the byte 1 of the 48bit result
 116 #define MultiU24X24toH16(intRes, longIn1, longIn2) \
 117 asm volatile ( \
 118 "clr r26 \n\t" \
 119 "mul %A1, %B2 \n\t" \
 120 "mov r27, r1 \n\t" \
 121 "mul %B1, %C2 \n\t" \
 122 "movw %A0, r0 \n\t" \
 123 "mul %C1, %C2 \n\t" \
 124 "add %B0, r0 \n\t" \
 125 "mul %C1, %B2 \n\t" \
 126 "add %A0, r0 \n\t" \
 127 "adc %B0, r1 \n\t" \
 128 "mul %A1, %C2 \n\t" \
 129 "add r27, r0 \n\t" \
 130 "adc %A0, r1 \n\t" \
 131 "adc %B0, r26 \n\t" \
 132 "mul %B1, %B2 \n\t" \
 133 "add r27, r0 \n\t" \
 134 "adc %A0, r1 \n\t" \
 135 "adc %B0, r26 \n\t" \
 136 "mul %C1, %A2 \n\t" \
 137 "add r27, r0 \n\t" \
 138 "adc %A0, r1 \n\t" \
 139 "adc %B0, r26 \n\t" \
 140 "mul %B1, %A2 \n\t" \
 141 "add r27, r1 \n\t" \
 142 "adc %A0, r26 \n\t" \
 143 "adc %B0, r26 \n\t" \
 144 "lsr r27 \n\t" \
 145 "adc %A0, r26 \n\t" \
 146 "adc %B0, r26 \n\t" \
 147 "clr r1 \n\t" \
 148 : \
 149 "=&r" (intRes) \
 150 : \
 151 "d" (longIn1), \
 152 "d" (longIn2) \
 153 : \
 154 "r26" , "r27" \
 155 )
 156
 157 // Some useful constants
 158
 159 #define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
 160 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
 161
 162
 163 void checkHitEndstops()
 164 {
 165  if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
 166    SERIAL_ECHO_START;
 167    SERIAL_ECHOPGM(MSG_ENDSTOPS_HIT);
 168    if(endstop_x_hit) {
 169      SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
 170    }
 171    if(endstop_y_hit) {
 172      SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]);
 173    }
 174    if(endstop_z_hit) {
 175      SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
 176    }
 177    SERIAL_ECHOLN("");
 178    endstop_x_hit=false;
 179    endstop_y_hit=false;
 180    endstop_z_hit=false;
 181  }
 182 }
 183
 184 void endstops_hit_on_purpose()
 185 {
 186   endstop_x_hit=false;
 187   endstop_y_hit=false;
 188   endstop_z_hit=false;
 189 }
 190
 191 void enable_endstops(bool check)
 192 {
 193   check_endstops = check;
 194 }
 195
 196 //         __________________________
 197 //        /|                        |\     _________________         ^
 198 //       / |                        | \   /|               |\        |
 199 //      /  |                        |  \ / |               | \       s
 200 //     /   |                        |   |  |               |  \      p
 201 //    /    |                        |   |  |               |   \     e
 202 //   +-----+------------------------+---+--+---------------+----+    e
 203 //   |               BLOCK 1            |      BLOCK 2          |    d
 204 //
 205 //                           time ----->
 206 //
 207 //  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
 208 //  first block->accelerate_until step_events_completed, then keeps going at constant speed until
 209 //  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
 210 //  The slope of acceleration is calculated with the leib ramp alghorithm.
 211
 212 void st_wake_up() {
 213   //  TCNT1 = 0;
 214   ENABLE_STEPPER_DRIVER_INTERRUPT();
 215 }
 216
 217 FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
 218   unsigned short timer;
 219   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
 220
 221   if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
 222     step_rate = (step_rate >> 2)&0x3fff;
 223     step_loops = 4;
 224   }
 225   else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
 226     step_rate = (step_rate >> 1)&0x7fff;
 227     step_loops = 2;
 228   }
 229   else {
 230     step_loops = 1;
 231   }
 232
 233   if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
 234   step_rate -= (F_CPU/500000); // Correct for minimal speed
 235   if(step_rate >= (8*256)){ // higher step rate
 236     unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
 237     unsigned char tmp_step_rate = (step_rate & 0x00ff);
 238     unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
 239     MultiU16X8toH16(timer, tmp_step_rate, gain);
 240     timer = (unsigned short)pgm_read_word_near(table_address) - timer;
 241   }
 242   else { // lower step rates
 243     unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
 244     table_address += ((step_rate)>>1) & 0xfffc;
 245     timer = (unsigned short)pgm_read_word_near(table_address);
 246     timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
 247   }
 248   if(timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
 249   return timer;
 250 }
 251
 252 // Initializes the trapezoid generator from the current block. Called whenever a new
 253 // block begins.
 254 FORCE_INLINE void trapezoid_generator_reset() {
 255   #ifdef ADVANCE
 256     advance = current_block->initial_advance;
 257     final_advance = current_block->final_advance;
 258     // Do E steps + advance steps
 259     e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
 260     old_advance = advance >>8;
 261   #endif
 262   deceleration_time = 0;
 263   // step_rate to timer interval
 264   OCR1A_nominal = calc_timer(current_block->nominal_rate);
 265   acc_step_rate = current_block->initial_rate;
 266   acceleration_time = calc_timer(acc_step_rate);
 267   OCR1A = acceleration_time;
 268
 269 //    SERIAL_ECHO_START;
 270 //    SERIAL_ECHOPGM("advance :");
 271 //    SERIAL_ECHO(current_block->advance/256.0);
 272 //    SERIAL_ECHOPGM("advance rate :");
 273 //    SERIAL_ECHO(current_block->advance_rate/256.0);
 274 //    SERIAL_ECHOPGM("initial advance :");
 275 //  SERIAL_ECHO(current_block->initial_advance/256.0);
 276 //    SERIAL_ECHOPGM("final advance :");
 277 //    SERIAL_ECHOLN(current_block->final_advance/256.0);
 278
 279 }
 280
 281 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
 282 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
 283 ISR(TIMER1_COMPA_vect)
 284 {
 285   // If there is no current block, attempt to pop one from the buffer
 286   if (current_block == NULL) {
 287     // Anything in the buffer?
 288     current_block = plan_get_current_block();
 289     if (current_block != NULL) {
 290       current_block->busy = true;
 291       trapezoid_generator_reset();
 292       counter_x = -(current_block->step_event_count >> 1);
 293       counter_y = counter_x;
 294       counter_z = counter_x;
 295       counter_e = counter_x;
 296       step_events_completed = 0;
 297
 298       #ifdef Z_LATE_ENABLE
 299         if(current_block->steps_z > 0) {
 300           enable_z();
 301           OCR1A = 2000; //1ms wait
 302           return;
 303         }
 304       #endif
 305
 306 //      #ifdef ADVANCE
 307 //      e_steps[current_block->active_extruder] = 0;
 308 //      #endif
 309     }
 310     else {
 311         OCR1A=2000; // 1kHz.
 312     }
 313   }
 314
 315   if (current_block != NULL) {
 316     // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
 317     out_bits = current_block->direction_bits;
 318
 319     // Set direction en check limit switches
 320     if ((out_bits & (1<<X_AXIS)) != 0) {   // -direction
 321       WRITE(X_DIR_PIN, INVERT_X_DIR);
 322       count_direction[X_AXIS]=-1;
 323       CHECK_ENDSTOPS
 324       {
 325         #if X_MIN_PIN > -1
 326           bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOPS_INVERTING);
 327           if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
 328             endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
 329             endstop_x_hit=true;
 330             step_events_completed = current_block->step_event_count;
 331           }
 332           old_x_min_endstop = x_min_endstop;
 333         #endif
 334       }
 335     }
 336     else { // +direction
 337       WRITE(X_DIR_PIN,!INVERT_X_DIR);
 338       count_direction[X_AXIS]=1;
 339       CHECK_ENDSTOPS
 340       {
 341         #if X_MAX_PIN > -1
 342           bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOPS_INVERTING);
 343           if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
 344             endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
 345             endstop_x_hit=true;
 346             step_events_completed = current_block->step_event_count;
 347           }
 348           old_x_max_endstop = x_max_endstop;
 349         #endif
 350       }
 351     }
 352
 353     if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction
 354       WRITE(Y_DIR_PIN,INVERT_Y_DIR);
 355       count_direction[Y_AXIS]=-1;
 356       CHECK_ENDSTOPS
 357       {
 358         #if Y_MIN_PIN > -1
 359           bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOPS_INVERTING);
 360           if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
 361             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
 362             endstop_y_hit=true;
 363             step_events_completed = current_block->step_event_count;
 364           }
 365           old_y_min_endstop = y_min_endstop;
 366         #endif
 367       }
 368     }
 369     else { // +direction
 370     WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
 371       count_direction[Y_AXIS]=1;
 372       CHECK_ENDSTOPS
 373       {
 374         #if Y_MAX_PIN > -1
 375           bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOPS_INVERTING);
 376           if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
 377             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
 378             endstop_y_hit=true;
 379             step_events_completed = current_block->step_event_count;
 380           }
 381           old_y_max_endstop = y_max_endstop;
 382         #endif
 383       }
 384     }
 385
 386     if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
 387       WRITE(Z_DIR_PIN,INVERT_Z_DIR);
 388       count_direction[Z_AXIS]=-1;
 389       CHECK_ENDSTOPS
 390       {
 391         #if Z_MIN_PIN > -1
 392           bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOPS_INVERTING);
 393           if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
 394             endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
 395             endstop_z_hit=true;
 396             step_events_completed = current_block->step_event_count;
 397           }
 398           old_z_min_endstop = z_min_endstop;
 399         #endif
 400       }
 401     }
 402     else { // +direction
 403       WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
 404       count_direction[Z_AXIS]=1;
 405       CHECK_ENDSTOPS
 406       {
 407         #if Z_MAX_PIN > -1
 408           bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOPS_INVERTING);
 409           if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
 410             endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
 411             endstop_z_hit=true;
 412             step_events_completed = current_block->step_event_count;
 413           }
 414           old_z_max_endstop = z_max_endstop;
 415         #endif
 416       }
 417     }
 418
 419     #ifndef ADVANCE
 420       if ((out_bits & (1<<E_AXIS)) != 0) {  // -direction
 421         REV_E_DIR();
 422         count_direction[E_AXIS]=-1;
 423       }
 424       else { // +direction
 425         NORM_E_DIR();
 426         count_direction[E_AXIS]=1;
 427       }
 428     #endif //!ADVANCE
 429
 430
 431
 432     for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
 433       #if MOTHERBOARD != 8 // !teensylu
 434       MSerial.checkRx(); // Check for serial chars.
 435       #endif
 436
 437       #ifdef ADVANCE
 438       counter_e += current_block->steps_e;
 439       if (counter_e > 0) {
 440         counter_e -= current_block->step_event_count;
 441         if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
 442           e_steps[current_block->active_extruder]--;
 443         }
 444         else {
 445           e_steps[current_block->active_extruder]++;
 446         }
 447       }
 448       #endif //ADVANCE
 449
 450       counter_x += current_block->steps_x;
 451       if (counter_x > 0) {
 452         WRITE(X_STEP_PIN, HIGH);
 453         counter_x -= current_block->step_event_count;
 454         WRITE(X_STEP_PIN, LOW);
 455         count_position[X_AXIS]+=count_direction[X_AXIS];
 456       }
 457
 458       counter_y += current_block->steps_y;
 459       if (counter_y > 0) {
 460         WRITE(Y_STEP_PIN, HIGH);
 461         counter_y -= current_block->step_event_count;
 462         WRITE(Y_STEP_PIN, LOW);
 463         count_position[Y_AXIS]+=count_direction[Y_AXIS];
 464       }
 465
 466       counter_z += current_block->steps_z;
 467       if (counter_z > 0) {
 468         WRITE(Z_STEP_PIN, HIGH);
 469         counter_z -= current_block->step_event_count;
 470         WRITE(Z_STEP_PIN, LOW);
 471         count_position[Z_AXIS]+=count_direction[Z_AXIS];
 472       }
 473
 474       #ifndef ADVANCE
 475         counter_e += current_block->steps_e;
 476         if (counter_e > 0) {
 477           WRITE_E_STEP(HIGH);
 478           counter_e -= current_block->step_event_count;
 479           WRITE_E_STEP(LOW);
 480           count_position[E_AXIS]+=count_direction[E_AXIS];
 481         }
 482       #endif //!ADVANCE
 483       step_events_completed += 1;
 484       if(step_events_completed >= current_block->step_event_count) break;
 485     }
 486     // Calculare new timer value
 487     unsigned short timer;
 488     unsigned short step_rate;
 489     if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
 490
 491       MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
 492       acc_step_rate += current_block->initial_rate;
 493
 494       // upper limit
 495       if(acc_step_rate > current_block->nominal_rate)
 496         acc_step_rate = current_block->nominal_rate;
 497
 498       // step_rate to timer interval
 499       timer = calc_timer(acc_step_rate);
 500       OCR1A = timer;
 501       acceleration_time += timer;
 502       #ifdef ADVANCE
 503         for(int8_t i=0; i < step_loops; i++) {
 504           advance += advance_rate;
 505         }
 506         //if(advance > current_block->advance) advance = current_block->advance;
 507         // Do E steps + advance steps
 508         e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
 509         old_advance = advance >>8;
 510
 511       #endif
 512     }
 513     else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
 514       MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
 515
 516       if(step_rate > acc_step_rate) { // Check step_rate stays positive
 517         step_rate = current_block->final_rate;
 518       }
 519       else {
 520         step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
 521       }
 522
 523       // lower limit
 524       if(step_rate < current_block->final_rate)
 525         step_rate = current_block->final_rate;
 526
 527       // step_rate to timer interval
 528       timer = calc_timer(step_rate);
 529       OCR1A = timer;
 530       deceleration_time += timer;
 531       #ifdef ADVANCE
 532         for(int8_t i=0; i < step_loops; i++) {
 533           advance -= advance_rate;
 534         }
 535         if(advance < final_advance) advance = final_advance;
 536         // Do E steps + advance steps
 537         e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
 538         old_advance = advance >>8;
 539       #endif //ADVANCE
 540     }
 541     else {
 542       OCR1A = OCR1A_nominal;
 543     }
 544
 545     // If current block is finished, reset pointer
 546     if (step_events_completed >= current_block->step_event_count) {
 547       current_block = NULL;
 548       plan_discard_current_block();
 549     }
 550   }
 551 }
 552
 553 #ifdef ADVANCE
 554   unsigned char old_OCR0A;
 555   // Timer interrupt for E. e_steps is set in the main routine;
 556   // Timer 0 is shared with millies
 557   ISR(TIMER0_COMPA_vect)
 558   {
 559     old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
 560     OCR0A = old_OCR0A;
 561     // Set E direction (Depends on E direction + advance)
 562     for(unsigned char i=0; i<4;i++) {
 563       if (e_steps[0] != 0) {
 564         WRITE(E0_STEP_PIN, LOW);
 565         if (e_steps[0] < 0) {
 566           WRITE(E0_DIR_PIN, INVERT_E0_DIR);
 567           e_steps[0]++;
 568           WRITE(E0_STEP_PIN, HIGH);
 569         }
 570         else if (e_steps[0] > 0) {
 571           WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
 572           e_steps[0]--;
 573           WRITE(E0_STEP_PIN, HIGH);
 574         }
 575       }
 576  #if EXTRUDERS > 1
 577       if (e_steps[1] != 0) {
 578         WRITE(E1_STEP_PIN, LOW);
 579         if (e_steps[1] < 0) {
 580           WRITE(E1_DIR_PIN, INVERT_E1_DIR);
 581           e_steps[1]++;
 582           WRITE(E1_STEP_PIN, HIGH);
 583         }
 584         else if (e_steps[1] > 0) {
 585           WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
 586           e_steps[1]--;
 587           WRITE(E1_STEP_PIN, HIGH);
 588         }
 589       }
 590  #endif
 591  #if EXTRUDERS > 2
 592       if (e_steps[2] != 0) {
 593         WRITE(E2_STEP_PIN, LOW);
 594         if (e_steps[2] < 0) {
 595           WRITE(E2_DIR_PIN, INVERT_E2_DIR);
 596           e_steps[2]++;
 597           WRITE(E2_STEP_PIN, HIGH);
 598         }
 599         else if (e_steps[2] > 0) {
 600           WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
 601           e_steps[2]--;
 602           WRITE(E2_STEP_PIN, HIGH);
 603         }
 604       }
 605  #endif
 606     }
 607   }
 608 #endif // ADVANCE
 609
 610 void st_init()
 611 {
 612   //Initialize Dir Pins
 613   #if X_DIR_PIN > -1
 614     SET_OUTPUT(X_DIR_PIN);
 615   #endif
 616   #if Y_DIR_PIN > -1
 617     SET_OUTPUT(Y_DIR_PIN);
 618   #endif
 619   #if Z_DIR_PIN > -1
 620     SET_OUTPUT(Z_DIR_PIN);
 621   #endif
 622   #if E0_DIR_PIN > -1
 623     SET_OUTPUT(E0_DIR_PIN);
 624   #endif
 625   #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
 626     SET_OUTPUT(E1_DIR_PIN);
 627   #endif
 628   #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
 629     SET_OUTPUT(E2_DIR_PIN);
 630   #endif
 631
 632   //Initialize Enable Pins - steppers default to disabled.
 633
 634   #if (X_ENABLE_PIN > -1)
 635     SET_OUTPUT(X_ENABLE_PIN);
 636     if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
 637   #endif
 638   #if (Y_ENABLE_PIN > -1)
 639     SET_OUTPUT(Y_ENABLE_PIN);
 640     if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
 641   #endif
 642   #if (Z_ENABLE_PIN > -1)
 643     SET_OUTPUT(Z_ENABLE_PIN);
 644     if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
 645   #endif
 646   #if (E0_ENABLE_PIN > -1)
 647     SET_OUTPUT(E0_ENABLE_PIN);
 648     if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
 649   #endif
 650   #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
 651     SET_OUTPUT(E1_ENABLE_PIN);
 652     if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
 653   #endif
 654   #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
 655     SET_OUTPUT(E2_ENABLE_PIN);
 656     if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
 657   #endif
 658
 659   //endstops and pullups
 660
 661   #if X_MIN_PIN > -1
 662     SET_INPUT(X_MIN_PIN);
 663     #ifdef ENDSTOPPULLUP_XMIN
 664       WRITE(X_MIN_PIN,HIGH);
 665     #endif
 666   #endif
 667
 668   #if Y_MIN_PIN > -1
 669     SET_INPUT(Y_MIN_PIN);
 670     #ifdef ENDSTOPPULLUP_YMIN
 671       WRITE(Y_MIN_PIN,HIGH);
 672     #endif
 673   #endif
 674
 675   #if Z_MIN_PIN > -1
 676     SET_INPUT(Z_MIN_PIN);
 677     #ifdef ENDSTOPPULLUP_ZMIN
 678       WRITE(Z_MIN_PIN,HIGH);
 679     #endif
 680   #endif
 681
 682   #if X_MAX_PIN > -1
 683     SET_INPUT(X_MAX_PIN);
 684     #ifdef ENDSTOPPULLUP_XMAX
 685       WRITE(X_MAX_PIN,HIGH);
 686     #endif
 687   #endif
 688
 689   #if Y_MAX_PIN > -1
 690     SET_INPUT(Y_MAX_PIN);
 691     #ifdef ENDSTOPPULLUP_YMAX
 692       WRITE(Y_MAX_PIN,HIGH);
 693     #endif
 694   #endif
 695
 696   #if Z_MAX_PIN > -1
 697     SET_INPUT(Z_MAX_PIN);
 698     #ifdef ENDSTOPPULLUP_ZMAX
 699       WRITE(Z_MAX_PIN,HIGH);
 700     #endif
 701   #endif
 702
 703
 704   //Initialize Step Pins
 705   #if (X_STEP_PIN > -1)
 706     SET_OUTPUT(X_STEP_PIN);
 707     if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
 708   #endif
 709   #if (Y_STEP_PIN > -1)
 710     SET_OUTPUT(Y_STEP_PIN);
 711     if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
 712   #endif
 713   #if (Z_STEP_PIN > -1)
 714     SET_OUTPUT(Z_STEP_PIN);
 715     if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
 716   #endif
 717   #if (E0_STEP_PIN > -1)
 718     SET_OUTPUT(E0_STEP_PIN);
 719     if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
 720   #endif
 721   #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
 722     SET_OUTPUT(E1_STEP_PIN);
 723     if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
 724   #endif
 725   #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
 726     SET_OUTPUT(E2_STEP_PIN);
 727     if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
 728   #endif
 729
 730   #ifdef CONTROLLERFAN_PIN
 731     SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
 732   #endif
 733
 734   // waveform generation = 0100 = CTC
 735   TCCR1B &= ~(1<<WGM13);
 736   TCCR1B |=  (1<<WGM12);
 737   TCCR1A &= ~(1<<WGM11);
 738   TCCR1A &= ~(1<<WGM10);
 739
 740   // output mode = 00 (disconnected)
 741   TCCR1A &= ~(3<<COM1A0);
 742   TCCR1A &= ~(3<<COM1B0);
 743
 744   // Set the timer pre-scaler
 745   // Generally we use a divider of 8, resulting in a 2MHz timer
 746   // frequency on a 16MHz MCU. If you are going to change this, be
 747   // sure to regenerate speed_lookuptable.h with
 748   // create_speed_lookuptable.py
 749   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
 750
 751   OCR1A = 0x4000;
 752   TCNT1 = 0;
 753   ENABLE_STEPPER_DRIVER_INTERRUPT();
 754
 755   #ifdef ADVANCE
 756   #if defined(TCCR0A) && defined(WGM01)
 757     TCCR0A &= ~(1<<WGM01);
 758     TCCR0A &= ~(1<<WGM00);
 759   #endif
 760     e_steps[0] = 0;
 761     e_steps[1] = 0;
 762     e_steps[2] = 0;
 763     TIMSK0 |= (1<<OCIE0A);
 764   #endif //ADVANCE
 765
 766   enable_endstops(true); // Start with endstops active. After homing they can be disabled
 767   sei();
 768 }
 769
 770
 771 // Block until all buffered steps are executed
 772 void st_synchronize()
 773 {
 774     while( blocks_queued()) {
 775     manage_heater();
 776     manage_inactivity(1);
 777     LCD_STATUS;
 778   }
 779 }
 780
 781 void st_set_position(const long &x, const long &y, const long &z, const long &e)
 782 {
 783   CRITICAL_SECTION_START;
 784   count_position[X_AXIS] = x;
 785   count_position[Y_AXIS] = y;
 786   count_position[Z_AXIS] = z;
 787   count_position[E_AXIS] = e;
 788   CRITICAL_SECTION_END;
 789 }
 790
 791 void st_set_e_position(const long &e)
 792 {
 793   CRITICAL_SECTION_START;
 794   count_position[E_AXIS] = e;
 795   CRITICAL_SECTION_END;
 796 }
 797
 798 long st_get_position(uint8_t axis)
 799 {
 800   long count_pos;
 801   CRITICAL_SECTION_START;
 802   count_pos = count_position[axis];
 803   CRITICAL_SECTION_END;
 804   return count_pos;
 805 }
 806
 807 void finishAndDisableSteppers()
 808 {
 809   st_synchronize();
 810   LCD_MESSAGEPGM(MSG_STEPPER_RELEASED);
 811   disable_x();
 812   disable_y();
 813   disable_z();
 814   disable_e0();
 815   disable_e1();
 816   disable_e2();
 817 }
 818
 819 void quickStop()
 820 {
 821   DISABLE_STEPPER_DRIVER_INTERRUPT();
 822   while(blocks_queued())
 823     plan_discard_current_block();
 824   current_block = NULL;
 825   ENABLE_STEPPER_DRIVER_INTERRUPT();
 826 }
 827