123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692 |
- /*
- 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
- /*
- * Stepping macros
- */
- #define _STEP_PIN_X_AXIS X_STEP_PIN
- #define _STEP_PIN_Y_AXIS Y_STEP_PIN
- #define _STEP_PIN_Z_AXIS Z_STEP_PIN
- #define _STEP_PIN_E_AXIS E0_STEP_PIN
- #ifdef DEBUG_XSTEP_DUP_PIN
- #define _STEP_PIN_X_DUP_AXIS DEBUG_XSTEP_DUP_PIN
- #endif
- #ifdef DEBUG_YSTEP_DUP_PIN
- #define _STEP_PIN_Y_DUP_AXIS DEBUG_YSTEP_DUP_PIN
- #endif
- #ifdef Y_DUAL_STEPPER_DRIVERS
- #error Y_DUAL_STEPPER_DRIVERS not fully implemented
- #define _STEP_PIN_Y2_AXIS Y2_STEP_PIN
- #endif
- #ifdef Z_DUAL_STEPPER_DRIVERS
- #error Z_DUAL_STEPPER_DRIVERS not fully implemented
- #define _STEP_PIN_Z2_AXIS Z2_STEP_PIN
- #endif
- #ifdef TMC2130
- #define STEPPER_MINIMUM_PULSE TMC2130_MINIMUM_PULSE
- #define STEPPER_SET_DIR_DELAY TMC2130_SET_DIR_DELAY
- #else
- #define STEPPER_MINIMUM_PULSE 2
- #define STEPPER_SET_DIR_DELAY 100
- #endif
- #ifdef TMC2130_DEDGE_STEPPING
- #define STEP_NC_HI(axis) TOGGLE(_STEP_PIN_##axis)
- #define STEP_NC_LO(axis) //NOP
- #else
- #define _STEP_HI_X_AXIS !INVERT_X_STEP_PIN
- #define _STEP_LO_X_AXIS INVERT_X_STEP_PIN
- #define _STEP_HI_Y_AXIS !INVERT_Y_STEP_PIN
- #define _STEP_LO_Y_AXIS INVERT_Y_STEP_PIN
- #define _STEP_HI_Z_AXIS !INVERT_Z_STEP_PIN
- #define _STEP_LO_Z_AXIS INVERT_Z_STEP_PIN
- #define _STEP_HI_E_AXIS !INVERT_E_STEP_PIN
- #define _STEP_LO_E_AXIS INVERT_E_STEP_PIN
- #define STEP_NC_HI(axis) WRITE_NC(_STEP_PIN_##axis, _STEP_HI_##axis)
- #define STEP_NC_LO(axis) WRITE_NC(_STEP_PIN_##axis, _STEP_LO_##axis)
- #endif //TMC2130_DEDGE_STEPPING
- //===========================================================================
- //=============================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; // initialize LA
- static const uint8_t ADV_ACC_VARY = 0b10; // varying acceleration phase
- 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 target_adv_steps;
- static int8_t e_steps; // scheduled e-steps during each isr loop
- static uint8_t e_step_loops; // e-steps to execute at most in each isr loop
- static uint8_t e_extruding; // current move is an extrusion move
- static int8_t LA_phase; // LA compensation 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++)
- {
- STEP_NC_HI(X_AXIS);
- _delay_us(100);
- STEP_NC_LO(X_AXIS);
- _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++)
- {
- STEP_NC_HI(Y_AXIS);
- _delay_us(100);
- STEP_NC_LO(Y_AXIS);
- _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) {
- e_step_loops = current_block->advance_step_loops;
- target_adv_steps = current_block->max_adv_steps;
- } else {
- e_step_loops = 1;
- }
- e_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;
- #ifdef LIN_ADVANCE
- e_extruding = current_block->steps_e.lo != 0;
- #endif
- } 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;
- #ifdef LIN_ADVANCE
- e_extruding = current_block->steps_e.wide != 0;
- #endif
- }
- 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) {
- STEP_NC_HI(X_AXIS);
- #ifdef DEBUG_XSTEP_DUP_PIN
- STEP_NC_HI(X_DUP_AXIS);
- #endif //DEBUG_XSTEP_DUP_PIN
- counter_x.lo -= current_block->step_event_count.lo;
- count_position[X_AXIS]+=count_direction[X_AXIS];
- STEP_NC_LO(X_AXIS);
- #ifdef DEBUG_XSTEP_DUP_PIN
- STEP_NC_LO(X_DUP_AXIS);
- #endif //DEBUG_XSTEP_DUP_PIN
- }
- // Step in Y axis
- counter_y.lo += current_block->steps_y.lo;
- if (counter_y.lo > 0) {
- STEP_NC_HI(Y_AXIS);
- #ifdef DEBUG_YSTEP_DUP_PIN
- STEP_NC_HI(Y_DUP_AXIS);
- #endif //DEBUG_YSTEP_DUP_PIN
- counter_y.lo -= current_block->step_event_count.lo;
- count_position[Y_AXIS]+=count_direction[Y_AXIS];
- STEP_NC_LO(Y_AXIS);
- #ifdef DEBUG_YSTEP_DUP_PIN
- STEP_NC_LO(Y_DUP_AXIS);
- #endif //DEBUG_YSTEP_DUP_PIN
- }
- // Step in Z axis
- counter_z.lo += current_block->steps_z.lo;
- if (counter_z.lo > 0) {
- STEP_NC_HI(Z_AXIS);
- counter_z.lo -= current_block->step_event_count.lo;
- count_position[Z_AXIS]+=count_direction[Z_AXIS];
- STEP_NC_LO(Z_AXIS);
- }
- // Step in E axis
- counter_e.lo += current_block->steps_e.lo;
- if (counter_e.lo > 0) {
- #ifndef LIN_ADVANCE
- STEP_NC_HI(E_AXIS);
- #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
- STEP_NC_LO(E_AXIS);
- #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) {
- STEP_NC_HI(X_AXIS);
- #ifdef DEBUG_XSTEP_DUP_PIN
- STEP_NC_HI(X_DUP_AXIS);
- #endif //DEBUG_XSTEP_DUP_PIN
- counter_x.wide -= current_block->step_event_count.wide;
- count_position[X_AXIS]+=count_direction[X_AXIS];
- STEP_NC_LO(X_AXIS);
- #ifdef DEBUG_XSTEP_DUP_PIN
- STEP_NC_LO(X_DUP_AXIS);
- #endif //DEBUG_XSTEP_DUP_PIN
- }
- // Step in Y axis
- counter_y.wide += current_block->steps_y.wide;
- if (counter_y.wide > 0) {
- STEP_NC_HI(Y_AXIS);
- #ifdef DEBUG_YSTEP_DUP_PIN
- STEP_NC_HI(Y_DUP_AXIS);
- #endif //DEBUG_YSTEP_DUP_PIN
- counter_y.wide -= current_block->step_event_count.wide;
- count_position[Y_AXIS]+=count_direction[Y_AXIS];
- STEP_NC_LO(Y_AXIS);
- #ifdef DEBUG_YSTEP_DUP_PIN
- STEP_NC_LO(Y_DUP_AXIS);
- #endif //DEBUG_YSTEP_DUP_PIN
- }
- // Step in Z axis
- counter_z.wide += current_block->steps_z.wide;
- if (counter_z.wide > 0) {
- STEP_NC_HI(Z_AXIS);
- counter_z.wide -= current_block->step_event_count.wide;
- count_position[Z_AXIS]+=count_direction[Z_AXIS];
- STEP_NC_LO(Z_AXIS);
- }
- // Step in E axis
- counter_e.wide += current_block->steps_e.wide;
- if (counter_e.wide > 0) {
- #ifndef LIN_ADVANCE
- STEP_NC_HI(E_AXIS);
- #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
- STEP_NC_LO(E_AXIS);
- #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 | ADV_ACC_VARY;
- }
- #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) {
- if (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops) {
- target_adv_steps = current_block->final_adv_steps;
- la_state = ADV_INIT | ADV_ACC_VARY;
- }
- }
- #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;
- #ifdef LIN_ADVANCE
- if(current_block->use_advance_lead) {
- if (!nextAdvanceISR) {
- // Due to E-jerk, there can be discontinuities in pressure state where an
- // acceleration or deceleration can be skipped or joined with the previous block.
- // If LA was not previously active, re-check the pressure level
- la_state = ADV_INIT;
- }
- }
- #endif
- }
- _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) {
- if (current_adv_steps == target_adv_steps) {
- // nothing to be done in this phase
- la_state = 0;
- }
- else {
- eISR_Err = current_block->advance_rate / 4;
- if ((la_state & ADV_ACC_VARY) && e_extruding && (current_adv_steps > target_adv_steps)) {
- // LA could reverse the direction of extrusion in this phase
- LA_phase = 0;
- }
- }
- }
- if (la_state & ADV_INIT || nextAdvanceISR != ADV_NEVER) {
- // update timers & phase for the next iteration
- advance_spread(main_Rate);
- if (LA_phase >= 0) {
- 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 (current_adv_steps > target_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 (current_adv_steps < target_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
- {
- STEP_NC_HI(E_AXIS);
- e_steps += (rev? 1: -1);
- STEP_NC_LO(E_AXIS);
- #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()
- {
- #ifdef TMC2130
- tmc2130_init();
- #endif //TMC2130
- st_current_init(); //Initialize Digipot Motor Current
- microstep_init(); //Initialize Microstepping Pins
- //Initialize Dir Pins
- #if defined(X_DIR_PIN) && X_DIR_PIN > -1
- SET_OUTPUT(X_DIR_PIN);
- #endif
- #if defined(X2_DIR_PIN) && X2_DIR_PIN > -1
- SET_OUTPUT(X2_DIR_PIN);
- #endif
- #if defined(Y_DIR_PIN) && Y_DIR_PIN > -1
- SET_OUTPUT(Y_DIR_PIN);
-
- #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_DIR_PIN) && (Y2_DIR_PIN > -1)
- SET_OUTPUT(Y2_DIR_PIN);
- #endif
- #endif
- #if defined(Z_DIR_PIN) && Z_DIR_PIN > -1
- SET_OUTPUT(Z_DIR_PIN);
- #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_DIR_PIN) && (Z2_DIR_PIN > -1)
- SET_OUTPUT(Z2_DIR_PIN);
- #endif
- #endif
- #if defined(E0_DIR_PIN) && E0_DIR_PIN > -1
- SET_OUTPUT(E0_DIR_PIN);
- #endif
- #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
- SET_OUTPUT(E1_DIR_PIN);
- #endif
- #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
- SET_OUTPUT(E2_DIR_PIN);
- #endif
- //Initialize Enable Pins - steppers default to disabled.
- #if defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
- SET_OUTPUT(X_ENABLE_PIN);
- if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
- #endif
- #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
- SET_OUTPUT(X2_ENABLE_PIN);
- if(!X_ENABLE_ON) WRITE(X2_ENABLE_PIN,HIGH);
- #endif
- #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
- SET_OUTPUT(Y_ENABLE_PIN);
- if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
-
- #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_ENABLE_PIN) && (Y2_ENABLE_PIN > -1)
- SET_OUTPUT(Y2_ENABLE_PIN);
- if(!Y_ENABLE_ON) WRITE(Y2_ENABLE_PIN,HIGH);
- #endif
- #endif
- #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
- SET_OUTPUT(Z_ENABLE_PIN);
- if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
- #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_ENABLE_PIN) && (Z2_ENABLE_PIN > -1)
- SET_OUTPUT(Z2_ENABLE_PIN);
- if(!Z_ENABLE_ON) WRITE(Z2_ENABLE_PIN,HIGH);
- #endif
- #endif
- #if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
- SET_OUTPUT(E0_ENABLE_PIN);
- if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
- #endif
- #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
- SET_OUTPUT(E1_ENABLE_PIN);
- if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
- #endif
- #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
- SET_OUTPUT(E2_ENABLE_PIN);
- if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
- #endif
- //endstops and pullups
- #ifdef TMC2130_SG_HOMING
- SET_INPUT(X_TMC2130_DIAG);
- WRITE(X_TMC2130_DIAG,HIGH);
-
- SET_INPUT(Y_TMC2130_DIAG);
- WRITE(Y_TMC2130_DIAG,HIGH);
-
- SET_INPUT(Z_TMC2130_DIAG);
- WRITE(Z_TMC2130_DIAG,HIGH);
- SET_INPUT(E0_TMC2130_DIAG);
- WRITE(E0_TMC2130_DIAG,HIGH);
-
- #endif
-
- #if defined(X_MIN_PIN) && X_MIN_PIN > -1
- SET_INPUT(X_MIN_PIN);
- #ifdef ENDSTOPPULLUP_XMIN
- WRITE(X_MIN_PIN,HIGH);
- #endif
- #endif
- #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
- SET_INPUT(Y_MIN_PIN);
- #ifdef ENDSTOPPULLUP_YMIN
- WRITE(Y_MIN_PIN,HIGH);
- #endif
- #endif
- #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
- SET_INPUT(Z_MIN_PIN);
- #ifdef ENDSTOPPULLUP_ZMIN
- WRITE(Z_MIN_PIN,HIGH);
- #endif
- #endif
- #if defined(X_MAX_PIN) && X_MAX_PIN > -1
- SET_INPUT(X_MAX_PIN);
- #ifdef ENDSTOPPULLUP_XMAX
- WRITE(X_MAX_PIN,HIGH);
- #endif
- #endif
- #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
- SET_INPUT(Y_MAX_PIN);
- #ifdef ENDSTOPPULLUP_YMAX
- WRITE(Y_MAX_PIN,HIGH);
- #endif
- #endif
- #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
- SET_INPUT(Z_MAX_PIN);
- #ifdef ENDSTOPPULLUP_ZMAX
- WRITE(Z_MAX_PIN,HIGH);
- #endif
- #endif
- #if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
- SET_INPUT(TACH_0);
- #ifdef TACH0PULLUP
- WRITE(TACH_0, HIGH);
- #endif
- #endif
- //Initialize Step Pins
- #if defined(X_STEP_PIN) && (X_STEP_PIN > -1)
- SET_OUTPUT(X_STEP_PIN);
- WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
- #ifdef DEBUG_XSTEP_DUP_PIN
- SET_OUTPUT(DEBUG_XSTEP_DUP_PIN);
- WRITE(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
- #endif //DEBUG_XSTEP_DUP_PIN
- disable_x();
- #endif
- #if defined(X2_STEP_PIN) && (X2_STEP_PIN > -1)
- SET_OUTPUT(X2_STEP_PIN);
- WRITE(X2_STEP_PIN,INVERT_X_STEP_PIN);
- disable_x();
- #endif
- #if defined(Y_STEP_PIN) && (Y_STEP_PIN > -1)
- SET_OUTPUT(Y_STEP_PIN);
- WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
- #ifdef DEBUG_YSTEP_DUP_PIN
- SET_OUTPUT(DEBUG_YSTEP_DUP_PIN);
- WRITE(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
- #endif //DEBUG_YSTEP_DUP_PIN
- #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && (Y2_STEP_PIN > -1)
- SET_OUTPUT(Y2_STEP_PIN);
- WRITE(Y2_STEP_PIN,INVERT_Y_STEP_PIN);
- #endif
- disable_y();
- #endif
- #if defined(Z_STEP_PIN) && (Z_STEP_PIN > -1)
- SET_OUTPUT(Z_STEP_PIN);
- WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
- #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && (Z2_STEP_PIN > -1)
- SET_OUTPUT(Z2_STEP_PIN);
- WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
- #endif
- #ifdef PSU_Delta
- init_force_z();
- #endif // PSU_Delta
- disable_z();
- #endif
- #if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1)
- SET_OUTPUT(E0_STEP_PIN);
- WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
- disable_e0();
- #endif
- #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
- SET_OUTPUT(E1_STEP_PIN);
- WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
- disable_e1();
- #endif
- #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
- SET_OUTPUT(E2_STEP_PIN);
- WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
- disable_e2();
- #endif
- // waveform generation = 0100 = CTC
- TCCR1B &= ~(1<<WGM13);
- TCCR1B |= (1<<WGM12);
- TCCR1A &= ~(1<<WGM11);
- TCCR1A &= ~(1<<WGM10);
- // output mode = 00 (disconnected)
- TCCR1A &= ~(3<<COM1A0);
- TCCR1A &= ~(3<<COM1B0);
- // Set the timer pre-scaler
- // Generally we use a divider of 8, resulting in a 2MHz timer
- // frequency on a 16MHz MCU. If you are going to change this, be
- // sure to regenerate speed_lookuptable.h with
- // create_speed_lookuptable.py
- TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
- // Plan the first interrupt after 8ms from now.
- OCR1A = 0x4000;
- TCNT1 = 0;
- #ifdef LIN_ADVANCE
- #ifdef LA_DEBUG_LOGIC
- LOGIC_ANALYZER_CH0_ENABLE;
- LOGIC_ANALYZER_CH1_ENABLE;
- WRITE_NC(LOGIC_ANALYZER_CH0, false);
- WRITE_NC(LOGIC_ANALYZER_CH1, false);
- #endif
- // Initialize state for the linear advance scheduler
- nextMainISR = 0;
- nextAdvanceISR = ADV_NEVER;
- main_Rate = ADV_NEVER;
- current_adv_steps = 0;
- #endif
- enable_endstops(true); // Start with endstops active. After homing they can be disabled
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- sei();
- }
- void st_reset_timer()
- {
- // Clear a possible pending interrupt on OCR1A overflow.
- TIFR1 |= 1 << OCF1A;
- // Reset the counter.
- TCNT1 = 0;
- // Wake up after 1ms from now.
- OCR1A = 2000;
- #ifdef LIN_ADVANCE
- nextMainISR = 0;
- if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
- nextAdvanceISR = 0;
- #endif
- }
- // Block until all buffered steps are executed
- void st_synchronize()
- {
- while(blocks_queued())
- {
- #ifdef TMC2130
- manage_heater();
- // Vojtech: Don't disable motors inside the planner!
- if (!tmc2130_update_sg())
- {
- manage_inactivity(true);
- lcd_update(0);
- }
- #else //TMC2130
- manage_heater();
- // Vojtech: Don't disable motors inside the planner!
- manage_inactivity(true);
- lcd_update(0);
- #endif //TMC2130
- }
- }
- void st_set_position(const long &x, const long &y, const long &z, const long &e)
- {
- CRITICAL_SECTION_START;
- // Copy 4x4B.
- // This block locks the interrupts globally for 4.56 us,
- // which corresponds to a maximum repeat frequency of 219.18 kHz.
- // This blocking is safe in the context of a 10kHz stepper driver interrupt
- // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
- count_position[X_AXIS] = x;
- count_position[Y_AXIS] = y;
- count_position[Z_AXIS] = z;
- count_position[E_AXIS] = e;
- CRITICAL_SECTION_END;
- }
- void st_set_e_position(const long &e)
- {
- CRITICAL_SECTION_START;
- count_position[E_AXIS] = e;
- CRITICAL_SECTION_END;
- }
- long st_get_position(uint8_t axis)
- {
- long count_pos;
- CRITICAL_SECTION_START;
- count_pos = count_position[axis];
- CRITICAL_SECTION_END;
- return count_pos;
- }
- void st_get_position_xy(long &x, long &y)
- {
- CRITICAL_SECTION_START;
- x = count_position[X_AXIS];
- y = count_position[Y_AXIS];
- CRITICAL_SECTION_END;
- }
- float st_get_position_mm(uint8_t axis)
- {
- float steper_position_in_steps = st_get_position(axis);
- return steper_position_in_steps / cs.axis_steps_per_unit[axis];
- }
- void quickStop()
- {
- DISABLE_STEPPER_DRIVER_INTERRUPT();
- while (blocks_queued()) plan_discard_current_block();
- current_block = NULL;
- #ifdef LIN_ADVANCE
- nextAdvanceISR = ADV_NEVER;
- current_adv_steps = 0;
- #endif
- st_reset_timer();
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- }
- #ifdef BABYSTEPPING
- void babystep(const uint8_t axis,const bool direction)
- {
- // MUST ONLY BE CALLED BY A ISR as stepper pins are manipulated directly.
- // note: when switching direction no delay is inserted at the end when the
- // original is restored. We assume enough time passes as the function
- // returns and the stepper is manipulated again (to avoid dead times)
- switch(axis)
- {
- case X_AXIS:
- {
- enable_x();
- uint8_t old_x_dir_pin = READ(X_DIR_PIN); //if dualzstepper, both point to same direction.
- uint8_t new_x_dir_pin = (INVERT_X_DIR)^direction;
- //setup new step
- if (new_x_dir_pin != old_x_dir_pin) {
- WRITE_NC(X_DIR_PIN, new_x_dir_pin);
- delayMicroseconds(STEPPER_SET_DIR_DELAY);
- }
- //perform step
- STEP_NC_HI(X_AXIS);
- #ifdef DEBUG_XSTEP_DUP_PIN
- STEP_NC_HI(X_DUP_AXIS);
- #endif
- #ifndef TMC2130_DEDGE_STEPPING
- delayMicroseconds(STEPPER_MINIMUM_PULSE);
- STEP_NC_LO(X_AXIS);
- #ifdef DEBUG_XSTEP_DUP_PIN
- STEP_NC_LO(X_DUP_AXIS);
- #endif
- #endif
- //get old pin state back.
- WRITE_NC(X_DIR_PIN, old_x_dir_pin);
- }
- break;
- case Y_AXIS:
- {
- enable_y();
- uint8_t old_y_dir_pin = READ(Y_DIR_PIN); //if dualzstepper, both point to same direction.
- uint8_t new_y_dir_pin = (INVERT_Y_DIR)^direction;
- //setup new step
- if (new_y_dir_pin != old_y_dir_pin) {
- WRITE_NC(Y_DIR_PIN, new_y_dir_pin);
- delayMicroseconds(STEPPER_SET_DIR_DELAY);
- }
- //perform step
- STEP_NC_HI(Y_AXIS);
- #ifdef DEBUG_YSTEP_DUP_PIN
- STEP_NC_HI(Y_DUP_AXIS);
- #endif
- #ifndef TMC2130_DEDGE_STEPPING
- delayMicroseconds(STEPPER_MINIMUM_PULSE);
- STEP_NC_LO(Y_AXIS);
- #ifdef DEBUG_YSTEP_DUP_PIN
- STEP_NC_LO(Y_DUP_AXIS);
- #endif
- #endif
- //get old pin state back.
- WRITE_NC(Y_DIR_PIN, old_y_dir_pin);
- }
- break;
- case Z_AXIS:
- {
- enable_z();
- uint8_t old_z_dir_pin = READ(Z_DIR_PIN); //if dualzstepper, both point to same direction.
- uint8_t new_z_dir_pin = (INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z;
- //setup new step
- if (new_z_dir_pin != old_z_dir_pin) {
- WRITE_NC(Z_DIR_PIN, new_z_dir_pin);
- #ifdef Z_DUAL_STEPPER_DRIVERS
- WRITE_NC(Z2_DIR_PIN, new_z_dir_pin);
- #endif
- delayMicroseconds(STEPPER_SET_DIR_DELAY);
- }
- //perform step
- STEP_NC_HI(Z_AXIS);
- #ifdef Z_DUAL_STEPPER_DRIVERS
- STEP_NC_HI(Z2_AXIS);
- #endif
- #ifndef TMC2130_DEDGE_STEPPING
- delayMicroseconds(STEPPER_MINIMUM_PULSE);
- STEP_NC_LO(Z_AXIS);
- #ifdef Z_DUAL_STEPPER_DRIVERS
- STEP_NC_LO(Z2_AXIS);
- #endif
- #endif
- //get old pin state back.
- if (new_z_dir_pin != old_z_dir_pin) {
- WRITE_NC(Z_DIR_PIN, old_z_dir_pin);
- #ifdef Z_DUAL_STEPPER_DRIVERS
- WRITE_NC(Z2_DIR_PIN, old_z_dir_pin);
- #endif
- }
- }
- break;
- default: break;
- }
- }
- #endif //BABYSTEPPING
- #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
- void digitalPotWrite(int address, int value) // From Arduino DigitalPotControl example
- {
- digitalWrite(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip
- SPI.transfer(address); // send in the address and value via SPI:
- SPI.transfer(value);
- digitalWrite(DIGIPOTSS_PIN,HIGH); // take the SS pin high to de-select the chip:
- //_delay(10);
- }
- #endif
- void EEPROM_read_st(int pos, uint8_t* value, uint8_t size)
- {
- do
- {
- *value = eeprom_read_byte((unsigned char*)pos);
- pos++;
- value++;
- }while(--size);
- }
- void st_current_init() //Initialize Digipot Motor Current
- {
- #ifdef MOTOR_CURRENT_PWM_XY_PIN
- uint8_t SilentMode = eeprom_read_byte((uint8_t*)EEPROM_SILENT);
- SilentModeMenu = SilentMode;
- pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
- pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
- pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
- if((SilentMode == SILENT_MODE_OFF) || (farm_mode) ){
- motor_current_setting[0] = motor_current_setting_loud[0];
- motor_current_setting[1] = motor_current_setting_loud[1];
- motor_current_setting[2] = motor_current_setting_loud[2];
- }else{
- motor_current_setting[0] = motor_current_setting_silent[0];
- motor_current_setting[1] = motor_current_setting_silent[1];
- motor_current_setting[2] = motor_current_setting_silent[2];
- }
- st_current_set(0, motor_current_setting[0]);
- st_current_set(1, motor_current_setting[1]);
- st_current_set(2, motor_current_setting[2]);
- //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
- TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
- #endif
- }
- #ifdef MOTOR_CURRENT_PWM_XY_PIN
- void st_current_set(uint8_t driver, int current)
- {
- if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
- if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
- if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
- }
- #else //MOTOR_CURRENT_PWM_XY_PIN
- void st_current_set(uint8_t, int ){}
- #endif //MOTOR_CURRENT_PWM_XY_PIN
- void microstep_init()
- {
- #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
- pinMode(E1_MS1_PIN,OUTPUT);
- pinMode(E1_MS2_PIN,OUTPUT);
- #endif
- #if defined(X_MS1_PIN) && X_MS1_PIN > -1
- const uint8_t microstep_modes[] = MICROSTEP_MODES;
- pinMode(X_MS1_PIN,OUTPUT);
- pinMode(X_MS2_PIN,OUTPUT);
- pinMode(Y_MS1_PIN,OUTPUT);
- pinMode(Y_MS2_PIN,OUTPUT);
- pinMode(Z_MS1_PIN,OUTPUT);
- pinMode(Z_MS2_PIN,OUTPUT);
- pinMode(E0_MS1_PIN,OUTPUT);
- pinMode(E0_MS2_PIN,OUTPUT);
- for(int i=0;i<=4;i++) microstep_mode(i,microstep_modes[i]);
- #endif
- }
- #ifndef TMC2130
- void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
- {
- if(ms1 > -1) switch(driver)
- {
- case 0: digitalWrite( X_MS1_PIN,ms1); break;
- case 1: digitalWrite( Y_MS1_PIN,ms1); break;
- case 2: digitalWrite( Z_MS1_PIN,ms1); break;
- case 3: digitalWrite(E0_MS1_PIN,ms1); break;
- #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
- case 4: digitalWrite(E1_MS1_PIN,ms1); break;
- #endif
- }
- if(ms2 > -1) switch(driver)
- {
- case 0: digitalWrite( X_MS2_PIN,ms2); break;
- case 1: digitalWrite( Y_MS2_PIN,ms2); break;
- case 2: digitalWrite( Z_MS2_PIN,ms2); break;
- case 3: digitalWrite(E0_MS2_PIN,ms2); break;
- #if defined(E1_MS2_PIN) && E1_MS2_PIN > -1
- case 4: digitalWrite(E1_MS2_PIN,ms2); break;
- #endif
- }
- }
- void microstep_mode(uint8_t driver, uint8_t stepping_mode)
- {
- switch(stepping_mode)
- {
- case 1: microstep_ms(driver,MICROSTEP1); break;
- case 2: microstep_ms(driver,MICROSTEP2); break;
- case 4: microstep_ms(driver,MICROSTEP4); break;
- case 8: microstep_ms(driver,MICROSTEP8); break;
- case 16: microstep_ms(driver,MICROSTEP16); break;
- }
- }
- void microstep_readings()
- {
- SERIAL_PROTOCOLPGM("MS1,MS2 Pins\n");
- SERIAL_PROTOCOLPGM("X: ");
- SERIAL_PROTOCOL( digitalRead(X_MS1_PIN));
- SERIAL_PROTOCOLLN( digitalRead(X_MS2_PIN));
- SERIAL_PROTOCOLPGM("Y: ");
- SERIAL_PROTOCOL( digitalRead(Y_MS1_PIN));
- SERIAL_PROTOCOLLN( digitalRead(Y_MS2_PIN));
- SERIAL_PROTOCOLPGM("Z: ");
- SERIAL_PROTOCOL( digitalRead(Z_MS1_PIN));
- SERIAL_PROTOCOLLN( digitalRead(Z_MS2_PIN));
- SERIAL_PROTOCOLPGM("E0: ");
- SERIAL_PROTOCOL( digitalRead(E0_MS1_PIN));
- SERIAL_PROTOCOLLN( digitalRead(E0_MS2_PIN));
- #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
- SERIAL_PROTOCOLPGM("E1: ");
- SERIAL_PROTOCOL( digitalRead(E1_MS1_PIN));
- SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
- #endif
- }
- #endif //TMC2130
- #if defined(FILAMENT_SENSOR) && defined(PAT9125)
- void st_reset_fsensor()
- {
- CRITICAL_SECTION_START;
- fsensor_counter = 0;
- CRITICAL_SECTION_END;
- }
- #endif //FILAMENT_SENSOR
|