forked from prusa3d/Prusa-Firmware
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathstepper.cpp
1603 lines (1446 loc) · 49 KB
/
stepper.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
stepper.c - stepper motor driver: executes motion plans using stepper motors
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl 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.
Grbl 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 Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. */
#include "Marlin.h"
#include "stepper.h"
#include "planner.h"
#include "temperature.h"
#include "ultralcd.h"
#include "language.h"
#include "cardreader.h"
#include "speed_lookuptable.h"
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
#include <SPI.h>
#endif
#ifdef TMC2130
#include "tmc2130.h"
#endif //TMC2130
#if defined(FILAMENT_SENSOR) && defined(PAT9125)
#include "fsensor.h"
int fsensor_counter; //counter for e-steps
#endif //FILAMENT_SENSOR
#include "mmu.h"
#include "ConfigurationStore.h"
#ifdef DEBUG_STACK_MONITOR
uint16_t SP_min = 0x21FF;
#endif //DEBUG_STACK_MONITOR
//===========================================================================
//=============================public variables ============================
//===========================================================================
block_t *current_block; // A pointer to the block currently being traced
bool x_min_endstop = false;
bool x_max_endstop = false;
bool y_min_endstop = false;
bool y_max_endstop = false;
bool z_min_endstop = false;
bool z_max_endstop = false;
//===========================================================================
//=============================private variables ============================
//===========================================================================
//static makes it inpossible to be called from outside of this file by extern.!
// Variables used by The Stepper Driver Interrupt
static unsigned char out_bits; // The next stepping-bits to be output
static dda_isteps_t
counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z,
counter_e;
volatile dda_usteps_t step_events_completed; // The number of step events executed in the current block
static int32_t acceleration_time, deceleration_time;
//static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
static uint16_t acc_step_rate; // needed for deccelaration start point
static uint8_t step_loops;
static uint16_t OCR1A_nominal;
static uint8_t step_loops_nominal;
volatile long endstops_trigsteps[3]={0,0,0};
volatile long endstops_stepsTotal,endstops_stepsDone;
static volatile bool endstop_x_hit=false;
static volatile bool endstop_y_hit=false;
static volatile bool endstop_z_hit=false;
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
bool abort_on_endstop_hit = false;
#endif
#ifdef MOTOR_CURRENT_PWM_XY_PIN
int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
int motor_current_setting_silent[3] = DEFAULT_PWM_MOTOR_CURRENT;
int motor_current_setting_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
#endif
#if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT)
static bool old_x_max_endstop=false;
#endif
#if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT)
static bool old_y_max_endstop=false;
#endif
static bool old_x_min_endstop=false;
static bool old_y_min_endstop=false;
static bool old_z_min_endstop=false;
static bool old_z_max_endstop=false;
static bool check_endstops = true;
static bool check_z_endstop = false;
static bool z_endstop_invert = false;
volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
#ifdef LIN_ADVANCE
void advance_isr_scheduler();
void advance_isr();
static const uint16_t ADV_NEVER = 0xFFFF;
static const uint8_t ADV_INIT = 0b01;
static const uint8_t ADV_DECELERATE = 0b10;
static uint16_t nextMainISR;
static uint16_t nextAdvanceISR;
static uint16_t main_Rate;
static uint16_t eISR_Rate;
static uint16_t eISR_Err;
static uint16_t current_adv_steps;
static uint16_t final_adv_steps;
static uint16_t max_adv_steps;
static uint32_t LA_decelerate_after;
static int8_t e_steps;
static uint8_t e_step_loops;
static int8_t LA_phase;
#define _NEXT_ISR(T) main_Rate = nextMainISR = T
#else
#define _NEXT_ISR(T) OCR1A = T
#endif
#ifdef DEBUG_STEPPER_TIMER_MISSED
extern bool stepper_timer_overflow_state;
extern uint16_t stepper_timer_overflow_last;
#endif /* DEBUG_STEPPER_TIMER_MISSED */
//===========================================================================
//=============================functions ============================
//===========================================================================
void checkHitEndstops()
{
if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
SERIAL_ECHO_START;
SERIAL_ECHORPGM(MSG_ENDSTOPS_HIT);
if(endstop_x_hit) {
SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/cs.axis_steps_per_unit[X_AXIS]);
// LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT), PSTR("X")));
}
if(endstop_y_hit) {
SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/cs.axis_steps_per_unit[Y_AXIS]);
// LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT), PSTR("Y")));
}
if(endstop_z_hit) {
SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/cs.axis_steps_per_unit[Z_AXIS]);
// LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT),PSTR("Z")));
}
SERIAL_ECHOLN("");
endstop_x_hit=false;
endstop_y_hit=false;
endstop_z_hit=false;
#if defined(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && defined(SDSUPPORT)
if (abort_on_endstop_hit)
{
card.sdprinting = false;
card.closefile();
quickStop();
setTargetHotend0(0);
setTargetHotend1(0);
setTargetHotend2(0);
}
#endif
}
}
bool endstops_hit_on_purpose()
{
bool hit = endstop_x_hit || endstop_y_hit || endstop_z_hit;
endstop_x_hit=false;
endstop_y_hit=false;
endstop_z_hit=false;
return hit;
}
bool endstop_z_hit_on_purpose()
{
bool hit = endstop_z_hit;
endstop_z_hit=false;
return hit;
}
bool enable_endstops(bool check)
{
bool old = check_endstops;
check_endstops = check;
return old;
}
bool enable_z_endstop(bool check)
{
bool old = check_z_endstop;
check_z_endstop = check;
endstop_z_hit = false;
return old;
}
void invert_z_endstop(bool endstop_invert)
{
z_endstop_invert = endstop_invert;
}
// __________________________
// /| |\ _________________ ^
// / | | \ /| |\ |
// / | | \ / | | \ s
// / | | | | | \ p
// / | | | | | \ e
// +-----+------------------------+---+--+---------------+----+ e
// | BLOCK 1 | BLOCK 2 | d
//
// time ----->
//
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
// first block->accelerate_until step_events_completed, then keeps going at constant speed until
// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
// The slope of acceleration is calculated with the leib ramp alghorithm.
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
ISR(TIMER1_COMPA_vect) {
#ifdef DEBUG_STACK_MONITOR
uint16_t sp = SPL + 256 * SPH;
if (sp < SP_min) SP_min = sp;
#endif //DEBUG_STACK_MONITOR
#ifdef LIN_ADVANCE
advance_isr_scheduler();
#else
isr();
#endif
// Don't run the ISR faster than possible
// Is there a 8us time left before the next interrupt triggers?
if (OCR1A < TCNT1 + 16) {
#ifdef DEBUG_STEPPER_TIMER_MISSED
// Verify whether the next planned timer interrupt has not been missed already.
// This debugging test takes < 1.125us
// This skews the profiling slightly as the fastest stepper timer
// interrupt repeats at a 100us rate (10kHz).
if (OCR1A + 40 < TCNT1) {
// The interrupt was delayed by more than 20us (which is 1/5th of the 10kHz ISR repeat rate).
// Give a warning.
stepper_timer_overflow_state = true;
stepper_timer_overflow_last = TCNT1 - OCR1A;
// Beep, the beeper will be cleared at the stepper_timer_overflow() called from the main thread.
WRITE(BEEPER, HIGH);
}
#endif
// Fix the next interrupt to be executed after 8us from now.
OCR1A = TCNT1 + 16;
}
}
uint8_t last_dir_bits = 0;
#ifdef BACKLASH_X
uint8_t st_backlash_x = 0;
#endif //BACKLASH_X
#ifdef BACKLASH_Y
uint8_t st_backlash_y = 0;
#endif //BACKLASH_Y
FORCE_INLINE void stepper_next_block()
{
// Anything in the buffer?
//WRITE_NC(LOGIC_ANALYZER_CH2, true);
current_block = plan_get_current_block();
if (current_block != NULL) {
#ifdef BACKLASH_X
if (current_block->steps_x.wide)
{ //X-axis movement
if ((current_block->direction_bits ^ last_dir_bits) & 1)
{
printf_P(PSTR("BL %d\n"), (current_block->direction_bits & 1)?st_backlash_x:-st_backlash_x);
if (current_block->direction_bits & 1)
WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
else
WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
_delay_us(100);
for (uint8_t i = 0; i < st_backlash_x; i++)
{
WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
_delay_us(100);
WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
_delay_us(900);
}
}
last_dir_bits &= ~1;
last_dir_bits |= current_block->direction_bits & 1;
}
#endif
#ifdef BACKLASH_Y
if (current_block->steps_y.wide)
{ //Y-axis movement
if ((current_block->direction_bits ^ last_dir_bits) & 2)
{
printf_P(PSTR("BL %d\n"), (current_block->direction_bits & 2)?st_backlash_y:-st_backlash_y);
if (current_block->direction_bits & 2)
WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
else
WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
_delay_us(100);
for (uint8_t i = 0; i < st_backlash_y; i++)
{
WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
_delay_us(100);
WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
_delay_us(900);
}
}
last_dir_bits &= ~2;
last_dir_bits |= current_block->direction_bits & 2;
}
#endif
// The busy flag is set by the plan_get_current_block() call.
// current_block->busy = true;
// Initializes the trapezoid generator from the current block. Called whenever a new
// block begins.
deceleration_time = 0;
// Set the nominal step loops to zero to indicate, that the timer value is not known yet.
// That means, delay the initialization of nominal step rate and step loops until the steady
// state is reached.
step_loops_nominal = 0;
acc_step_rate = uint16_t(current_block->initial_rate);
acceleration_time = calc_timer(acc_step_rate, step_loops);
#ifdef LIN_ADVANCE
if (current_block->use_advance_lead) {
LA_decelerate_after = current_block->decelerate_after;
final_adv_steps = current_block->final_adv_steps;
max_adv_steps = current_block->max_adv_steps;
e_step_loops = current_block->advance_step_loops;
} else {
e_steps = 0;
e_step_loops = 1;
current_adv_steps = 0;
}
nextAdvanceISR = ADV_NEVER;
LA_phase = -1;
#endif
if (current_block->flag & BLOCK_FLAG_E_RESET) {
count_position[E_AXIS] = 0;
}
if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) {
counter_x.lo = -(current_block->step_event_count.lo >> 1);
counter_y.lo = counter_x.lo;
counter_z.lo = counter_x.lo;
counter_e.lo = counter_x.lo;
} else {
counter_x.wide = -(current_block->step_event_count.wide >> 1);
counter_y.wide = counter_x.wide;
counter_z.wide = counter_x.wide;
counter_e.wide = counter_x.wide;
}
step_events_completed.wide = 0;
// Set directions.
out_bits = current_block->direction_bits;
// Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
if((out_bits & (1<<X_AXIS))!=0){
WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
count_direction[X_AXIS]=-1;
} else {
WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
count_direction[X_AXIS]=1;
}
if((out_bits & (1<<Y_AXIS))!=0){
WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
count_direction[Y_AXIS]=-1;
} else {
WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
count_direction[Y_AXIS]=1;
}
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
WRITE_NC(Z_DIR_PIN,INVERT_Z_DIR);
count_direction[Z_AXIS]=-1;
} else { // +direction
WRITE_NC(Z_DIR_PIN,!INVERT_Z_DIR);
count_direction[Z_AXIS]=1;
}
if ((out_bits & (1 << E_AXIS)) != 0) { // -direction
#ifndef LIN_ADVANCE
WRITE(E0_DIR_PIN,
#ifdef SNMM
(mmu_extruder == 0 || mmu_extruder == 2) ? !INVERT_E0_DIR :
#endif // SNMM
INVERT_E0_DIR);
#endif /* LIN_ADVANCE */
count_direction[E_AXIS] = -1;
} else { // +direction
#ifndef LIN_ADVANCE
WRITE(E0_DIR_PIN,
#ifdef SNMM
(mmu_extruder == 0 || mmu_extruder == 2) ? INVERT_E0_DIR :
#endif // SNMM
!INVERT_E0_DIR);
#endif /* LIN_ADVANCE */
count_direction[E_AXIS] = 1;
}
#if defined(FILAMENT_SENSOR) && defined(PAT9125)
fsensor_st_block_begin(count_direction[E_AXIS] < 0);
#endif //FILAMENT_SENSOR
}
else {
_NEXT_ISR(2000); // 1kHz.
#ifdef LIN_ADVANCE
// reset LA state when there's no block
nextAdvanceISR = ADV_NEVER;
e_steps = 0;
// incrementally lose pressure to give a chance for
// a new LA block to be scheduled and recover
if(current_adv_steps)
--current_adv_steps;
#endif
}
//WRITE_NC(LOGIC_ANALYZER_CH2, false);
}
// Check limit switches.
FORCE_INLINE void stepper_check_endstops()
{
if(check_endstops)
{
#ifndef COREXY
if ((out_bits & (1<<X_AXIS)) != 0) // stepping along -X axis
#else
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) //-X occurs for -A and -B
#endif
{
#if ( (defined(X_MIN_PIN) && (X_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT)
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
x_min_endstop = (READ(X_TMC2130_DIAG) != 0);
#else
// Normal homing
x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
#endif
if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) {
endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_x_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_x_min_endstop = x_min_endstop;
#endif
} else { // +direction
#if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT)
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
x_max_endstop = (READ(X_TMC2130_DIAG) != 0);
#else
// Normal homing
x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
#endif
if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){
endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_x_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_x_max_endstop = x_max_endstop;
#endif
}
#ifndef COREXY
if ((out_bits & (1<<Y_AXIS)) != 0) // -direction
#else
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) // -Y occurs for -A and +B
#endif
{
#if ( (defined(Y_MIN_PIN) && (Y_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT)
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
y_min_endstop = (READ(Y_TMC2130_DIAG) != 0);
#else
// Normal homing
y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
#endif
if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) {
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_y_min_endstop = y_min_endstop;
#endif
} else { // +direction
#if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT)
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
y_max_endstop = (READ(Y_TMC2130_DIAG) != 0);
#else
// Normal homing
y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
#endif
if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){
endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_y_max_endstop = y_max_endstop;
#endif
}
if ((out_bits & (1<<Z_AXIS)) != 0) // -direction
{
#if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
if (! check_z_endstop) {
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
#ifdef TMC2130_STEALTH_Z
if ((tmc2130_mode == TMC2130_MODE_SILENT) && !(tmc2130_sg_homing_axes_mask & 0x04))
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
else
#endif //TMC2130_STEALTH_Z
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
#else
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
#endif //TMC2130_SG_HOMING
if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_z_min_endstop = z_min_endstop;
}
#endif
} else { // +direction
#if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT)
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
#ifdef TMC2130_STEALTH_Z
if ((tmc2130_mode == TMC2130_MODE_SILENT) && !(tmc2130_sg_homing_axes_mask & 0x04))
z_max_endstop = false;
else
#endif //TMC2130_STEALTH_Z
z_max_endstop = (READ(Z_TMC2130_DIAG) != 0);
#else
z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
#endif //TMC2130_SG_HOMING
if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_z_max_endstop = z_max_endstop;
#endif
}
}
// Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements.
#if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
if (check_z_endstop) {
// Check the Z min end-stop no matter what.
// Good for searching for the center of an induction target.
#ifdef TMC2130_SG_HOMING
// Stall guard homing turned on
#ifdef TMC2130_STEALTH_Z
if ((tmc2130_mode == TMC2130_MODE_SILENT) && !(tmc2130_sg_homing_axes_mask & 0x04))
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
else
#endif //TMC2130_STEALTH_Z
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
#else
z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
#endif //TMC2130_SG_HOMING
if(z_min_endstop && old_z_min_endstop) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
step_events_completed.wide = current_block->step_event_count.wide;
}
old_z_min_endstop = z_min_endstop;
}
#endif
}
FORCE_INLINE void stepper_tick_lowres()
{
for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves)
MSerial.checkRx(); // Check for serial chars.
// Step in X axis
counter_x.lo += current_block->steps_x.lo;
if (counter_x.lo > 0) {
WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
#ifdef DEBUG_XSTEP_DUP_PIN
WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN);
#endif //DEBUG_XSTEP_DUP_PIN
counter_x.lo -= current_block->step_event_count.lo;
count_position[X_AXIS]+=count_direction[X_AXIS];
WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
#ifdef DEBUG_XSTEP_DUP_PIN
WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
#endif //DEBUG_XSTEP_DUP_PIN
}
// Step in Y axis
counter_y.lo += current_block->steps_y.lo;
if (counter_y.lo > 0) {
WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
#ifdef DEBUG_YSTEP_DUP_PIN
WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN);
#endif //DEBUG_YSTEP_DUP_PIN
counter_y.lo -= current_block->step_event_count.lo;
count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
#ifdef DEBUG_YSTEP_DUP_PIN
WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
#endif //DEBUG_YSTEP_DUP_PIN
}
// Step in Z axis
counter_z.lo += current_block->steps_z.lo;
if (counter_z.lo > 0) {
WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
counter_z.lo -= current_block->step_event_count.lo;
count_position[Z_AXIS]+=count_direction[Z_AXIS];
WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
}
// Step in E axis
counter_e.lo += current_block->steps_e.lo;
if (counter_e.lo > 0) {
#ifndef LIN_ADVANCE
WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
#endif /* LIN_ADVANCE */
counter_e.lo -= current_block->step_event_count.lo;
count_position[E_AXIS] += count_direction[E_AXIS];
#ifdef LIN_ADVANCE
e_steps += count_direction[E_AXIS];
#else
#ifdef FILAMENT_SENSOR
fsensor_counter += count_direction[E_AXIS];
#endif //FILAMENT_SENSOR
WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
#endif
}
if(++ step_events_completed.lo >= current_block->step_event_count.lo)
break;
}
}
FORCE_INLINE void stepper_tick_highres()
{
for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves)
MSerial.checkRx(); // Check for serial chars.
// Step in X axis
counter_x.wide += current_block->steps_x.wide;
if (counter_x.wide > 0) {
WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
#ifdef DEBUG_XSTEP_DUP_PIN
WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN);
#endif //DEBUG_XSTEP_DUP_PIN
counter_x.wide -= current_block->step_event_count.wide;
count_position[X_AXIS]+=count_direction[X_AXIS];
WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
#ifdef DEBUG_XSTEP_DUP_PIN
WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
#endif //DEBUG_XSTEP_DUP_PIN
}
// Step in Y axis
counter_y.wide += current_block->steps_y.wide;
if (counter_y.wide > 0) {
WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
#ifdef DEBUG_YSTEP_DUP_PIN
WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN);
#endif //DEBUG_YSTEP_DUP_PIN
counter_y.wide -= current_block->step_event_count.wide;
count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
#ifdef DEBUG_YSTEP_DUP_PIN
WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
#endif //DEBUG_YSTEP_DUP_PIN
}
// Step in Z axis
counter_z.wide += current_block->steps_z.wide;
if (counter_z.wide > 0) {
WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
counter_z.wide -= current_block->step_event_count.wide;
count_position[Z_AXIS]+=count_direction[Z_AXIS];
WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
}
// Step in E axis
counter_e.wide += current_block->steps_e.wide;
if (counter_e.wide > 0) {
#ifndef LIN_ADVANCE
WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
#endif /* LIN_ADVANCE */
counter_e.wide -= current_block->step_event_count.wide;
count_position[E_AXIS]+=count_direction[E_AXIS];
#ifdef LIN_ADVANCE
e_steps += count_direction[E_AXIS];
#else
#ifdef FILAMENT_SENSOR
fsensor_counter += count_direction[E_AXIS];
#endif //FILAMENT_SENSOR
WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
#endif
}
if(++ step_events_completed.wide >= current_block->step_event_count.wide)
break;
}
}
#ifdef LIN_ADVANCE
// @wavexx: fast uint16_t division for small dividends<5
// q/3 based on "Hacker's delight" formula
FORCE_INLINE uint16_t fastdiv(uint16_t q, uint8_t d)
{
if(d != 3) return q >> (d / 2);
else return ((uint32_t)0xAAAB * q) >> 17;
}
FORCE_INLINE void advance_spread(uint16_t timer)
{
if(eISR_Err > timer)
{
// advance-step skipped
eISR_Err -= timer;
eISR_Rate = timer;
nextAdvanceISR = timer;
return;
}
// at least one step
uint8_t ticks = 1;
uint32_t block = current_block->advance_rate;
uint16_t max_t = timer - eISR_Err;
while (block < max_t)
{
++ticks;
block += current_block->advance_rate;
}
if (block > timer)
eISR_Err += block - timer;
else
eISR_Err -= timer - block;
if (ticks <= 4)
eISR_Rate = fastdiv(timer, ticks);
else
{
// >4 ticks are still possible on slow moves
eISR_Rate = timer / ticks;
}
nextAdvanceISR = eISR_Rate / 2;
}
#endif
FORCE_INLINE void isr() {
//WRITE_NC(LOGIC_ANALYZER_CH0, true);
//if (UVLO) uvlo();
// If there is no current block, attempt to pop one from the buffer
if (current_block == NULL)
stepper_next_block();
if (current_block != NULL)
{
stepper_check_endstops();
if (current_block->flag & BLOCK_FLAG_DDA_LOWRES)
stepper_tick_lowres();
else
stepper_tick_highres();
#ifdef LIN_ADVANCE
if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
uint8_t la_state = 0;
#endif
// Calculate new timer value
// 13.38-14.63us for steady state,
// 25.12us for acceleration / deceleration.
{
//WRITE_NC(LOGIC_ANALYZER_CH1, true);
if (step_events_completed.wide <= (unsigned long int)current_block->accelerate_until) {
// v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
acc_step_rate += uint16_t(current_block->initial_rate);
// upper limit
if(acc_step_rate > uint16_t(current_block->nominal_rate))
acc_step_rate = current_block->nominal_rate;
// step_rate to timer interval
uint16_t timer = calc_timer(acc_step_rate, step_loops);
_NEXT_ISR(timer);
acceleration_time += timer;
#ifdef LIN_ADVANCE
if (current_block->use_advance_lead) {
if (step_events_completed.wide <= (unsigned long int)step_loops)
la_state = ADV_INIT;
}
#endif
}
else if (step_events_completed.wide > (unsigned long int)current_block->decelerate_after) {
uint16_t step_rate;
MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
if ((step_rate & 0x8000) || step_rate < uint16_t(current_block->final_rate)) {
// Result is negative or too small.
step_rate = uint16_t(current_block->final_rate);
}
// Step_rate to timer interval.
uint16_t timer = calc_timer(step_rate, step_loops);
_NEXT_ISR(timer);
deceleration_time += timer;
#ifdef LIN_ADVANCE
if (current_block->use_advance_lead) {
la_state = ADV_DECELERATE;
if (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops)
la_state |= ADV_INIT;
}
#endif
}
else {
if (! step_loops_nominal) {
// Calculation of the steady state timer rate has been delayed to the 1st tick of the steady state to lower
// the initial interrupt blocking.
OCR1A_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
step_loops_nominal = step_loops;
}
_NEXT_ISR(OCR1A_nominal);
}
//WRITE_NC(LOGIC_ANALYZER_CH1, false);
}
#ifdef LIN_ADVANCE
// avoid multiple instances or function calls to advance_spread
if (la_state & ADV_INIT) eISR_Err = current_block->advance_rate / 4;
if (la_state & ADV_INIT || nextAdvanceISR != ADV_NEVER) {
advance_spread(main_Rate);
if (la_state & ADV_DECELERATE) {
if (step_loops == e_step_loops)
LA_phase = (eISR_Rate > main_Rate);
else {
// avoid overflow through division. warning: we need to _guarantee_ step_loops
// and e_step_loops are <= 4 due to fastdiv's limit
LA_phase = (fastdiv(eISR_Rate, step_loops) > fastdiv(main_Rate, e_step_loops));
}
}
}
// Check for serial chars. This executes roughtly inbetween 50-60% of the total runtime of the
// entire isr, making this spot a much better choice than checking during esteps
MSerial.checkRx();
#endif
// If current block is finished, reset pointer
if (step_events_completed.wide >= current_block->step_event_count.wide) {
#if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
fsensor_st_block_chunk(fsensor_counter);
fsensor_counter = 0;
#endif //FILAMENT_SENSOR
current_block = NULL;
plan_discard_current_block();
}
#if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
else if ((abs(fsensor_counter) >= fsensor_chunk_len))
{
fsensor_st_block_chunk(fsensor_counter);
fsensor_counter = 0;
}
#endif //FILAMENT_SENSOR
}
#ifdef TMC2130
tmc2130_st_isr();
#endif //TMC2130
//WRITE_NC(LOGIC_ANALYZER_CH0, false);
}
#ifdef LIN_ADVANCE
// Timer interrupt for E. e_steps is set in the main routine.
FORCE_INLINE void advance_isr() {
if (step_events_completed.wide > LA_decelerate_after && current_adv_steps > final_adv_steps) {
// decompression
e_steps -= e_step_loops;
if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
if(current_adv_steps > e_step_loops)
current_adv_steps -= e_step_loops;
else
current_adv_steps = 0;
nextAdvanceISR = eISR_Rate;
}
else if (step_events_completed.wide < LA_decelerate_after && current_adv_steps < max_adv_steps) {
// compression
e_steps += e_step_loops;
if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
current_adv_steps += e_step_loops;
nextAdvanceISR = eISR_Rate;
}
else {
// advance steps completed
nextAdvanceISR = ADV_NEVER;
LA_phase = -1;
e_step_loops = 1;
}
}
FORCE_INLINE void advance_isr_scheduler() {
// Integrate the final timer value, accounting for scheduling adjustments
if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
{
if(nextAdvanceISR > OCR1A)
nextAdvanceISR -= OCR1A;
else
nextAdvanceISR = 0;
}
if(nextMainISR > OCR1A)
nextMainISR -= OCR1A;
else
nextMainISR = 0;
// Run main stepping ISR if flagged
if (!nextMainISR)
{
#ifdef LA_DEBUG_LOGIC
WRITE_NC(LOGIC_ANALYZER_CH0, true);
#endif
isr();
#ifdef LA_DEBUG_LOGIC
WRITE_NC(LOGIC_ANALYZER_CH0, false);
#endif
}
// Run the next advance isr if triggered
bool eisr = !nextAdvanceISR;
if (eisr)
{
#ifdef LA_DEBUG_LOGIC
WRITE_NC(LOGIC_ANALYZER_CH1, true);
#endif
advance_isr();
#ifdef LA_DEBUG_LOGIC
WRITE_NC(LOGIC_ANALYZER_CH1, false);
#endif
}
// Tick E steps if any
if (e_steps && (LA_phase < 0 || LA_phase == eisr)) {
uint8_t max_ticks = (eisr? e_step_loops: step_loops);
max_ticks = min(abs(e_steps), max_ticks);
bool rev = (e_steps < 0);
do
{
WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
e_steps += (rev? 1: -1);
WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
#if defined(FILAMENT_SENSOR) && defined(PAT9125)
fsensor_counter += (rev? -1: 1);
#endif
}
while(--max_ticks);
#if defined(FILAMENT_SENSOR) && defined(PAT9125)
if (abs(fsensor_counter) >= fsensor_chunk_len)
{
fsensor_st_block_chunk(fsensor_counter);
fsensor_counter = 0;
}
#endif
}
// Schedule the next closest tick, ignoring advance if scheduled too
// soon in order to avoid skewing the regular stepper acceleration
if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + TCNT1 + 40) < nextMainISR)
OCR1A = nextAdvanceISR;
else
OCR1A = nextMainISR;
}
#endif // LIN_ADVANCE
void st_init()
{