Mercurial > hg-public > Marlin / file comparison

comparison: stepper.cpp

stepper.cpp

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

Mercurial Repository: Marlin

mercurial