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 void step_wait(){
 218     for(int8_t i=0; i < 6; i++){
 219     }
 220 }
 221
 222
 223 FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
 224   unsigned short timer;
 225   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
 226
 227   if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
 228     step_rate = (step_rate >> 2)&0x3fff;
 229     step_loops = 4;
 230   }
 231   else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
 232     step_rate = (step_rate >> 1)&0x7fff;
 233     step_loops = 2;
 234   }
 235   else {
 236     step_loops = 1;
 237   }
 238
 239   if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
 240   step_rate -= (F_CPU/500000); // Correct for minimal speed
 241   if(step_rate >= (8*256)){ // higher step rate
 242     unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
 243     unsigned char tmp_step_rate = (step_rate & 0x00ff);
 244     unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
 245     MultiU16X8toH16(timer, tmp_step_rate, gain);
 246     timer = (unsigned short)pgm_read_word_near(table_address) - timer;
 247   }
 248   else { // lower step rates
 249     unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
 250     table_address += ((step_rate)>>1) & 0xfffc;
 251     timer = (unsigned short)pgm_read_word_near(table_address);
 252     timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
 253   }
 254   if(timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
 255   return timer;
 256 }
 257
 258 // Initializes the trapezoid generator from the current block. Called whenever a new
 259 // block begins.
 260 FORCE_INLINE void trapezoid_generator_reset() {
 261   #ifdef ADVANCE
 262     advance = current_block->initial_advance;
 263     final_advance = current_block->final_advance;
 264     // Do E steps + advance steps
 265     e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
 266     old_advance = advance >>8;
 267   #endif
 268   deceleration_time = 0;
 269   // step_rate to timer interval
 270   OCR1A_nominal = calc_timer(current_block->nominal_rate);
 271   acc_step_rate = current_block->initial_rate;
 272   acceleration_time = calc_timer(acc_step_rate);
 273   OCR1A = acceleration_time;
 274
 275 //    SERIAL_ECHO_START;
 276 //    SERIAL_ECHOPGM("advance :");
 277 //    SERIAL_ECHO(current_block->advance/256.0);
 278 //    SERIAL_ECHOPGM("advance rate :");
 279 //    SERIAL_ECHO(current_block->advance_rate/256.0);
 280 //    SERIAL_ECHOPGM("initial advance :");
 281 //  SERIAL_ECHO(current_block->initial_advance/256.0);
 282 //    SERIAL_ECHOPGM("final advance :");
 283 //    SERIAL_ECHOLN(current_block->final_advance/256.0);
 284
 285 }
 286
 287 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
 288 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
 289 ISR(TIMER1_COMPA_vect)
 290 {
 291   // If there is no current block, attempt to pop one from the buffer
 292   if (current_block == NULL) {
 293     // Anything in the buffer?
 294     current_block = plan_get_current_block();
 295     if (current_block != NULL) {
 296       current_block->busy = true;
 297       trapezoid_generator_reset();
 298       counter_x = -(current_block->step_event_count >> 1);
 299       counter_y = counter_x;
 300       counter_z = counter_x;
 301       counter_e = counter_x;
 302       step_events_completed = 0;
 303
 304       #ifdef Z_LATE_ENABLE
 305         if(current_block->steps_z > 0) {
 306           enable_z();
 307           OCR1A = 2000; //1ms wait
 308           return;
 309         }
 310       #endif
 311
 312 //      #ifdef ADVANCE
 313 //      e_steps[current_block->active_extruder] = 0;
 314 //      #endif
 315     }
 316     else {
 317         OCR1A=2000; // 1kHz.
 318     }
 319   }
 320
 321   if (current_block != NULL) {
 322     // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
 323     out_bits = current_block->direction_bits;
 324
 325     // Set direction en check limit switches
 326     if ((out_bits & (1<<X_AXIS)) != 0) {   // stepping along -X axis
 327       #if !defined COREXY  //NOT COREXY
 328         WRITE(X_DIR_PIN, INVERT_X_DIR);
 329       #endif
 330       count_direction[X_AXIS]=-1;
 331       CHECK_ENDSTOPS
 332       {
 333         #if X_MIN_PIN > -1
 334           bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOPS_INVERTING);
 335           if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
 336             endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
 337             endstop_x_hit=true;
 338             step_events_completed = current_block->step_event_count;
 339           }
 340           old_x_min_endstop = x_min_endstop;
 341         #endif
 342       }
 343     }
 344     else { // +direction
 345       #if !defined COREXY  //NOT COREXY
 346         WRITE(X_DIR_PIN,!INVERT_X_DIR);
 347       #endif
 348
 349       count_direction[X_AXIS]=1;
 350       CHECK_ENDSTOPS
 351       {
 352         #if X_MAX_PIN > -1
 353           bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOPS_INVERTING);
 354           if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
 355             endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
 356             endstop_x_hit=true;
 357             step_events_completed = current_block->step_event_count;
 358           }
 359           old_x_max_endstop = x_max_endstop;
 360         #endif
 361       }
 362     }
 363
 364     if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction
 365       #if !defined COREXY  //NOT COREXY
 366         WRITE(Y_DIR_PIN,INVERT_Y_DIR);
 367       #endif
 368       count_direction[Y_AXIS]=-1;
 369       CHECK_ENDSTOPS
 370       {
 371         #if Y_MIN_PIN > -1
 372           bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOPS_INVERTING);
 373           if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
 374             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
 375             endstop_y_hit=true;
 376             step_events_completed = current_block->step_event_count;
 377           }
 378           old_y_min_endstop = y_min_endstop;
 379         #endif
 380       }
 381     }
 382     else { // +direction
 383       #if !defined COREXY  //NOT COREXY
 384         WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
 385       #endif
 386       count_direction[Y_AXIS]=1;
 387       CHECK_ENDSTOPS
 388       {
 389         #if Y_MAX_PIN > -1
 390           bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOPS_INVERTING);
 391           if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
 392             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
 393             endstop_y_hit=true;
 394             step_events_completed = current_block->step_event_count;
 395           }
 396           old_y_max_endstop = y_max_endstop;
 397         #endif
 398       }
 399     }
 400
 401
 402     #ifdef COREXY  //coreXY kinematics defined
 403       if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) == 0)){  //+X is major axis
 404         WRITE(X_DIR_PIN, !INVERT_X_DIR);
 405         WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
 406       }
 407       if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) != 0)){  //-X is major axis
 408         WRITE(X_DIR_PIN, INVERT_X_DIR);
 409         WRITE(Y_DIR_PIN, INVERT_Y_DIR);
 410       }
 411       if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) == 0)){  //+Y is major axis
 412         WRITE(X_DIR_PIN, !INVERT_X_DIR);
 413         WRITE(Y_DIR_PIN, INVERT_Y_DIR);
 414       }
 415       if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) != 0)){  //-Y is major axis
 416         WRITE(X_DIR_PIN, INVERT_X_DIR);
 417         WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
 418       }
 419     #endif //coreXY
 420
 421
 422     if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
 423       WRITE(Z_DIR_PIN,INVERT_Z_DIR);
 424       count_direction[Z_AXIS]=-1;
 425       CHECK_ENDSTOPS
 426       {
 427         #if Z_MIN_PIN > -1
 428           bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOPS_INVERTING);
 429           if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
 430             endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
 431             endstop_z_hit=true;
 432             step_events_completed = current_block->step_event_count;
 433           }
 434           old_z_min_endstop = z_min_endstop;
 435         #endif
 436       }
 437     }
 438     else { // +direction
 439       WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
 440       count_direction[Z_AXIS]=1;
 441       CHECK_ENDSTOPS
 442       {
 443         #if Z_MAX_PIN > -1
 444           bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOPS_INVERTING);
 445           if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
 446             endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
 447             endstop_z_hit=true;
 448             step_events_completed = current_block->step_event_count;
 449           }
 450           old_z_max_endstop = z_max_endstop;
 451         #endif
 452       }
 453     }
 454
 455     #ifndef ADVANCE
 456       if ((out_bits & (1<<E_AXIS)) != 0) {  // -direction
 457         REV_E_DIR();
 458         count_direction[E_AXIS]=-1;
 459       }
 460       else { // +direction
 461         NORM_E_DIR();
 462         count_direction[E_AXIS]=1;
 463       }
 464     #endif //!ADVANCE
 465
 466
 467
 468     for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
 469       #if MOTHERBOARD != 8 // !teensylu
 470       MSerial.checkRx(); // Check for serial chars.
 471       #endif
 472
 473       #ifdef ADVANCE
 474       counter_e += current_block->steps_e;
 475       if (counter_e > 0) {
 476         counter_e -= current_block->step_event_count;
 477         if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
 478           e_steps[current_block->active_extruder]--;
 479         }
 480         else {
 481           e_steps[current_block->active_extruder]++;
 482         }
 483       }
 484       #endif //ADVANCE
 485
 486       #if !defined COREXY
 487         counter_x += current_block->steps_x;
 488         if (counter_x > 0) {
 489           WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
 490           counter_x -= current_block->step_event_count;
 491           count_position[X_AXIS]+=count_direction[X_AXIS];
 492           WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
 493         }
 494
 495         counter_y += current_block->steps_y;
 496         if (counter_y > 0) {
 497           WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
 498           counter_y -= current_block->step_event_count;
 499           count_position[Y_AXIS]+=count_direction[Y_AXIS];
 500           WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
 501         }
 502       #endif
 503
 504       #ifdef COREXY
 505         counter_x += current_block->steps_x;
 506         counter_y += current_block->steps_y;
 507
 508         if ((counter_x > 0)&&!(counter_y>0)){  //X step only
 509           WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
 510           WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
 511           counter_x -= current_block->step_event_count;
 512           count_position[X_AXIS]+=count_direction[X_AXIS];
 513           WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
 514           WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
 515         }
 516
 517         if (!(counter_x > 0)&&(counter_y>0)){  //Y step only
 518           WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
 519           WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
 520           counter_y -= current_block->step_event_count;
 521           count_position[Y_AXIS]+=count_direction[Y_AXIS];
 522           WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
 523           WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
 524         }
 525
 526         if ((counter_x > 0)&&(counter_y>0)){  //step in both axes
 527           if (((out_bits & (1<<X_AXIS)) == 0)^((out_bits & (1<<Y_AXIS)) == 0)){  //X and Y in different directions
 528             WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
 529             counter_x -= current_block->step_event_count;
 530             WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
 531             step_wait();
 532             count_position[X_AXIS]+=count_direction[X_AXIS];
 533             count_position[Y_AXIS]+=count_direction[Y_AXIS];
 534             WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
 535             counter_y -= current_block->step_event_count;
 536             WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
 537           }
 538           else{  //X and Y in same direction
 539             WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
 540             counter_x -= current_block->step_event_count;
 541             WRITE(X_STEP_PIN, INVERT_X_STEP_PIN) ;
 542             step_wait();
 543             count_position[X_AXIS]+=count_direction[X_AXIS];
 544             count_position[Y_AXIS]+=count_direction[Y_AXIS];
 545             WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
 546             counter_y -= current_block->step_event_count;
 547             WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
 548           }
 549         }
 550       #endif //corexy
 551
 552       counter_z += current_block->steps_z;
 553       if (counter_z > 0) {
 554         WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
 555         counter_z -= current_block->step_event_count;
 556         count_position[Z_AXIS]+=count_direction[Z_AXIS];
 557         WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
 558       }
 559
 560       #ifndef ADVANCE
 561         counter_e += current_block->steps_e;
 562         if (counter_e > 0) {
 563           WRITE_E_STEP(!INVERT_E_STEP_PIN);
 564           counter_e -= current_block->step_event_count;
 565           count_position[E_AXIS]+=count_direction[E_AXIS];
 566           WRITE_E_STEP(INVERT_E_STEP_PIN);
 567         }
 568       #endif //!ADVANCE
 569       step_events_completed += 1;
 570       if(step_events_completed >= current_block->step_event_count) break;
 571     }
 572     // Calculare new timer value
 573     unsigned short timer;
 574     unsigned short step_rate;
 575     if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
 576
 577       MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
 578       acc_step_rate += current_block->initial_rate;
 579
 580       // upper limit
 581       if(acc_step_rate > current_block->nominal_rate)
 582         acc_step_rate = current_block->nominal_rate;
 583
 584       // step_rate to timer interval
 585       timer = calc_timer(acc_step_rate);
 586       OCR1A = timer;
 587       acceleration_time += timer;
 588       #ifdef ADVANCE
 589         for(int8_t i=0; i < step_loops; i++) {
 590           advance += advance_rate;
 591         }
 592         //if(advance > current_block->advance) advance = current_block->advance;
 593         // Do E steps + advance steps
 594         e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
 595         old_advance = advance >>8;
 596
 597       #endif
 598     }
 599     else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
 600       MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
 601
 602       if(step_rate > acc_step_rate) { // Check step_rate stays positive
 603         step_rate = current_block->final_rate;
 604       }
 605       else {
 606         step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
 607       }
 608
 609       // lower limit
 610       if(step_rate < current_block->final_rate)
 611         step_rate = current_block->final_rate;
 612
 613       // step_rate to timer interval
 614       timer = calc_timer(step_rate);
 615       OCR1A = timer;
 616       deceleration_time += timer;
 617       #ifdef ADVANCE
 618         for(int8_t i=0; i < step_loops; i++) {
 619           advance -= advance_rate;
 620         }
 621         if(advance < final_advance) advance = final_advance;
 622         // Do E steps + advance steps
 623         e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
 624         old_advance = advance >>8;
 625       #endif //ADVANCE
 626     }
 627     else {
 628       OCR1A = OCR1A_nominal;
 629     }
 630
 631     // If current block is finished, reset pointer
 632     if (step_events_completed >= current_block->step_event_count) {
 633       current_block = NULL;
 634       plan_discard_current_block();
 635     }
 636   }
 637 }
 638
 639 #ifdef ADVANCE
 640   unsigned char old_OCR0A;
 641   // Timer interrupt for E. e_steps is set in the main routine;
 642   // Timer 0 is shared with millies
 643   ISR(TIMER0_COMPA_vect)
 644   {
 645     old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
 646     OCR0A = old_OCR0A;
 647     // Set E direction (Depends on E direction + advance)
 648     for(unsigned char i=0; i<4;i++) {
 649       if (e_steps[0] != 0) {
 650         WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 651         if (e_steps[0] < 0) {
 652           WRITE(E0_DIR_PIN, INVERT_E0_DIR);
 653           e_steps[0]++;
 654           WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
 655         }
 656         else if (e_steps[0] > 0) {
 657           WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
 658           e_steps[0]--;
 659           WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
 660         }
 661       }
 662  #if EXTRUDERS > 1
 663       if (e_steps[1] != 0) {
 664         WRITE(E1_STEP_PIN, INVERT_E_STEP_PIN);
 665         if (e_steps[1] < 0) {
 666           WRITE(E1_DIR_PIN, INVERT_E1_DIR);
 667           e_steps[1]++;
 668           WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
 669         }
 670         else if (e_steps[1] > 0) {
 671           WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
 672           e_steps[1]--;
 673           WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
 674         }
 675       }
 676  #endif
 677  #if EXTRUDERS > 2
 678       if (e_steps[2] != 0) {
 679         WRITE(E2_STEP_PIN, INVERT_E_STEP_PIN);
 680         if (e_steps[2] < 0) {
 681           WRITE(E2_DIR_PIN, INVERT_E2_DIR);
 682           e_steps[2]++;
 683           WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
 684         }
 685         else if (e_steps[2] > 0) {
 686           WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
 687           e_steps[2]--;
 688           WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
 689         }
 690       }
 691  #endif
 692     }
 693   }
 694 #endif // ADVANCE
 695
 696 void st_init()
 697 {
 698   //Initialize Dir Pins
 699   #if X_DIR_PIN > -1
 700     SET_OUTPUT(X_DIR_PIN);
 701   #endif
 702   #if Y_DIR_PIN > -1
 703     SET_OUTPUT(Y_DIR_PIN);
 704   #endif
 705   #if Z_DIR_PIN > -1
 706     SET_OUTPUT(Z_DIR_PIN);
 707   #endif
 708   #if E0_DIR_PIN > -1
 709     SET_OUTPUT(E0_DIR_PIN);
 710   #endif
 711   #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
 712     SET_OUTPUT(E1_DIR_PIN);
 713   #endif
 714   #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
 715     SET_OUTPUT(E2_DIR_PIN);
 716   #endif
 717
 718   //Initialize Enable Pins - steppers default to disabled.
 719
 720   #if (X_ENABLE_PIN > -1)
 721     SET_OUTPUT(X_ENABLE_PIN);
 722     if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
 723   #endif
 724   #if (Y_ENABLE_PIN > -1)
 725     SET_OUTPUT(Y_ENABLE_PIN);
 726     if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
 727   #endif
 728   #if (Z_ENABLE_PIN > -1)
 729     SET_OUTPUT(Z_ENABLE_PIN);
 730     if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
 731   #endif
 732   #if (E0_ENABLE_PIN > -1)
 733     SET_OUTPUT(E0_ENABLE_PIN);
 734     if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
 735   #endif
 736   #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
 737     SET_OUTPUT(E1_ENABLE_PIN);
 738     if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
 739   #endif
 740   #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
 741     SET_OUTPUT(E2_ENABLE_PIN);
 742     if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
 743   #endif
 744
 745   //endstops and pullups
 746
 747   #if X_MIN_PIN > -1
 748     SET_INPUT(X_MIN_PIN);
 749     #ifdef ENDSTOPPULLUP_XMIN
 750       WRITE(X_MIN_PIN,HIGH);
 751     #endif
 752   #endif
 753
 754   #if Y_MIN_PIN > -1
 755     SET_INPUT(Y_MIN_PIN);
 756     #ifdef ENDSTOPPULLUP_YMIN
 757       WRITE(Y_MIN_PIN,HIGH);
 758     #endif
 759   #endif
 760
 761   #if Z_MIN_PIN > -1
 762     SET_INPUT(Z_MIN_PIN);
 763     #ifdef ENDSTOPPULLUP_ZMIN
 764       WRITE(Z_MIN_PIN,HIGH);
 765     #endif
 766   #endif
 767
 768   #if X_MAX_PIN > -1
 769     SET_INPUT(X_MAX_PIN);
 770     #ifdef ENDSTOPPULLUP_XMAX
 771       WRITE(X_MAX_PIN,HIGH);
 772     #endif
 773   #endif
 774
 775   #if Y_MAX_PIN > -1
 776     SET_INPUT(Y_MAX_PIN);
 777     #ifdef ENDSTOPPULLUP_YMAX
 778       WRITE(Y_MAX_PIN,HIGH);
 779     #endif
 780   #endif
 781
 782   #if Z_MAX_PIN > -1
 783     SET_INPUT(Z_MAX_PIN);
 784     #ifdef ENDSTOPPULLUP_ZMAX
 785       WRITE(Z_MAX_PIN,HIGH);
 786     #endif
 787   #endif
 788
 789
 790   //Initialize Step Pins
 791   #if (X_STEP_PIN > -1)
 792     SET_OUTPUT(X_STEP_PIN);
 793     WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
 794     if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
 795   #endif
 796   #if (Y_STEP_PIN > -1)
 797     SET_OUTPUT(Y_STEP_PIN);
 798     WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
 799     if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
 800   #endif
 801   #if (Z_STEP_PIN > -1)
 802     SET_OUTPUT(Z_STEP_PIN);
 803     WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
 804     if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
 805   #endif
 806   #if (E0_STEP_PIN > -1)
 807     SET_OUTPUT(E0_STEP_PIN);
 808     WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
 809     if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
 810   #endif
 811   #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
 812     SET_OUTPUT(E1_STEP_PIN);
 813     WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
 814     if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
 815   #endif
 816   #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
 817     SET_OUTPUT(E2_STEP_PIN);
 818     WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
 819     if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
 820   #endif
 821
 822   #ifdef CONTROLLERFAN_PIN
 823     SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
 824   #endif
 825
 826   // waveform generation = 0100 = CTC
 827   TCCR1B &= ~(1<<WGM13);
 828   TCCR1B |=  (1<<WGM12);
 829   TCCR1A &= ~(1<<WGM11);
 830   TCCR1A &= ~(1<<WGM10);
 831
 832   // output mode = 00 (disconnected)
 833   TCCR1A &= ~(3<<COM1A0);
 834   TCCR1A &= ~(3<<COM1B0);
 835
 836   // Set the timer pre-scaler
 837   // Generally we use a divider of 8, resulting in a 2MHz timer
 838   // frequency on a 16MHz MCU. If you are going to change this, be
 839   // sure to regenerate speed_lookuptable.h with
 840   // create_speed_lookuptable.py
 841   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
 842
 843   OCR1A = 0x4000;
 844   TCNT1 = 0;
 845   ENABLE_STEPPER_DRIVER_INTERRUPT();
 846
 847   #ifdef ADVANCE
 848   #if defined(TCCR0A) && defined(WGM01)
 849     TCCR0A &= ~(1<<WGM01);
 850     TCCR0A &= ~(1<<WGM00);
 851   #endif
 852     e_steps[0] = 0;
 853     e_steps[1] = 0;
 854     e_steps[2] = 0;
 855     TIMSK0 |= (1<<OCIE0A);
 856   #endif //ADVANCE
 857
 858   enable_endstops(true); // Start with endstops active. After homing they can be disabled
 859   sei();
 860 }
 861
 862
 863 // Block until all buffered steps are executed
 864 void st_synchronize()
 865 {
 866     while( blocks_queued()) {
 867     manage_heater();
 868     manage_inactivity(1);
 869     LCD_STATUS;
 870   }
 871 }
 872
 873 void st_set_position(const long &x, const long &y, const long &z, const long &e)
 874 {
 875   CRITICAL_SECTION_START;
 876   count_position[X_AXIS] = x;
 877   count_position[Y_AXIS] = y;
 878   count_position[Z_AXIS] = z;
 879   count_position[E_AXIS] = e;
 880   CRITICAL_SECTION_END;
 881 }
 882
 883 void st_set_e_position(const long &e)
 884 {
 885   CRITICAL_SECTION_START;
 886   count_position[E_AXIS] = e;
 887   CRITICAL_SECTION_END;
 888 }
 889
 890 long st_get_position(uint8_t axis)
 891 {
 892   long count_pos;
 893   CRITICAL_SECTION_START;
 894   count_pos = count_position[axis];
 895   CRITICAL_SECTION_END;
 896   return count_pos;
 897 }
 898
 899 void finishAndDisableSteppers()
 900 {
 901   st_synchronize();
 902   LCD_MESSAGEPGM(MSG_STEPPER_RELEASED);
 903   disable_x();
 904   disable_y();
 905   disable_z();
 906   disable_e0();
 907   disable_e1();
 908   disable_e2();
 909 }
 910
 911 void quickStop()
 912 {
 913   DISABLE_STEPPER_DRIVER_INTERRUPT();
 914   while(blocks_queued())
 915     plan_discard_current_block();
 916   current_block = NULL;
 917   ENABLE_STEPPER_DRIVER_INTERRUPT();
 918 }
 919